Rev 4719 Rev 4723
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   45  
45 class axis: 46 class axis:
46 def __init__(self, SPI_CS, Direction, StepsPerUnit, MaxSpeed): 47 def __init__(self, SPI_CS, Direction, StepsPerUnit, MaxSpeed):
47 ' One axis of robot ' 48 ' One axis of robot '
48 self.CS = SPI_CS 49 self.CS = SPI_CS
49 self.Dir = Direction 50 self.Dir = Direction
50 self.SPU = StepsPerUnit 51 self.SPU = StepsPerUnit
51 self.maxspeed = MaxSpeed 52 self.maxspeed = MaxSpeed
52   53  
53 self.L6470_ABS_POS =0x01 54 self.L6470_ABS_POS =0x01
54 self.L6470_EL_POS =0x02 55 self.L6470_EL_POS =0x02
55 self.L6470_MARK =0x03 56 self.L6470_MARK =0x03
56 self.L6470_SPEED =0x04 57 self.L6470_SPEED =0x04
57 self.L6470_ACC =0x05 58 self.L6470_ACC =0x05
58 self.L6470_DEC =0x06 59 self.L6470_DEC =0x06
59 self.L6470_MAX_SPEED =0x07 60 self.L6470_MAX_SPEED =0x07
60 self.L6470_MIN_SPEED =0x08 61 self.L6470_MIN_SPEED =0x08
61 self.L6470_FS_SPD =0x15 62 self.L6470_FS_SPD =0x15
62 self.L6470_KVAL_HOLD =0x09 63 self.L6470_KVAL_HOLD =0x09
63 self.L6470_KVAL_RUN =0x0A 64 self.L6470_KVAL_RUN =0x0A
64 self.L6470_KVAL_ACC =0x0B 65 self.L6470_KVAL_ACC =0x0B
65 self.L6470_KVAL_DEC =0x0C 66 self.L6470_KVAL_DEC =0x0C
66 self.L6470_INT_SPEED =0x0D 67 self.L6470_INT_SPEED =0x0D
67 self.L6470_ST_SLP =0x0E 68 self.L6470_ST_SLP =0x0E
68 self.L6470_FN_SLP_ACC =0x0F 69 self.L6470_FN_SLP_ACC =0x0F
69 self.L6470_FN_SLP_DEC =0x10 70 self.L6470_FN_SLP_DEC =0x10
70 self.L6470_K_THERM =0x11 71 self.L6470_K_THERM =0x11
71 self.L6470_ADC_OUT =0x12 72 self.L6470_ADC_OUT =0x12
72 self.L6470_OCD_TH =0x13 73 self.L6470_OCD_TH =0x13
73 self.L6470_STALL_TH =0x14 74 self.L6470_STALL_TH =0x14
74 self.L6470_STEP_MODE =0x16 75 self.L6470_STEP_MODE =0x16
75 self.L6470_ALARM_EN =0x17 76 self.L6470_ALARM_EN =0x17
76 self.L6470_CONFIG =0x18 77 self.L6470_CONFIG =0x18
77 self.L6470_STATUS =0x19 78 self.L6470_STATUS =0x19
78   79  
79 self.Reset() 80 self.Reset()
80 self.Initialize() 81 self.Initialize()
-   82 self.MaxSpeed(self.maxspeed)
81   83  
82 def Reset(self): 84 def Reset(self):
83 'Reset the Axis' 85 'Reset the Axis'
84 spi.SPI_write_byte(self.CS, 0xC0) # reset 86 spi.SPI_write_byte(self.CS, 0xC0) # reset
85   87  
86 def Initialize(self): 88 def Initialize(self):
87 'set default parameters for H-bridge ' 89 'set default parameters for H-bridge '
88 # spi.SPI_write_byte(self.CS, 0x14) # Stall Treshold setup 90 # spi.SPI_write_byte(self.CS, 0x14) # Stall Treshold setup
89 # spi.SPI_write_byte(self.CS, 0xFF) 91 # spi.SPI_write_byte(self.CS, 0xFF)
90 # spi.SPI_write_byte(self.CS, 0x13) # Over Current Treshold setup 92 # spi.SPI_write_byte(self.CS, 0x13) # Over Current Treshold setup
91 # spi.SPI_write_byte(self.CS, 0xFF) 93 # spi.SPI_write_byte(self.CS, 0xFF)
92 spi.SPI_write_byte(self.CS, 0x15) # Full Step speed 94 spi.SPI_write_byte(self.CS, 0x15) # Full Step speed
93 spi.SPI_write_byte(self.CS, 0xFF) 95 spi.SPI_write_byte(self.CS, 0xFF)
94 spi.SPI_write_byte(self.CS, 0xFF) 96 spi.SPI_write_byte(self.CS, 0xFF)
95 spi.SPI_write_byte(self.CS, 0x05) # ACC 97 spi.SPI_write_byte(self.CS, 0x05) # ACC
96 spi.SPI_write_byte(self.CS, 0x00) 98 spi.SPI_write_byte(self.CS, 0x00)
97 spi.SPI_write_byte(self.CS, 0x10) 99 spi.SPI_write_byte(self.CS, 0x10)
98 spi.SPI_write_byte(self.CS, 0x06) # DEC 100 spi.SPI_write_byte(self.CS, 0x06) # DEC
99 spi.SPI_write_byte(self.CS, 0x00) 101 spi.SPI_write_byte(self.CS, 0x00)
100 spi.SPI_write_byte(self.CS, 0x10) 102 spi.SPI_write_byte(self.CS, 0x10)
101 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
102 spi.SPI_write_byte(self.CS, 0x58) 104 spi.SPI_write_byte(self.CS, 0x58)
103 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
104 spi.SPI_write_byte(self.CS, 0x58) 106 spi.SPI_write_byte(self.CS, 0x58)
105 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
106 spi.SPI_write_byte(self.CS, 0x58) 108 spi.SPI_write_byte(self.CS, 0x58)
107 # spi.SPI_write_byte(self.CS, 0x18) # CONFIG 109 # spi.SPI_write_byte(self.CS, 0x18) # CONFIG
108 # spi.SPI_write_byte(self.CS, 0b00111000) 110 # spi.SPI_write_byte(self.CS, 0b00111000)
109 # spi.SPI_write_byte(self.CS, 0b00000000) 111 # spi.SPI_write_byte(self.CS, 0b00000000)
110 self.MaxSpeed(self.maxspeed) 112 self.MaxSpeed(self.maxspeed)
111   113  
112 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):
113 """ 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 """
114   116  
115 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):
116 """ 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 """
117   119  
118   120  
119 121
120 def MaxSpeed(self, speed): 122 def MaxSpeed(self, speed):
121 '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.'
122 speed_value = int(speed / 15.25) 124 speed_value = int(speed / 15.25)
123 if (speed_value <= 0): 125 if (speed_value <= 0):
124 speed_value = 1 126 speed_value = 1
125 elif (speed_value >= 1023): 127 elif (speed_value >= 1023):
126 speed_value = 1023 128 speed_value = 1023
127   129  
128 data = [(speed_value >> i & 0xff) for i in (8,0)] 130 data = [(speed_value >> i & 0xff) for i in (8,0)]
129 print data 131 print data
130 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
131 spi.SPI_write_byte(self.CS, data[0]) 133 spi.SPI_write_byte(self.CS, data[0])
132 spi.SPI_write_byte(self.CS, data[1]) 134 spi.SPI_write_byte(self.CS, data[1])
133 return (speed_value * 15.25) 135 return (speed_value * 15.25)
134   136  
135 def ReleaseSW(self): 137 def ReleaseSW(self):
136 ' Go away from Limit Switch ' 138 ' Go away from Limit Switch '
137 while self.ReadStatusBit(2) == 1: # is Limit Switch ON ? 139 while self.ReadStatusBit(2) == 1: # is Limit Switch ON ?
138 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
139 while self.IsBusy(): 141 while self.IsBusy():
140 pass 142 pass
141 self.MoveWait(10) # move 10 units away 143 self.MoveWait(10) # move 10 units away
142 144
143 def GoZero(self, speed): 145 def GoZero(self, speed):
144 ' Go to Zero position ' 146 ' Go to Zero position '
145 self.ReleaseSW() 147 self.ReleaseSW()
146   148  
147 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
148 spi.SPI_write_byte(self.CS, 0x00) 150 spi.SPI_write_byte(self.CS, 0x00)
149 spi.SPI_write_byte(self.CS, speed) 151 spi.SPI_write_byte(self.CS, speed)
150 while self.IsBusy(): 152 while self.IsBusy():
151 pass 153 pass
152 time.sleep(0.3) 154 time.sleep(0.3)
153 self.ReleaseSW() 155 self.ReleaseSW()
154   156  
-   157 def GetStatus(self):
-   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
-   160 spi.SPI_write_byte(self.CS, 0x00)
-   161 data = spi.SPI_read_byte()
-   162 spi.SPI_write_byte(self.CS, 0x00)
-   163 data = data + spi.SPI_read_byte()
-   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
-   166 ('STEP_LOSS_B',data[0] & 0x40 == 0x40),
-   167 ('STEP_LOSS_A',data[0] & 0x20 == 0x20),
-   168 ('OCD',data[0] & 0x10 == 0x10),
-   169 ('TH_SD',data[0] & 0x08 == 0x08),
-   170 ('TH_WRN',data[0] & 0x04 == 0x04),
-   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.
-   173 ('NOTPERF_CMD',data[1] & 0x80 == 0x80),
-   174 ('MOT_STATUS',data[1] & 0x60),
-   175 ('DIR',data[1] & 0x10 == 0x10),
-   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).
-   178 ('BUSY',data[1] & 0x02 != 0x02),
-   179 ('HIZ',data[1] & 0x01 == 0x01)])
-   180 return status
-   181  
-   182 def GetACC(self):
-   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
-   185 spi.SPI_write_byte(self.CS, 0x00)
-   186 data = spi.SPI_read_byte()
-   187 spi.SPI_write_byte(self.CS, 0x00)
-   188 data = data + self.spi.readbytes(1)
-   189 print data # return speed in real units
-   190  
-   191  
155 def Move(self, units): 192 def Move(self, units):
156 ' Move some distance units from current position ' 193 ' Move some distance units from current position '
157 steps = units * self.SPU # translate units to steps 194 steps = units * self.SPU # translate units to steps
158 if steps > 0: # look for direction 195 if steps > 0: # look for direction
159 spi.SPI_write_byte(self.CS, 0x40 | (~self.Dir & 1)) 196 spi.SPI_write_byte(self.CS, 0x40 | (~self.Dir & 1))
160 else: 197 else:
161 spi.SPI_write_byte(self.CS, 0x40 | (self.Dir & 1)) 198 spi.SPI_write_byte(self.CS, 0x40 | (self.Dir & 1))
162 steps = int(abs(steps)) 199 steps = int(abs(steps))
163 spi.SPI_write_byte(self.CS, (steps >> 16) & 0xFF) 200 spi.SPI_write_byte(self.CS, (steps >> 16) & 0xFF)
164 spi.SPI_write_byte(self.CS, (steps >> 8) & 0xFF) 201 spi.SPI_write_byte(self.CS, (steps >> 8) & 0xFF)
165 spi.SPI_write_byte(self.CS, steps & 0xFF) 202 spi.SPI_write_byte(self.CS, steps & 0xFF)
166   203  
167 def Run(self, direction, speed): 204 def Run(self, direction, speed):
168 speed_value = int(speed / 0.015) 205 speed_value = int(speed / 0.015)
169 print hex(speed_value) 206 print hex(speed_value)
170   207  
171 command = 0b01010000 + int(direction) 208 command = 0b01010000 + int(direction)
172 data = [(speed_value >> i & 0xff) for i in (16,8,0)] 209 data = [(speed_value >> i & 0xff) for i in (16,8,0)]
173 spi.SPI_write_byte(self.CS, command) # Max Speed setup 210 spi.SPI_write_byte(self.CS, command) # Max Speed setup
174 spi.SPI_write_byte(self.CS, data[0]) 211 spi.SPI_write_byte(self.CS, data[0])
175 spi.SPI_write_byte(self.CS, data[1]) 212 spi.SPI_write_byte(self.CS, data[1])
176 spi.SPI_write_byte(self.CS, data[2]) 213 spi.SPI_write_byte(self.CS, data[2])
177 return (speed_value * 0.015) 214 return (speed_value * 0.015)
178   215  
179 def MoveWait(self, units): 216 def MoveWait(self, units):
180 ' Move some distance units from current position and wait for execution ' 217 ' Move some distance units from current position and wait for execution '
181 self.Move(units) 218 self.Move(units)
182 while self.IsBusy(): 219 while self.GetStatus()['BUSY']:
183 pass 220 time.sleep(0.1)
184   221  
185 def Float(self, hard = False): 222 def Float(self, hard = False):
186 ' switch H-bridge to High impedance state ' 223 ' switch H-bridge to High impedance state '
187 if (hard == False): 224 if (hard == False):
188 spi.SPI_write_byte(self.CS, 0xA0) 225 spi.SPI_write_byte(self.CS, 0xA0)
189 else: 226 else:
190 spi.SPI_write_byte(self.CS, 0xA8) 227 spi.SPI_write_byte(self.CS, 0xA8)
191   228  
192 def ReadStatusBit(self, bit): -  
193 ' Report given status bit ' -  
194 spi.SPI_write_byte(self.CS, 0x39) # Read from address 0x19 (STATUS) -  
195 spi.SPI_write_byte(self.CS, 0x00) -  
196 data0 = spi.SPI_read_byte() # 1st byte -  
197 spi.SPI_write_byte(self.CS, 0x00) -  
198 data1 = spi.SPI_read_byte() # 2nd byte -  
199 #print hex(data0), hex(data1) -  
200 if bit > 7: # extract requested bit -  
201 OutputBit = (data0 >> (bit - 8)) & 1 -  
202 else: -  
203 OutputBit = (data1 >> bit) & 1 -  
204 return OutputBit -  
205   -  
206 -  
207 def IsBusy(self): -  
208 """ Return True if tehre are motion """ -  
209 if self.ReadStatusBit(1) == 1: -  
210 return False -  
211 else: -  
212 return True -  
213   229  
214 # End Class axis -------------------------------------------------- 230 # End Class axis --------------------------------------------------
215   231  
216   232  
217   233  
218 cfg = config.Config( 234 cfg = config.Config(
219 i2c = { 235 i2c = {
220 "port": 1, 236 "port": 1,
221 }, 237 },
222   238  
223 bus = [ 239 bus = [
224 { 240 {
225 "name":"spi", 241 "name":"spi",
226 "type":"i2cspi", 242 "type":"i2cspi",
227 "address": 0x2e, 243 "address": 0x2e,
228 }, 244 },
229 ], 245 ],
230 ) 246 )
231   247  
232   248  
233 cfg.initialize() 249 cfg.initialize()
234   250  
235 print "Stepper motor control test started. \r\n" 251 print "Stepper motor control test started. \r\n"
236 print "Max motor speed: %d " % SPEED 252 print "Max motor speed: %d " % SPEED
237 print "Distance to run: %d " % DISTANCE 253 print "Distance to run: %d " % DISTANCE
238   254  
239 spi = cfg.get_device("spi") 255 spi = cfg.get_device("spi")
240   256  
241 spi.route() 257 spi.route()
242   258  
243 try: 259 try:
244 print "SPI configuration.." 260 print "SPI configuration.."
245 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)
246 time.sleep(1) 262 time.sleep(1)
247   263  
248 print "Axis inicialization" 264 print "Axis inicialization"
249 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
250 X.MaxSpeed(SPEED) # set maximal motor speed 266 X.MaxSpeed(SPEED) # Reset maximal motor speed
251   267  
252 print "Axis is running" 268 print "Axis is running"
253   269  
254 for i in range(5): 270 for i in range(5):
255 print i 271 print i
256 X.MoveWait(DISTANCE) # move forward and wait for motor stop 272 X.MoveWait(DISTANCE) # move forward and wait for motor stop
257 print "Changing direction of rotation.." 273 print "Changing direction of rotation.."
258 X.MoveWait(-DISTANCE) # move backward and wait for motor stop 274 X.MoveWait(-DISTANCE) # move backward and wait for motor stop
259 print "Changing direction of rotation.." 275 print "Changing direction of rotation.."
260   276  
261 time.sleep(1) 277 time.sleep(1)
262   278  
263 X.Run(1, SPEED/2) 279 X.Run(1, SPEED/2)
264   280  
265 time.sleep(5) 281 time.sleep(5)
266   282  
267 X.Float(hard = False) # release power 283 X.Float(hard = False) # release power
268   284  
269   285  
270 finally: 286 finally:
271 print "stop" 287 print "stop"