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 = 50 |
27 |
DISTANCE = 50 |
27 |
DISTANCE = 5000 |
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 = 1000 |
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 = 20 |
40 |
DISTANCE = 50 |
40 |
DISTANCE = 5000 |
41 |
|
41 |
|
42 |
class axis: |
42 |
class axis: |
43 |
def __init__(self, SPI_handler, Direction, StepsPerUnit, MaxSpeed): |
43 |
def __init__(self, SPI_handler, Direction, StepsPerUnit, MaxSpeed): |
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.maxseed = MaxSpeed |
48 |
self.maxspeed = MaxSpeed |
- |
|
49 |
|
- |
|
50 |
self.L6470_ABS_POS =0x01 |
- |
|
51 |
self.L6470_EL_POS =0x02 |
- |
|
52 |
self.L6470_MARK =0x03 |
- |
|
53 |
self.L6470_SPEED =0x04 |
- |
|
54 |
self.L6470_ACC =0x05 |
- |
|
55 |
self.L6470_DEC =0x06 |
- |
|
56 |
self.L6470_MAX_SPEED =0x07 |
- |
|
57 |
self.L6470_MIN_SPEED =0x08 |
- |
|
58 |
self.L6470_FS_SPD =0x15 |
- |
|
59 |
self.L6470_KVAL_HOLD =0x09 |
- |
|
60 |
self.L6470_KVAL_RUN =0x0A |
- |
|
61 |
self.L6470_KVAL_ACC =0x0B |
- |
|
62 |
self.L6470_KVAL_DEC =0x0C |
- |
|
63 |
self.L6470_INT_SPEED =0x0D |
- |
|
64 |
self.L6470_ST_SLP =0x0E |
- |
|
65 |
self.L6470_FN_SLP_ACC =0x0F |
- |
|
66 |
self.L6470_FN_SLP_DEC =0x10 |
- |
|
67 |
self.L6470_K_THERM =0x11 |
- |
|
68 |
self.L6470_ADC_OUT =0x12 |
- |
|
69 |
self.L6470_OCD_TH =0x13 |
- |
|
70 |
self.L6470_STALL_TH =0x14 |
- |
|
71 |
self.L6470_STEP_MODE =0x16 |
- |
|
72 |
self.L6470_ALARM_EN =0x17 |
- |
|
73 |
self.L6470_CONFIG =0x18 |
- |
|
74 |
self.L6470_STATUS =0x19 |
- |
|
75 |
|
49 |
self.Reset() |
76 |
self.Reset() |
50 |
self.Initialize() |
77 |
self.Initialize() |
51 |
|
78 |
|
- |
|
79 |
|
- |
|
80 |
|
- |
|
81 |
|
52 |
def Reset(self): |
82 |
def Reset(self): |
53 |
'Reset the Axis' |
83 |
'Reset the Axis' |
54 |
self.spi.xfer([0xC0]) # reset |
84 |
self.spi.xfer([0xC0]) # reset |
55 |
|
85 |
|
56 |
def Initialize(self): |
86 |
def Initialize(self): |
57 |
'set default parameters for H-bridge ' |
87 |
'set default parameters for H-bridge ' |
58 |
# self.spi.xfer( 0x14) # Stall Treshold setup |
88 |
# self.spi.xfer( 0x14) # Stall Treshold setup |
59 |
# self.spi.xfer( 0xFF) |
89 |
# self.spi.xfer( 0xFF) |
60 |
# self.spi.xfer( 0x13) # Over Current Treshold setup |
90 |
# self.spi.xfer( 0x13) # Over Current Treshold setup |
61 |
# self.spi.xfer( 0xFF) |
91 |
# self.spi.xfer( 0xFF) |
62 |
self.spi.xfer([0x15]) # Full Step speed |
92 |
self.spi.xfer([0x15]) # Full Step speed |
63 |
self.spi.xfer([0xFF]) |
93 |
self.spi.xfer([0xFF]) |
64 |
self.spi.xfer([0xFF]) |
94 |
self.spi.xfer([0xFF]) |
65 |
self.spi.xfer([0x05]) # ACC |
95 |
self.spi.xfer([0x05]) # ACC |
66 |
self.spi.xfer([0x00]) |
96 |
self.spi.xfer([0x00]) |
67 |
self.spi.xfer([0x10]) |
97 |
self.spi.xfer([0x10]) |
68 |
self.spi.xfer([0x06]) # DEC |
98 |
self.spi.xfer([0x06]) # DEC |
69 |
self.spi.xfer([0x00]) |
99 |
self.spi.xfer([0x00]) |
70 |
self.spi.xfer([0x10]) |
100 |
self.spi.xfer([0x10]) |
71 |
self.spi.xfer([0x0A]) # KVAL_RUN |
101 |
self.spi.xfer([self.L6470_KVAL_RUN]) # KVAL_RUN |
72 |
self.spi.xfer([0x50]) |
102 |
self.spi.xfer([0x50]) |
73 |
self.spi.xfer([0x0B]) # KVAL_ACC |
103 |
self.spi.xfer([self.L6470_KVAL_ACC]) # KVAL_ACC |
74 |
self.spi.xfer([0x50]) |
104 |
self.spi.xfer([0x50]) |
75 |
self.spi.xfer([0x0C]) # KVAL_DEC |
105 |
self.spi.xfer([self.L6470_KVAL_DEC]) # KVAL_DEC |
76 |
self.spi.xfer([0x50]) |
106 |
self.spi.xfer([0x50]) |
77 |
# self.spi.xfer([0x18]) # CONFIG |
107 |
# self.spi.xfer([0x18]) # CONFIG |
78 |
# self.spi.xfer([0b00111000]) |
108 |
# self.spi.xfer([0b00111000]) |
79 |
# self.spi.xfer([0b00000000]) |
109 |
# self.spi.xfer([0b00000000]) |
- |
|
110 |
self.MaxSpeed(self.maxspeed) |
80 |
|
111 |
|
81 |
def MaxSpeed(self, speed): |
112 |
def MaxSpeed(self, speed): |
82 |
'Setup of maximum speed in steps/s' |
113 |
'Setup of maximum speed in steps/s. The available range is from 15.25 to 15610 step/s with a resolution of 15.25 step/s.' |
83 |
speed_value = int(speed / 15.25) |
114 |
speed_value = int(speed / 15.25) |
84 |
if (speed_value == 0): |
115 |
if (speed_value <= 0): |
85 |
speed_value = 1 |
116 |
speed_value = 1 |
86 |
print hex(speed_value) |
117 |
elif (speed_value >= 1023): |
- |
|
118 |
speed_value = 1023 |
87 |
|
119 |
|
88 |
data = [(speed_value >> i & 0xff) for i in (16,8,0)] |
120 |
data = [(speed_value >> i & 0xff) for i in (8,0)] |
89 |
self.spi.xfer([data[0]]) # Max Speed setup |
121 |
self.spi.xfer([self.L6470_MAX_SPEED]) # Max Speed setup |
- |
|
122 |
self.spi.xfer([data[0]]) |
90 |
self.spi.xfer([data[1]]) |
123 |
self.spi.xfer([data[1]]) |
91 |
self.spi.xfer([data[2]]) |
- |
|
92 |
return (speed_value * 15.25) |
124 |
return (speed_value * 15.25) |
93 |
|
125 |
|
94 |
def ReleaseSW(self): |
126 |
def ReleaseSW(self): |
95 |
' Go away from Limit Switch ' |
127 |
' Go away from Limit Switch ' |
96 |
while self.ReadStatusBit(2) == 1: # is Limit Switch ON ? |
128 |
while self.ReadStatusBit(2) == 1: # is Limit Switch ON ? |
97 |
self.spi.xfer([0x92 | (~self.Dir & 1)]) # release SW |
129 |
self.spi.xfer([0x92 | (~self.Dir & 1)]) # release SW |
98 |
while self.GetStatus()['BUSY']: |
130 |
while self.GetStatus()['BUSY']: |
99 |
pass |
131 |
pass |
100 |
self.MoveWait(10) # move 10 units away |
132 |
self.MoveWait(10) # move 10 units away |
101 |
|
133 |
|
102 |
def GoZero(self, speed): |
134 |
def GoZero(self, speed): |
103 |
' Go to Zero position ' |
135 |
' Go to Zero position ' |
104 |
self.ReleaseSW() |
136 |
self.ReleaseSW() |
105 |
self.spi.xfer([0x82 | (self.Dir & 1)]) # Go to Zero |
137 |
self.spi.xfer([0x82 | (self.Dir & 1)]) # Go to Zero |
106 |
self.spi.xfer([0x00]) |
138 |
self.spi.xfer([0x00]) |
107 |
self.spi.xfer([speed]) |
139 |
self.spi.xfer([speed]) |
108 |
while self.GetStatus()['BUSY']: |
140 |
while self.GetStatus()['BUSY']: |
109 |
pass |
141 |
pass |
110 |
time.sleep(0.3) |
142 |
time.sleep(0.3) |
111 |
self.ReleaseSW() |
143 |
self.ReleaseSW() |
112 |
|
144 |
|
113 |
def GetStatus(self): |
145 |
def GetStatus(self): |
114 |
#self.spi.xfer([0b11010000]) # Get status command from datasheet - does not work for uknown rasons |
146 |
#self.spi.xfer([0b11010000]) # Get status command from datasheet - does not work for uknown rasons |
115 |
self.spi.xfer([0x39]) # Gotparam command on status register |
147 |
self.spi.xfer([0x39]) # Gotparam command on status register |
116 |
data = self.spi.readbytes(1) |
148 |
data = self.spi.readbytes(1) |
117 |
data = data + self.spi.readbytes(1) |
149 |
data = data + self.spi.readbytes(1) |
118 |
|
150 |
|
119 |
status = dict([('SCK_MOD',data[0] & 0x80 == 0x80), #The SCK_MOD bit is an active high flag indicating that the device is working in Step-clock mode. In this case the step-clock signal should be provided through the STCK input pin. The DIR bit indicates the current motor direction |
151 |
status = dict([('SCK_MOD',data[0] & 0x80 == 0x80), #The SCK_MOD bit is an active high flag indicating that the device is working in Step-clock mode. In this case the step-clock signal should be provided through the STCK input pin. The DIR bit indicates the current motor direction |
120 |
('STEP_LOSS_B',data[0] & 0x40 == 0x40), |
152 |
('STEP_LOSS_B',data[0] & 0x40 == 0x40), |
121 |
('STEP_LOSS_A',data[0] & 0x20 == 0x20), |
153 |
('STEP_LOSS_A',data[0] & 0x20 == 0x20), |
122 |
('OCD',data[0] & 0x10 == 0x10), |
154 |
('OCD',data[0] & 0x10 == 0x10), |
123 |
('TH_SD',data[0] & 0x08 == 0x08), |
155 |
('TH_SD',data[0] & 0x08 == 0x08), |
124 |
('TH_WRN',data[0] & 0x04 == 0x04), |
156 |
('TH_WRN',data[0] & 0x04 == 0x04), |
125 |
('UVLO',data[0] & 0x02 == 0x02), |
157 |
('UVLO',data[0] & 0x02 == 0x02), |
126 |
('WRONG_CMD',data[0] & 0x01 == 0x01), #The NOTPERF_CMD and WRONG_CMD flags are active high and indicate, respectively, that the command received by SPI cannot be performed or does not exist at all. |
158 |
('WRONG_CMD',data[0] & 0x01 == 0x01), #The NOTPERF_CMD and WRONG_CMD flags are active high and indicate, respectively, that the command received by SPI cannot be performed or does not exist at all. |
127 |
('NOTPERF_CMD',data[1] & 0x80 == 0x80), |
159 |
('NOTPERF_CMD',data[1] & 0x80 == 0x80), |
128 |
('MOT_STATUS',data[1] & 0x60), |
160 |
('MOT_STATUS',data[1] & 0x60), |
129 |
('DIR',data[1] & 0x10 == 0x10), |
161 |
('DIR',data[1] & 0x10 == 0x10), |
130 |
('SW_EVN',data[1] & 0x08 == 0x08), |
162 |
('SW_EVN',data[1] & 0x08 == 0x08), |
131 |
('SW_F',data[1] & 0x04 == 0x04), #The SW_F flag reports the SW input status (low for open and high for closed). |
163 |
('SW_F',data[1] & 0x04 == 0x04), #The SW_F flag reports the SW input status (low for open and high for closed). |
132 |
('BUSY',data[1] & 0x02 != 0x02), |
164 |
('BUSY',data[1] & 0x02 != 0x02), |
133 |
('HIZ',data[1] & 0x01 == 0x01)]) |
165 |
('HIZ',data[1] & 0x01 == 0x01)]) |
134 |
return status |
166 |
return status |
135 |
|
167 |
|
136 |
def Move(self, units): |
168 |
def Move(self, units): |
137 |
' Move some distance units from current position ' |
169 |
' Move some distance units from current position ' |
138 |
steps = units * self.SPU # translate units to steps |
170 |
steps = units * self.SPU # translate units to steps |
139 |
if steps > 0: # look for direction |
171 |
if steps > 0: # look for direction |
140 |
self.spi.xfer([0x40 | (~self.Dir & 1)]) |
172 |
self.spi.xfer([0x40 | (~self.Dir & 1)]) |
141 |
else: |
173 |
else: |
142 |
self.spi.xfer([0x40 | (self.Dir & 1)]) |
174 |
self.spi.xfer([0x40 | (self.Dir & 1)]) |
143 |
steps = int(abs(steps)) |
175 |
steps = int(abs(steps)) |
144 |
self.spi.xfer([(steps >> 16) & 0xFF]) |
176 |
self.spi.xfer([(steps >> 16) & 0xFF]) |
145 |
self.spi.xfer([(steps >> 8) & 0xFF]) |
177 |
self.spi.xfer([(steps >> 8) & 0xFF]) |
146 |
self.spi.xfer([steps & 0xFF]) |
178 |
self.spi.xfer([steps & 0xFF]) |
147 |
|
179 |
|
148 |
def Run(self, direction, speed): |
180 |
def Run(self, direction, speed): |
149 |
speed_value = int(speed / 0.015) |
181 |
speed_value = int(speed / 0.015) |
150 |
print hex(speed_value) |
182 |
print hex(speed_value) |
151 |
|
183 |
|
152 |
data = [0b01010000 + direction] |
184 |
data = [0b01010000 + direction] |
153 |
data = data +[(speed_value >> i & 0xff) for i in (16,8,0)] |
185 |
data = data +[(speed_value >> i & 0xff) for i in (16,8,0)] |
154 |
self.spi.xfer([data[0]]) # Max Speed setup |
186 |
self.spi.xfer([data[0]]) # Max Speed setup |
155 |
self.spi.xfer([data[1]]) |
187 |
self.spi.xfer([data[1]]) |
156 |
self.spi.xfer([data[2]]) |
188 |
self.spi.xfer([data[2]]) |
157 |
self.spi.xfer([data[3]]) |
189 |
self.spi.xfer([data[3]]) |
158 |
return (speed_value * 0.015) |
190 |
return (speed_value * 0.015) |
159 |
|
191 |
|
160 |
def MoveWait(self, units): |
192 |
def MoveWait(self, units): |
161 |
' Move some distance units from current position and wait for execution ' |
193 |
' Move some distance units from current position and wait for execution ' |
162 |
self.Move(units) |
194 |
self.Move(units) |
163 |
while self.GetStatus()['BUSY']: |
195 |
while self.GetStatus()['BUSY']: |
164 |
pass |
196 |
pass |
165 |
time.sleep(0.8) |
197 |
time.sleep(0.8) |
166 |
|
198 |
|
167 |
def Float(self, hard = False): |
199 |
def Float(self, hard = False): |
168 |
' switch H-bridge to High impedance state ' |
200 |
' switch H-bridge to High impedance state ' |
169 |
if (hard == False): |
201 |
if (hard == False): |
170 |
self.spi.xfer([0xA0]) |
202 |
self.spi.xfer([0xA0]) |
171 |
else: |
203 |
else: |
172 |
self.spi.xfer([0xA8]) |
204 |
self.spi.xfer([0xA8]) |
173 |
|
205 |
|
174 |
|
206 |
|
175 |
# End Class axis -------------------------------------------------- |
207 |
# End Class axis -------------------------------------------------- |
176 |
|
208 |
|
177 |
print "Stepper motor control test started. \r\n" |
209 |
print "Stepper motor control test started. \r\n" |
178 |
print "Max motor speed: %f " % SPEED |
210 |
print "Max motor speed: %f " % SPEED |
179 |
print "Distance to run: %f " % DISTANCE |
211 |
print "Distance to run: %f " % DISTANCE |
180 |
|
212 |
|
181 |
try: |
213 |
try: |
182 |
print "SPI configuration.." |
214 |
print "SPI configuration.." |
183 |
spi = spidev.SpiDev() # create a spi object |
215 |
spi = spidev.SpiDev() # create a spi object |
184 |
spi.open(0, 0) # open spi port 0, device (CS) 0 |
216 |
spi.open(0, 0) # open spi port 0, device (CS) 0 |
185 |
spi.mode = 0b01 |
217 |
spi.mode = 0b01 |
186 |
spi.lsbfirst = False |
218 |
spi.lsbfirst = False |
187 |
spi.bits_per_word = 8 |
219 |
spi.bits_per_word = 8 |
188 |
spi.cshigh = False |
220 |
spi.cshigh = False |
189 |
spi.max_speed_hz = 100000 |
221 |
spi.max_speed_hz = 100000 |
190 |
#spi.SPI_config(spi.I2CSPI_MSB_FIRST| spi.I2CSPI_MODE_CLK_IDLE_HIGH_DATA_EDGE_TRAILING| spi.I2CSPI_CLK_461kHz) |
222 |
#spi.SPI_config(spi.I2CSPI_MSB_FIRST| spi.I2CSPI_MODE_CLK_IDLE_HIGH_DATA_EDGE_TRAILING| spi.I2CSPI_CLK_461kHz) |
191 |
time.sleep(1) |
223 |
time.sleep(1) |
192 |
|
224 |
|
193 |
print "Axis inicialization" |
225 |
print "Axis inicialization" |
194 |
X = axis(spi, 0, 641, MaxSpeed = SPEED) # set Number of Steps per axis Unit and set Direction of Rotation |
226 |
X = axis(spi, 0, 1, MaxSpeed = SPEED) # set Number of Steps per axis Unit and set Direction of Rotation |
195 |
|
227 |
|
196 |
print X.MaxSpeed(SPEED) # set maximal motor speed |
228 |
print X.MaxSpeed(SPEED) # set maximal motor speed |
197 |
|
229 |
|
198 |
#print X.Run(1, 200.456431) |
230 |
#print X.Run(1, 200.456431) |
199 |
#time.sleep(10) |
231 |
#time.sleep(10) |
200 |
|
232 |
|
201 |
print "Axis is running" |
233 |
print "Axis is running" |
202 |
for i in range(5): |
234 |
for i in range(5): |
203 |
print i |
235 |
print i |
204 |
X.MoveWait(DISTANCE) # move forward and wait for motor stop |
236 |
X.MoveWait(DISTANCE) # move forward and wait for motor stop |
205 |
print "Changing direction of rotation.." |
237 |
print "Changing direction of rotation.." |
206 |
time.sleep(1.1) |
238 |
time.sleep(1.1) |
207 |
X.MoveWait(-DISTANCE) # move backward and wait for motor stop |
239 |
X.MoveWait(-DISTANCE) # move backward and wait for motor stop |
208 |
print "Changing direction of rotation.." |
240 |
print "Changing direction of rotation.." |
209 |
time.sleep(1.1) |
241 |
time.sleep(1.1) |
210 |
|
242 |
|
211 |
X.Float(hard=False) # release power |
243 |
X.Float(hard=False) # release power |
212 |
|
244 |
|
213 |
finally: |
245 |
finally: |
214 |
print "stop" |
246 |
print "stop" |