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