1 |
#!/usr/bin/python |
1 |
#!/usr/bin/python |
2 |
# ------------------------------------------- |
2 |
# ------------------------------------------- |
3 |
# HBSTEP01B Stepper Motor control test code |
3 |
# HBSTEP01B Stepper Motor control test code |
4 |
# ------------------------------------------- |
4 |
# ------------------------------------------- |
5 |
# |
5 |
# |
6 |
# Program uses MLAB Python modules library from https://github.com/MLAB-project/pymlab |
6 |
# Program uses MLAB Python modules library from https://github.com/MLAB-project/pymlab |
7 |
|
7 |
|
8 |
|
8 |
|
9 |
#uncomment for debbug purposes |
9 |
#uncomment for debbug purposes |
10 |
#import logging |
10 |
#import logging |
11 |
#logging.basicConfig(level=logging.DEBUG) |
11 |
#logging.basicConfig(level=logging.DEBUG) |
12 |
|
12 |
|
13 |
import sys |
13 |
import sys |
14 |
import time |
14 |
import time |
15 |
import spidev |
15 |
import spidev |
16 |
|
16 |
|
17 |
#### Script Arguments ############################################### |
17 |
#### Script Arguments ############################################### |
18 |
|
18 |
|
19 |
if len(sys.argv) < 2: |
19 |
if len(sys.argv) < 2: |
20 |
sys.stderr.write("Invalid number of arguments.\n") |
20 |
sys.stderr.write("Invalid number of arguments.\n") |
21 |
sys.stderr.write("Usage: %s PORT ADDRESS SPEED MOVE_DISTANCE\n" % (sys.argv[0], )) |
21 |
sys.stderr.write("Usage: %s PORT ADDRESS SPEED MOVE_DISTANCE\n" % (sys.argv[0], )) |
22 |
sys.exit(1) |
22 |
sys.exit(1) |
23 |
|
23 |
|
24 |
elif len(sys.argv) == 2: |
24 |
elif len(sys.argv) == 2: |
25 |
PORT = eval(sys.argv[1]) |
25 |
PORT = eval(sys.argv[1]) |
26 |
SPEED = 5 |
26 |
SPEED = 5 |
27 |
DISTANCE = 50 |
27 |
DISTANCE = 50 |
28 |
|
28 |
|
29 |
elif len(sys.argv) == 3: |
29 |
elif len(sys.argv) == 3: |
30 |
SPEED = eval(sys.argv[2]) |
30 |
SPEED = eval(sys.argv[2]) |
31 |
DISTANCE = 100 |
31 |
DISTANCE = 100 |
32 |
|
32 |
|
33 |
elif len(sys.argv) == 4: |
33 |
elif len(sys.argv) == 4: |
34 |
SPEED = eval(sys.argv[2]) |
34 |
SPEED = eval(sys.argv[2]) |
35 |
DISTANCE = eval(sys.argv[3]) |
35 |
DISTANCE = eval(sys.argv[3]) |
36 |
|
36 |
|
37 |
else: |
37 |
else: |
38 |
PORT = 0 |
38 |
PORT = 0 |
39 |
SPEED = 10 |
39 |
SPEED = 10 |
40 |
DISTANCE = 50 |
40 |
DISTANCE = 50 |
41 |
|
41 |
|
42 |
class axis: |
42 |
class axis: |
43 |
def __init__(self, SPI_handler, Direction, StepsPerUnit): |
43 |
def __init__(self, SPI_handler, Direction, StepsPerUnit): |
44 |
' One axis of robot ' |
44 |
' One axis of robot ' |
45 |
self.spi = SPI_handler |
45 |
self.spi = SPI_handler |
46 |
self.Dir = Direction |
46 |
self.Dir = Direction |
47 |
self.SPU = StepsPerUnit |
47 |
self.SPU = StepsPerUnit |
48 |
self.Reset() |
48 |
self.Reset() |
49 |
|
49 |
|
50 |
def Reset(self): |
50 |
def Reset(self): |
51 |
' Reset Axis and set default parameters for H-bridge ' |
51 |
' Reset Axis and set default parameters for H-bridge ' |
52 |
self.spi.xfer( 0xC0) # reset |
52 |
self.spi.xfer([0xC0]) # reset |
53 |
# self.spi.xfer( 0x14) # Stall Treshold setup |
53 |
# self.spi.xfer( 0x14) # Stall Treshold setup |
54 |
# self.spi.xfer( 0xFF) |
54 |
# self.spi.xfer( 0xFF) |
55 |
# self.spi.xfer( 0x13) # Over Current Treshold setup |
55 |
# self.spi.xfer( 0x13) # Over Current Treshold setup |
56 |
# self.spi.xfer( 0xFF) |
56 |
# self.spi.xfer( 0xFF) |
57 |
self.spi.xfer( 0x15) # Full Step speed |
57 |
self.spi.xfer([0x15]) # Full Step speed |
58 |
self.spi.xfer( 0xFF) |
58 |
self.spi.xfer([0xFF]) |
59 |
self.spi.xfer( 0xFF) |
59 |
self.spi.xfer([0xFF]) |
60 |
self.spi.xfer( 0x05) # ACC |
60 |
self.spi.xfer([0x05]) # ACC |
61 |
self.spi.xfer( 0x00) |
61 |
self.spi.xfer([0x00]) |
62 |
self.spi.xfer( 0x10) |
62 |
self.spi.xfer([0x10]) |
63 |
self.spi.xfer( 0x06) # DEC |
63 |
self.spi.xfer([0x06]) # DEC |
64 |
self.spi.xfer( 0x00) |
64 |
self.spi.xfer([0x00]) |
65 |
self.spi.xfer( 0x10) |
65 |
self.spi.xfer([0x10]) |
66 |
self.spi.xfer( 0x0A) # KVAL_RUN |
66 |
self.spi.xfer([0x0A]) # KVAL_RUN |
67 |
self.spi.xfer( 0xFF) |
67 |
self.spi.xfer([0xFF]) |
68 |
self.spi.xfer( 0x0B) # KVAL_ACC |
68 |
self.spi.xfer([0x0B]) # KVAL_ACC |
69 |
self.spi.xfer( 0xFF) |
69 |
self.spi.xfer([0xFF]) |
70 |
self.spi.xfer( 0x0C) # KVAL_DEC |
70 |
self.spi.xfer([0x0C]) # KVAL_DEC |
71 |
self.spi.xfer( 0xFF) |
71 |
self.spi.xfer([0xFF]) |
72 |
self.spi.xfer( 0x18) # CONFIG |
72 |
self.spi.xfer([0x18]) # CONFIG |
73 |
self.spi.xfer( 0b00111000) |
73 |
self.spi.xfer([0b00111000]) |
74 |
self.spi.xfer( 0b00000000) |
74 |
self.spi.xfer([0b00000000]) |
75 |
|
75 |
|
76 |
def MaxSpeed(self, speed): |
76 |
def MaxSpeed(self, speed): |
77 |
' Setup of maximum speed ' |
77 |
' Setup of maximum speed ' |
78 |
self.spi.xfer( 0x07) # Max Speed setup |
78 |
self.spi.xfer([0x07]) # Max Speed setup |
79 |
self.spi.xfer( 0x00) |
79 |
self.spi.xfer([0x00]) |
80 |
self.spi.xfer( speed) |
80 |
self.spi.xfer([speed]) |
81 |
|
81 |
|
82 |
def ReleaseSW(self): |
82 |
def ReleaseSW(self): |
83 |
' Go away from Limit Switch ' |
83 |
' Go away from Limit Switch ' |
84 |
while self.ReadStatusBit(2) == 1: # is Limit Switch ON ? |
84 |
while self.ReadStatusBit(2) == 1: # is Limit Switch ON ? |
85 |
self.spi.xfer( 0x92 | (~self.Dir & 1)) # release SW |
85 |
self.spi.xfer([0x92 | (~self.Dir & 1)]) # release SW |
86 |
while self.IsBusy(): |
86 |
while self.IsBusy(): |
87 |
pass |
87 |
pass |
88 |
self.MoveWait(10) # move 10 units away |
88 |
self.MoveWait(10) # move 10 units away |
89 |
|
89 |
|
90 |
def GoZero(self, speed): |
90 |
def GoZero(self, speed): |
91 |
' Go to Zero position ' |
91 |
' Go to Zero position ' |
92 |
self.ReleaseSW() |
92 |
self.ReleaseSW() |
93 |
|
93 |
|
94 |
self.spi.xfer( 0x82 | (self.Dir & 1)) # Go to Zero |
94 |
self.spi.xfer([0x82 | (self.Dir & 1)]) # Go to Zero |
95 |
self.spi.xfer( 0x00) |
95 |
self.spi.xfer([0x00]) |
96 |
self.spi.xfer( speed) |
96 |
self.spi.xfer([speed]) |
97 |
while self.IsBusy(): |
97 |
while self.IsBusy(): |
98 |
pass |
98 |
pass |
99 |
time.sleep(0.3) |
99 |
time.sleep(0.3) |
100 |
self.ReleaseSW() |
100 |
self.ReleaseSW() |
101 |
|
101 |
|
102 |
def Move(self, units): |
102 |
def Move(self, units): |
103 |
' Move some distance units from current position ' |
103 |
' Move some distance units from current position ' |
104 |
steps = units * self.SPU # translate units to steps |
104 |
steps = units * self.SPU # translate units to steps |
105 |
if steps > 0: # look for direction |
105 |
if steps > 0: # look for direction |
106 |
self.spi.xfer( 0x40 | (~self.Dir & 1)) |
106 |
self.spi.xfer([0x40 | (~self.Dir & 1)]) |
107 |
else: |
107 |
else: |
108 |
self.spi.xfer( 0x40 | (self.Dir & 1)) |
108 |
self.spi.xfer([0x40 | (self.Dir & 1)]) |
109 |
steps = int(abs(steps)) |
109 |
steps = int(abs(steps)) |
110 |
self.spi.xfer( (steps >> 16) & 0xFF) |
110 |
self.spi.xfer([(steps >> 16) & 0xFF]) |
111 |
self.spi.xfer( (steps >> 8) & 0xFF) |
111 |
self.spi.xfer([(steps >> 8) & 0xFF]) |
112 |
self.spi.xfer( steps & 0xFF) |
112 |
self.spi.xfer([steps & 0xFF]) |
113 |
|
113 |
|
114 |
def MoveWait(self, units): |
114 |
def MoveWait(self, units): |
115 |
' Move some distance units from current position and wait for execution ' |
115 |
' Move some distance units from current position and wait for execution ' |
116 |
self.Move(units) |
116 |
self.Move(units) |
117 |
while self.IsBusy(): |
117 |
while self.IsBusy(): |
118 |
pass |
118 |
pass |
119 |
|
119 |
|
120 |
def Float(self): |
120 |
def Float(self): |
121 |
' switch H-bridge to High impedance state ' |
121 |
' switch H-bridge to High impedance state ' |
122 |
self.spi.xfer( 0xA0) |
122 |
self.spi.xfer( 0xA0) |
123 |
|
123 |
|
124 |
def ReadStatusBit(self, bit): |
124 |
def ReadStatusBit(self, bit): |
125 |
' Report given status bit ' |
125 |
' Report given status bit ' |
126 |
self.spi.xfer( 0x39) # Read from address 0x19 (STATUS) |
126 |
self.spi.xfer( 0x39) # Read from address 0x19 (STATUS) |
127 |
self.spi.xfer( 0x00) |
127 |
self.spi.xfer( 0x00) |
128 |
data0 = spi.SPI_read_byte() # 1st byte |
128 |
data0 = spi.SPI_read_byte() # 1st byte |
129 |
self.spi.xfer( 0x00) |
129 |
self.spi.xfer( 0x00) |
130 |
data1 = spi.SPI_read_byte() # 2nd byte |
130 |
data1 = spi.SPI_read_byte() # 2nd byte |
131 |
#print hex(data0), hex(data1) |
131 |
#print hex(data0), hex(data1) |
132 |
if bit > 7: # extract requested bit |
132 |
if bit > 7: # extract requested bit |
133 |
OutputBit = (data0 >> (bit - 8)) & 1 |
133 |
OutputBit = (data0 >> (bit - 8)) & 1 |
134 |
else: |
134 |
else: |
135 |
OutputBit = (data1 >> bit) & 1 |
135 |
OutputBit = (data1 >> bit) & 1 |
136 |
return OutputBit |
136 |
return OutputBit |
137 |
|
137 |
|
138 |
|
138 |
|
139 |
def IsBusy(self): |
139 |
def IsBusy(self): |
140 |
""" Return True if tehre are motion """ |
140 |
""" Return True if tehre are motion """ |
141 |
if self.ReadStatusBit(1) == 1: |
141 |
if self.ReadStatusBit(1) == 1: |
142 |
return False |
142 |
return False |
143 |
else: |
143 |
else: |
144 |
return True |
144 |
return True |
145 |
|
145 |
|
146 |
# End Class axis -------------------------------------------------- |
146 |
# End Class axis -------------------------------------------------- |
147 |
|
147 |
|
148 |
print "Stepper motor control test started. \r\n" |
148 |
print "Stepper motor control test started. \r\n" |
149 |
print "Max motor speed: %d " % SPEED |
149 |
print "Max motor speed: %d " % SPEED |
150 |
print "Distance to run: %d " % DISTANCE |
150 |
print "Distance to run: %d " % DISTANCE |
151 |
|
151 |
|
152 |
try: |
152 |
try: |
153 |
print "SPI configuration.." |
153 |
print "SPI configuration.." |
154 |
spi = spidev.SpiDev() # create a spi object |
154 |
spi = spidev.SpiDev() # create a spi object |
155 |
spi.open(0, 0) # open spi port 0, device (CS) 0 |
155 |
spi.open(0, 0) # open spi port 0, device (CS) 0 |
156 |
spi.mode = 1 |
156 |
spi.mode = 1 |
157 |
spi.lsbfirst = False |
157 |
spi.lsbfirst = False |
158 |
spi.bits_per_word = 8 |
158 |
spi.bits_per_word = 8 |
159 |
spi.cshigh = False |
159 |
spi.cshigh = False |
160 |
#spi.SPI_config(spi.I2CSPI_MSB_FIRST| spi.I2CSPI_MODE_CLK_IDLE_HIGH_DATA_EDGE_TRAILING| spi.I2CSPI_CLK_461kHz) |
160 |
#spi.SPI_config(spi.I2CSPI_MSB_FIRST| spi.I2CSPI_MODE_CLK_IDLE_HIGH_DATA_EDGE_TRAILING| spi.I2CSPI_CLK_461kHz) |
161 |
time.sleep(1) |
161 |
time.sleep(1) |
162 |
|
162 |
|
163 |
print "Axis inicialization" |
163 |
print "Axis inicialization" |
164 |
X = axis(spi, 0, 641) # set Number of Steps per axis Unit and set Direction of Rotation |
164 |
X = axis(spi, 0, 641) # set Number of Steps per axis Unit and set Direction of Rotation |
165 |
X.MaxSpeed(SPEED) # set maximal motor speed |
165 |
X.MaxSpeed(SPEED) # set maximal motor speed |
166 |
|
166 |
|
167 |
print "Axis is running" |
167 |
print "Axis is running" |
168 |
|
168 |
|
169 |
for i in range(5): |
169 |
for i in range(5): |
170 |
print i |
170 |
print i |
171 |
X.MoveWait(DISTANCE) # move forward and wait for motor stop |
171 |
X.MoveWait(DISTANCE) # move forward and wait for motor stop |
172 |
print "Changing direction of rotation.." |
172 |
print "Changing direction of rotation.." |
173 |
X.MoveWait(-DISTANCE) # move backward and wait for motor stop |
173 |
X.MoveWait(-DISTANCE) # move backward and wait for motor stop |
174 |
print "Changing direction of rotation.." |
174 |
print "Changing direction of rotation.." |
175 |
|
175 |
|
176 |
X.Float() # release power |
176 |
X.Float() # release power |
177 |
|
177 |
|
178 |
|
178 |
|
179 |
finally: |
179 |
finally: |
180 |
print "stop" |
180 |
print "stop" |