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