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