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