Rev 4723 Rev 4724
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 from pymlab import config 15 from pymlab import config
16   16  
17   17  
18   18  
19 #### Script Arguments ############################################### 19 #### Script Arguments ###############################################
20   20  
21 if len(sys.argv) < 2: 21 if len(sys.argv) < 2:
22 sys.stderr.write("Invalid number of arguments.\n") 22 sys.stderr.write("Invalid number of arguments.\n")
23 sys.stderr.write("Usage: %s PORT ADDRESS SPEED MOVE_DISTANCE\n" % (sys.argv[0], )) 23 sys.stderr.write("Usage: %s PORT ADDRESS SPEED MOVE_DISTANCE\n" % (sys.argv[0], ))
24 sys.exit(1) 24 sys.exit(1)
25   25  
26 elif len(sys.argv) == 2: 26 elif len(sys.argv) == 2:
27 PORT = eval(sys.argv[1]) 27 PORT = eval(sys.argv[1])
28 SPEED = 50 28 SPEED = 50
29 DISTANCE = 5000 29 DISTANCE = 5000
30   30  
31 elif len(sys.argv) == 3: 31 elif len(sys.argv) == 3:
32 SPEED = eval(sys.argv[2]) 32 SPEED = eval(sys.argv[2])
33 DISTANCE = 1000 33 DISTANCE = 1000
34   34  
35 elif len(sys.argv) == 4: 35 elif len(sys.argv) == 4:
36 SPEED = eval(sys.argv[2]) 36 SPEED = eval(sys.argv[2])
37 DISTANCE = eval(sys.argv[3]) 37 DISTANCE = eval(sys.argv[3])
38   38  
39 else: 39 else:
40 PORT = 0 40 PORT = 0
41 SPEED = 20 41 SPEED = 20
42 DISTANCE = 5000 42 DISTANCE = 5000
43   43  
44 # Begin of Class Axis -------------------------------------------------- 44 # Begin of Class Axis --------------------------------------------------
45   45  
46 class axis: 46 class axis:
47 def __init__(self, SPI_CS, Direction, StepsPerUnit, MaxSpeed): 47 def __init__(self, SPI_CS, Direction, StepsPerUnit, MaxSpeed):
48 ' One axis of robot ' 48 ' One axis of robot '
49 self.CS = SPI_CS 49 self.CS = SPI_CS
50 self.Dir = Direction 50 self.Dir = Direction
51 self.SPU = StepsPerUnit 51 self.SPU = StepsPerUnit
52 self.maxspeed = MaxSpeed 52 self.maxspeed = MaxSpeed
53   53  
54 self.L6470_ABS_POS =0x01 54 self.L6470_ABS_POS =0x01
55 self.L6470_EL_POS =0x02 55 self.L6470_EL_POS =0x02
56 self.L6470_MARK =0x03 56 self.L6470_MARK =0x03
57 self.L6470_SPEED =0x04 57 self.L6470_SPEED =0x04
58 self.L6470_ACC =0x05 58 self.L6470_ACC =0x05
59 self.L6470_DEC =0x06 59 self.L6470_DEC =0x06
60 self.L6470_MAX_SPEED =0x07 60 self.L6470_MAX_SPEED =0x07
61 self.L6470_MIN_SPEED =0x08 61 self.L6470_MIN_SPEED =0x08
62 self.L6470_FS_SPD =0x15 62 self.L6470_FS_SPD =0x15
63 self.L6470_KVAL_HOLD =0x09 63 self.L6470_KVAL_HOLD =0x09
64 self.L6470_KVAL_RUN =0x0A 64 self.L6470_KVAL_RUN =0x0A
65 self.L6470_KVAL_ACC =0x0B 65 self.L6470_KVAL_ACC =0x0B
66 self.L6470_KVAL_DEC =0x0C 66 self.L6470_KVAL_DEC =0x0C
67 self.L6470_INT_SPEED =0x0D 67 self.L6470_INT_SPEED =0x0D
68 self.L6470_ST_SLP =0x0E 68 self.L6470_ST_SLP =0x0E
69 self.L6470_FN_SLP_ACC =0x0F 69 self.L6470_FN_SLP_ACC =0x0F
70 self.L6470_FN_SLP_DEC =0x10 70 self.L6470_FN_SLP_DEC =0x10
71 self.L6470_K_THERM =0x11 71 self.L6470_K_THERM =0x11
72 self.L6470_ADC_OUT =0x12 72 self.L6470_ADC_OUT =0x12
73 self.L6470_OCD_TH =0x13 73 self.L6470_OCD_TH =0x13
74 self.L6470_STALL_TH =0x14 74 self.L6470_STALL_TH =0x14
75 self.L6470_STEP_MODE =0x16 75 self.L6470_STEP_MODE =0x16
76 self.L6470_ALARM_EN =0x17 76 self.L6470_ALARM_EN =0x17
77 self.L6470_CONFIG =0x18 77 self.L6470_CONFIG =0x18
78 self.L6470_STATUS =0x19 78 self.L6470_STATUS =0x19
79   79  
80 self.Reset() 80 self.Reset()
81 self.Initialize() 81 self.Initialize()
82 self.MaxSpeed(self.maxspeed) 82 self.MaxSpeed(self.maxspeed)
83   83  
84 def Reset(self): 84 def Reset(self):
85 'Reset the Axis' 85 'Reset the Axis'
86 spi.SPI_write_byte(self.CS, 0xC0) # reset 86 spi.SPI_write_byte(self.CS, 0xC0) # reset
87   87  
88 def Initialize(self): 88 def Initialize(self):
89 'set default parameters for H-bridge ' 89 'set default parameters for H-bridge '
90 # spi.SPI_write_byte(self.CS, 0x14) # Stall Treshold setup 90 # spi.SPI_write_byte(self.CS, 0x14) # Stall Treshold setup
91 # spi.SPI_write_byte(self.CS, 0xFF) 91 # spi.SPI_write_byte(self.CS, 0xFF)
92 # spi.SPI_write_byte(self.CS, 0x13) # Over Current Treshold setup 92 # spi.SPI_write_byte(self.CS, 0x13) # Over Current Treshold setup
93 # spi.SPI_write_byte(self.CS, 0xFF) 93 # spi.SPI_write_byte(self.CS, 0xFF)
94 spi.SPI_write_byte(self.CS, 0x15) # Full Step speed 94 spi.SPI_write_byte(self.CS, 0x15) # Full Step speed
95 spi.SPI_write_byte(self.CS, 0xFF) 95 spi.SPI_write_byte(self.CS, 0xFF)
96 spi.SPI_write_byte(self.CS, 0xFF) 96 spi.SPI_write_byte(self.CS, 0xFF)
97 spi.SPI_write_byte(self.CS, 0x05) # ACC 97 spi.SPI_write_byte(self.CS, 0x05) # ACC
98 spi.SPI_write_byte(self.CS, 0x00) 98 spi.SPI_write_byte(self.CS, 0x00)
99 spi.SPI_write_byte(self.CS, 0x10) 99 spi.SPI_write_byte(self.CS, 0x10)
100 spi.SPI_write_byte(self.CS, 0x06) # DEC 100 spi.SPI_write_byte(self.CS, 0x06) # DEC
101 spi.SPI_write_byte(self.CS, 0x00) 101 spi.SPI_write_byte(self.CS, 0x00)
102 spi.SPI_write_byte(self.CS, 0x10) 102 spi.SPI_write_byte(self.CS, 0x10)
103 spi.SPI_write_byte(self.CS, self.L6470_KVAL_RUN) # KVAL_RUN 103 spi.SPI_write_byte(self.CS, self.L6470_KVAL_RUN) # KVAL_RUN
104 spi.SPI_write_byte(self.CS, 0x58) 104 spi.SPI_write_byte(self.CS, 0x58)
105 spi.SPI_write_byte(self.CS, self.L6470_KVAL_ACC) # KVAL_ACC 105 spi.SPI_write_byte(self.CS, self.L6470_KVAL_ACC) # KVAL_ACC
106 spi.SPI_write_byte(self.CS, 0x58) 106 spi.SPI_write_byte(self.CS, 0x58)
107 spi.SPI_write_byte(self.CS, self.L6470_KVAL_DEC) # KVAL_DEC 107 spi.SPI_write_byte(self.CS, self.L6470_KVAL_DEC) # KVAL_DEC
108 spi.SPI_write_byte(self.CS, 0x58) 108 spi.SPI_write_byte(self.CS, 0x58)
109 # spi.SPI_write_byte(self.CS, 0x18) # CONFIG 109 # spi.SPI_write_byte(self.CS, 0x18) # CONFIG
110 # spi.SPI_write_byte(self.CS, 0b00111000) 110 # spi.SPI_write_byte(self.CS, 0b00111000)
111 # spi.SPI_write_byte(self.CS, 0b00000000) 111 # spi.SPI_write_byte(self.CS, 0b00000000)
112 self.MaxSpeed(self.maxspeed) 112 self.MaxSpeed(self.maxspeed)
113   113  
114 def setKVAL(self, hold = 0.5, run = 0.5, acc = 0.5, dec = 0.5): 114 def setKVAL(self, hold = 0.5, run = 0.5, acc = 0.5, dec = 0.5):
115 """ The available range is from 0 to 0.996 x VS with a resolution of 0.004 x VS """ 115 """ The available range is from 0 to 0.996 x VS with a resolution of 0.004 x VS """
116   116  
117 def setOverCurrentTH(self, hold = 0.5, run = 0.5, acc = 0.5, dec = 0.5): 117 def setOverCurrentTH(self, hold = 0.5, run = 0.5, acc = 0.5, dec = 0.5):
118 """ The available range is from 375 mA to 6 A, in steps of 375 mA """ 118 """ The available range is from 375 mA to 6 A, in steps of 375 mA """
119   119  
120   120  
121 121
122 def MaxSpeed(self, speed): 122 def MaxSpeed(self, speed):
123 '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.' 123 '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.'
124 speed_value = int(speed / 15.25) 124 speed_value = int(speed / 15.25)
125 if (speed_value <= 0): 125 if (speed_value <= 0):
126 speed_value = 1 126 speed_value = 1
127 elif (speed_value >= 1023): 127 elif (speed_value >= 1023):
128 speed_value = 1023 128 speed_value = 1023
129   129  
130 data = [(speed_value >> i & 0xff) for i in (8,0)] 130 data = [(speed_value >> i & 0xff) for i in (8,0)]
131 print data 131 print data
132 spi.SPI_write_byte(self.CS, self.L6470_MAX_SPEED) # Max Speed setup 132 spi.SPI_write_byte(self.CS, self.L6470_MAX_SPEED) # Max Speed setup
133 spi.SPI_write_byte(self.CS, data[0]) 133 spi.SPI_write_byte(self.CS, data[0])
134 spi.SPI_write_byte(self.CS, data[1]) 134 spi.SPI_write_byte(self.CS, data[1])
135 return (speed_value * 15.25) 135 return (speed_value * 15.25)
136   136  
137 def ReleaseSW(self): 137 def ReleaseSW(self):
138 ' Go away from Limit Switch ' 138 ' Go away from Limit Switch '
139 while self.ReadStatusBit(2) == 1: # is Limit Switch ON ? 139 while self.ReadStatusBit(2) == 1: # is Limit Switch ON ?
140 spi.SPI_write_byte(self.CS, 0x92 | (~self.Dir & 1)) # release SW 140 spi.SPI_write_byte(self.CS, 0x92 | (~self.Dir & 1)) # release SW
141 while self.IsBusy(): 141 while self.IsBusy():
142 pass 142 pass
143 self.MoveWait(10) # move 10 units away 143 self.MoveWait(10) # move 10 units away
144 144
145 def GoZero(self, speed): 145 def GoZero(self, speed):
146 ' Go to Zero position ' 146 ' Go to Zero position '
147 self.ReleaseSW() 147 self.ReleaseSW()
148   148  
149 spi.SPI_write_byte(self.CS, 0x82 | (self.Dir & 1)) # Go to Zero 149 spi.SPI_write_byte(self.CS, 0x82 | (self.Dir & 1)) # Go to Zero
150 spi.SPI_write_byte(self.CS, 0x00) 150 spi.SPI_write_byte(self.CS, 0x00)
151 spi.SPI_write_byte(self.CS, speed) 151 spi.SPI_write_byte(self.CS, speed)
152 while self.IsBusy(): 152 while self.IsBusy():
153 pass 153 pass
154 time.sleep(0.3) 154 time.sleep(0.3)
155 self.ReleaseSW() 155 self.ReleaseSW()
156   156  
157 def GetStatus(self): 157 def GetStatus(self):
158 #self.spi.xfer([0b11010000]) # Get status command from datasheet - does not work for uknown rasons 158 #self.spi.xfer([0b11010000]) # Get status command from datasheet - does not work for uknown rasons
159 spi.SPI_write_byte(self.CS, 0x39) # Gotparam command on status register 159 spi.SPI_write_byte(self.CS, 0x39) # Gotparam command on status register
160 spi.SPI_write_byte(self.CS, 0x00) 160 spi.SPI_write_byte(self.CS, 0x00)
161 data = spi.SPI_read_byte() 161 data = [spi.SPI_read_byte()]
162 spi.SPI_write_byte(self.CS, 0x00) 162 spi.SPI_write_byte(self.CS, 0x00)
163 data = data + spi.SPI_read_byte() 163 data = data + [spi.SPI_read_byte()]
164   164  
165 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 165 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
166 ('STEP_LOSS_B',data[0] & 0x40 == 0x40), 166 ('STEP_LOSS_B',data[0] & 0x40 == 0x40),
167 ('STEP_LOSS_A',data[0] & 0x20 == 0x20), 167 ('STEP_LOSS_A',data[0] & 0x20 == 0x20),
168 ('OCD',data[0] & 0x10 == 0x10), 168 ('OCD',data[0] & 0x10 == 0x10),
169 ('TH_SD',data[0] & 0x08 == 0x08), 169 ('TH_SD',data[0] & 0x08 == 0x08),
170 ('TH_WRN',data[0] & 0x04 == 0x04), 170 ('TH_WRN',data[0] & 0x04 == 0x04),
171 ('UVLO',data[0] & 0x02 == 0x02), 171 ('UVLO',data[0] & 0x02 == 0x02),
172 ('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. 172 ('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.
173 ('NOTPERF_CMD',data[1] & 0x80 == 0x80), 173 ('NOTPERF_CMD',data[1] & 0x80 == 0x80),
174 ('MOT_STATUS',data[1] & 0x60), 174 ('MOT_STATUS',data[1] & 0x60),
175 ('DIR',data[1] & 0x10 == 0x10), 175 ('DIR',data[1] & 0x10 == 0x10),
176 ('SW_EVN',data[1] & 0x08 == 0x08), 176 ('SW_EVN',data[1] & 0x08 == 0x08),
177 ('SW_F',data[1] & 0x04 == 0x04), #The SW_F flag reports the SW input status (low for open and high for closed). 177 ('SW_F',data[1] & 0x04 == 0x04), #The SW_F flag reports the SW input status (low for open and high for closed).
178 ('BUSY',data[1] & 0x02 != 0x02), 178 ('BUSY',data[1] & 0x02 != 0x02),
179 ('HIZ',data[1] & 0x01 == 0x01)]) 179 ('HIZ',data[1] & 0x01 == 0x01)])
180 return status 180 return status
181   181  
182 def GetACC(self): 182 def GetACC(self):
183 # self.spi.xfer([0x29]) # Gotparam command on status register 183 # self.spi.xfer([0x29]) # Gotparam command on status register
184 spi.SPI_write_byte(self.CS, self.L6470_ACC + 0x20) # TODO check register read address seting 184 spi.SPI_write_byte(self.CS, self.L6470_ACC + 0x20) # TODO check register read address seting
185 spi.SPI_write_byte(self.CS, 0x00) 185 spi.SPI_write_byte(self.CS, 0x00)
186 data = spi.SPI_read_byte() 186 data = spi.SPI_read_byte()
187 spi.SPI_write_byte(self.CS, 0x00) 187 spi.SPI_write_byte(self.CS, 0x00)
188 data = data + self.spi.readbytes(1) 188 data = data + [spi.SPI_read_byte()]
189 print data # return speed in real units 189 print data # return speed in real units
190   190  
191   191  
192 def Move(self, units): 192 def Move(self, units):
193 ' Move some distance units from current position ' 193 ' Move some distance units from current position '
194 steps = units * self.SPU # translate units to steps 194 steps = units * self.SPU # translate units to steps
195 if steps > 0: # look for direction 195 if steps > 0: # look for direction
196 spi.SPI_write_byte(self.CS, 0x40 | (~self.Dir & 1)) 196 spi.SPI_write_byte(self.CS, 0x40 | (~self.Dir & 1))
197 else: 197 else:
198 spi.SPI_write_byte(self.CS, 0x40 | (self.Dir & 1)) 198 spi.SPI_write_byte(self.CS, 0x40 | (self.Dir & 1))
199 steps = int(abs(steps)) 199 steps = int(abs(steps))
200 spi.SPI_write_byte(self.CS, (steps >> 16) & 0xFF) 200 spi.SPI_write_byte(self.CS, (steps >> 16) & 0xFF)
201 spi.SPI_write_byte(self.CS, (steps >> 8) & 0xFF) 201 spi.SPI_write_byte(self.CS, (steps >> 8) & 0xFF)
202 spi.SPI_write_byte(self.CS, steps & 0xFF) 202 spi.SPI_write_byte(self.CS, steps & 0xFF)
203   203  
204 def Run(self, direction, speed): 204 def Run(self, direction, speed):
205 speed_value = int(speed / 0.015) 205 speed_value = int(speed / 0.015)
206 print hex(speed_value) 206 print hex(speed_value)
207   207  
208 command = 0b01010000 + int(direction) 208 command = 0b01010000 + int(direction)
209 data = [(speed_value >> i & 0xff) for i in (16,8,0)] 209 data = [(speed_value >> i & 0xff) for i in (16,8,0)]
210 spi.SPI_write_byte(self.CS, command) # Max Speed setup 210 spi.SPI_write_byte(self.CS, command) # Max Speed setup
211 spi.SPI_write_byte(self.CS, data[0]) 211 spi.SPI_write_byte(self.CS, data[0])
212 spi.SPI_write_byte(self.CS, data[1]) 212 spi.SPI_write_byte(self.CS, data[1])
213 spi.SPI_write_byte(self.CS, data[2]) 213 spi.SPI_write_byte(self.CS, data[2])
214 return (speed_value * 0.015) 214 return (speed_value * 0.015)
215   215  
216 def MoveWait(self, units): 216 def MoveWait(self, units):
217 ' Move some distance units from current position and wait for execution ' 217 ' Move some distance units from current position and wait for execution '
218 self.Move(units) 218 self.Move(units)
219 while self.GetStatus()['BUSY']: 219 while self.GetStatus()['BUSY']:
220 time.sleep(0.1) 220 time.sleep(0.1)
221   221  
222 def Float(self, hard = False): 222 def Float(self, hard = False):
223 ' switch H-bridge to High impedance state ' 223 ' switch H-bridge to High impedance state '
224 if (hard == False): 224 if (hard == False):
225 spi.SPI_write_byte(self.CS, 0xA0) 225 spi.SPI_write_byte(self.CS, 0xA0)
226 else: 226 else:
227 spi.SPI_write_byte(self.CS, 0xA8) 227 spi.SPI_write_byte(self.CS, 0xA8)
228   228  
229   229  
230 # End Class axis -------------------------------------------------- 230 # End Class axis --------------------------------------------------
231   231  
232   232  
233   233  
234 cfg = config.Config( 234 cfg = config.Config(
235 i2c = { 235 i2c = {
236 "port": 1, 236 "port": 1,
237 }, 237 },
238   238  
239 bus = [ 239 bus = [
240 { 240 {
241 "name":"spi", 241 "name":"spi",
242 "type":"i2cspi", 242 "type":"i2cspi",
243 "address": 0x2e, 243 "address": 0x2e,
244 }, 244 },
245 ], 245 ],
246 ) 246 )
247   247  
248   248  
249 cfg.initialize() 249 cfg.initialize()
250   250  
251 print "Stepper motor control test started. \r\n" 251 print "Stepper motor control test started. \r\n"
252 print "Max motor speed: %d " % SPEED 252 print "Max motor speed: %d " % SPEED
253 print "Distance to run: %d " % DISTANCE 253 print "Distance to run: %d " % DISTANCE
254   254  
255 spi = cfg.get_device("spi") 255 spi = cfg.get_device("spi")
256   256  
257 spi.route() 257 spi.route()
258   258  
259 try: 259 try:
260 print "SPI configuration.." 260 print "SPI configuration.."
261 spi.SPI_config(spi.I2CSPI_MSB_FIRST| spi.I2CSPI_MODE_CLK_IDLE_HIGH_DATA_EDGE_TRAILING| spi.I2CSPI_CLK_461kHz) 261 spi.SPI_config(spi.I2CSPI_MSB_FIRST| spi.I2CSPI_MODE_CLK_IDLE_HIGH_DATA_EDGE_TRAILING| spi.I2CSPI_CLK_461kHz)
262 time.sleep(1) 262 time.sleep(1)
263   263  
264 print "Axis inicialization" 264 print "Axis inicialization"
265 X = axis(spi.I2CSPI_SS0, 0, 1, MaxSpeed = SPEED) # set Number of Steps per axis Unit and set Direction of Rotation 265 X = axis(spi.I2CSPI_SS0, 0, 1, MaxSpeed = SPEED) # set Number of Steps per axis Unit and set Direction of Rotation
266 X.MaxSpeed(SPEED) # Reset maximal motor speed 266 X.MaxSpeed(SPEED) # Reset maximal motor speed
267   267  
268 print "Axis is running" 268 print "Axis is running"
269   269  
270 for i in range(5): 270 for i in range(5):
271 print i 271 print i
272 X.MoveWait(DISTANCE) # move forward and wait for motor stop 272 X.MoveWait(DISTANCE) # move forward and wait for motor stop
273 print "Changing direction of rotation.." 273 print "Changing direction of rotation.."
274 X.MoveWait(-DISTANCE) # move backward and wait for motor stop 274 X.MoveWait(-DISTANCE) # move backward and wait for motor stop
275 print "Changing direction of rotation.." 275 print "Changing direction of rotation.."
276   276  
277 time.sleep(1) 277 time.sleep(1)
278   278  
279 X.Run(1, SPEED/2) 279 X.Run(1, SPEED/2)
280   280  
281 time.sleep(5) 281 time.sleep(5)
282   282  
283 X.Float(hard = False) # release power 283 X.Float(hard = False) # release power
284   284  
285   285  
286 finally: 286 finally:
287 print "stop" 287 print "stop"