Rev Author Line No. Line
4478 kaklik 1 #!/usr/bin/python
2 # -------------------------------------------
3 # HBSTEP01B Stepper Motor control test code
4 # -------------------------------------------
5 #
6 # Program uses MLAB Python modules library from https://github.com/MLAB-project/pymlab
7  
8  
9 #uncomment for debbug purposes
4734 kaklik 10 #import logging
11 #logging.basicConfig(level=logging.DEBUG)
4478 kaklik 12  
13 import sys
14 import time
4728 kaklik 15 from pymlab import config
4575 kaklik 16 import pylirc # infrared receiver binding
4478 kaklik 17  
4575 kaklik 18 #### Script Arguments ###############################################
4478 kaklik 19  
4575 kaklik 20 if len(sys.argv) == 2:
21 SPEED = eval(sys.argv[1])
4478 kaklik 22  
4575 kaklik 23 else:
4478 kaklik 24 sys.stderr.write("Invalid number of arguments.\n")
4575 kaklik 25 sys.stderr.write("Usage: %s BASE_SPEED (in steps/s)\n" % (sys.argv[0], ))
4478 kaklik 26 sys.exit(1)
27  
28  
4727 kaklik 29  
30 # Begin of Class Axis --------------------------------------------------
31  
4478 kaklik 32 class axis:
4727 kaklik 33 def __init__(self, SPI_CS, Direction, StepsPerUnit, MaxSpeed):
4478 kaklik 34 ' One axis of robot '
4727 kaklik 35 self.CS = SPI_CS
4478 kaklik 36 self.Dir = Direction
37 self.SPU = StepsPerUnit
4727 kaklik 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  
4478 kaklik 66 self.Reset()
4575 kaklik 67 self.Initialize()
4727 kaklik 68 self.MaxSpeed(self.maxspeed)
4478 kaklik 69  
70 def Reset(self):
4575 kaklik 71 'Reset the Axis'
4727 kaklik 72 spi.SPI_write_byte(self.CS, 0xC0) # reset
4575 kaklik 73  
74 def Initialize(self):
75 'set default parameters for H-bridge '
4727 kaklik 76 # spi.SPI_write_byte(self.CS, 0x14) # Stall Treshold setup
77 # spi.SPI_write_byte(self.CS, 0xFF)
78 # spi.SPI_write_byte(self.CS, 0x13) # Over Current Treshold setup
79 # spi.SPI_write_byte(self.CS, 0xFF)
80 spi.SPI_write_byte(self.CS, 0x15) # Full Step speed
81 spi.SPI_write_byte(self.CS, 0xFF)
82 spi.SPI_write_byte(self.CS, 0xFF)
83 spi.SPI_write_byte(self.CS, 0x05) # ACC
84 spi.SPI_write_byte(self.CS, 0x00)
85 spi.SPI_write_byte(self.CS, 0x10)
86 spi.SPI_write_byte(self.CS, 0x06) # DEC
87 spi.SPI_write_byte(self.CS, 0x00)
88 spi.SPI_write_byte(self.CS, 0x10)
89 spi.SPI_write_byte(self.CS, self.L6470_KVAL_RUN) # KVAL_RUN
4734 kaklik 90 spi.SPI_write_byte(self.CS, 0xF8)
4727 kaklik 91 spi.SPI_write_byte(self.CS, self.L6470_KVAL_ACC) # KVAL_ACC
4734 kaklik 92 spi.SPI_write_byte(self.CS, 0xF8)
4727 kaklik 93 spi.SPI_write_byte(self.CS, self.L6470_KVAL_DEC) # KVAL_DEC
4734 kaklik 94 spi.SPI_write_byte(self.CS, 0xF8)
95 spi.SPI_write_byte(self.CS, 0x18) # CONFIG
96 spi.SPI_write_byte(self.CS, 0b00101110)
97 spi.SPI_write_byte(self.CS, 0b10001000)
4727 kaklik 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  
4478 kaklik 107  
108 def MaxSpeed(self, speed):
4727 kaklik 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.'
4575 kaklik 110 speed_value = int(speed / 15.25)
4727 kaklik 111 if (speed_value <= 0):
4575 kaklik 112 speed_value = 1
4727 kaklik 113 elif (speed_value >= 1023):
114 speed_value = 1023
4478 kaklik 115  
4727 kaklik 116 data = [(speed_value >> i & 0xff) for i in (8,0)]
117 print data
118 spi.SPI_write_byte(self.CS, self.L6470_MAX_SPEED) # Max Speed setup
119 spi.SPI_write_byte(self.CS, data[0])
120 spi.SPI_write_byte(self.CS, data[1])
4575 kaklik 121 return (speed_value * 15.25)
122  
4478 kaklik 123 def ReleaseSW(self):
124 ' Go away from Limit Switch '
125 while self.ReadStatusBit(2) == 1: # is Limit Switch ON ?
4727 kaklik 126 spi.SPI_write_byte(self.CS, 0x92 | (~self.Dir & 1)) # release SW
127 while self.IsBusy():
4478 kaklik 128 pass
129 self.MoveWait(10) # move 10 units away
130  
131 def GoZero(self, speed):
132 ' Go to Zero position '
133 self.ReleaseSW()
4727 kaklik 134  
135 spi.SPI_write_byte(self.CS, 0x82 | (self.Dir & 1)) # Go to Zero
136 spi.SPI_write_byte(self.CS, 0x00)
137 spi.SPI_write_byte(self.CS, speed)
138 while self.IsBusy():
4478 kaklik 139 pass
140 time.sleep(0.3)
141 self.ReleaseSW()
142  
4575 kaklik 143 def GetStatus(self):
144 #self.spi.xfer([0b11010000]) # Get status command from datasheet - does not work for uknown rasons
4727 kaklik 145 spi.SPI_write_byte(self.CS, 0x39) # Gotparam command on status register
146 spi.SPI_write_byte(self.CS, 0x00)
147 data = [spi.SPI_read_byte()]
148 spi.SPI_write_byte(self.CS, 0x00)
149 data = data + [spi.SPI_read_byte()]
4575 kaklik 150  
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
152 ('STEP_LOSS_B',data[0] & 0x40 == 0x40),
153 ('STEP_LOSS_A',data[0] & 0x20 == 0x20),
154 ('OCD',data[0] & 0x10 == 0x10),
155 ('TH_SD',data[0] & 0x08 == 0x08),
156 ('TH_WRN',data[0] & 0x04 == 0x04),
157 ('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.
159 ('NOTPERF_CMD',data[1] & 0x80 == 0x80),
160 ('MOT_STATUS',data[1] & 0x60),
161 ('DIR',data[1] & 0x10 == 0x10),
162 ('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).
164 ('BUSY',data[1] & 0x02 != 0x02),
165 ('HIZ',data[1] & 0x01 == 0x01)])
166 return status
167  
4727 kaklik 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  
4478 kaklik 178 def Move(self, units):
179 ' Move some distance units from current position '
180 steps = units * self.SPU # translate units to steps
181 if steps > 0: # look for direction
4727 kaklik 182 spi.SPI_write_byte(self.CS, 0x40 | (~self.Dir & 1))
4478 kaklik 183 else:
4727 kaklik 184 spi.SPI_write_byte(self.CS, 0x40 | (self.Dir & 1))
4478 kaklik 185 steps = int(abs(steps))
4727 kaklik 186 spi.SPI_write_byte(self.CS, (steps >> 16) & 0xFF)
187 spi.SPI_write_byte(self.CS, (steps >> 8) & 0xFF)
188 spi.SPI_write_byte(self.CS, steps & 0xFF)
4478 kaklik 189  
4575 kaklik 190 def Run(self, direction, speed):
191 speed_value = int(speed / 0.015)
4727 kaklik 192 print hex(speed_value)
4575 kaklik 193  
4727 kaklik 194 command = 0b01010000 + int(direction)
195 data = [(speed_value >> i & 0xff) for i in (16,8,0)]
196 spi.SPI_write_byte(self.CS, command) # Max Speed setup
197 spi.SPI_write_byte(self.CS, data[0])
198 spi.SPI_write_byte(self.CS, data[1])
199 spi.SPI_write_byte(self.CS, data[2])
4575 kaklik 200 return (speed_value * 0.015)
201  
4478 kaklik 202 def MoveWait(self, units):
203 ' Move some distance units from current position and wait for execution '
204 self.Move(units)
4575 kaklik 205 while self.GetStatus()['BUSY']:
4727 kaklik 206 time.sleep(0.1)
4478 kaklik 207  
4575 kaklik 208 def Float(self, hard = False):
4478 kaklik 209 ' switch H-bridge to High impedance state '
4575 kaklik 210 if (hard == False):
4727 kaklik 211 spi.SPI_write_byte(self.CS, 0xA0)
4478 kaklik 212 else:
4727 kaklik 213 spi.SPI_write_byte(self.CS, 0xA8)
4478 kaklik 214  
215  
216 # End Class axis --------------------------------------------------
217  
4727 kaklik 218  
4575 kaklik 219 print "Clock motor control script started. \r\n"
220 print "Requested speed is: %f steps/s" % SPEED
4478 kaklik 221  
4575 kaklik 222 pylirc.init("pylirc", "/home/odroid/conf")
4478 kaklik 223  
4727 kaklik 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 spi = cfg.get_device("spi")
244  
245 spi.route()
246  
247  
4575 kaklik 248 try:
249 print "Configuring SPI.."
4727 kaklik 250 spi.SPI_config(spi.I2CSPI_MSB_FIRST| spi.I2CSPI_MODE_CLK_IDLE_HIGH_DATA_EDGE_TRAILING| spi.I2CSPI_CLK_461kHz)
251 time.sleep(0.1)
4478 kaklik 252  
4575 kaklik 253 print "Configuring stepper motor.."
4727 kaklik 254 X = axis(spi.I2CSPI_SS0, 0, 1, MaxSpeed = SPEED) # set Number of Steps per axis Unit and set Direction of Rotation
4721 kaklik 255 # maximum_speed = X.MaxSpeed(100.0)
256 #X.GetStatus()
4478 kaklik 257  
4721 kaklik 258 # print "Motor speed limit is: %f steps/s" % maximum_speed
4640 kaklik 259 running = False
4478 kaklik 260  
4575 kaklik 261 print "Waiting for IR command.."
262 while True: # set maximal motor speed
263 key = pylirc.nextcode() ## preccessing the IR remote control commands.
4478 kaklik 264  
4575 kaklik 265 if key == ['start']:
4640 kaklik 266 running = True
4715 kaklik 267 direction = True
4640 kaklik 268 requested_speed = SPEED
4478 kaklik 269  
4575 kaklik 270 if key == ['faster']:
4640 kaklik 271 running = True
4715 kaklik 272 direction = True
4640 kaklik 273 requested_speed = SPEED * 1.2 # runnig the motor at 120% of the base motor speed
4478 kaklik 274  
4575 kaklik 275 if key == ['slower']:
4640 kaklik 276 running = True
4715 kaklik 277 direction = False
4734 kaklik 278 requested_speed = SPEED * 0.8
4478 kaklik 279  
4575 kaklik 280 if key == ['stop']:
4640 kaklik 281 running = False
282  
283 time.sleep(0.1)
284  
285 if running == True:
4715 kaklik 286 real_speed = X.Run(direction, requested_speed)
4640 kaklik 287 print "Motor running at: %f steps/s" % real_speed
288 else:
4654 kaklik 289 X.Reset()
4721 kaklik 290 #X.Initialize()
4575 kaklik 291 X.Float(hard=False) # release power
292 print "Stopping the motor."
4478 kaklik 293  
4575 kaklik 294 except KeyboardInterrupt:
295 print "stop"
296 X.Float(hard=False) # release power
297 sys.exit(0)
4478 kaklik 298  
4575 kaklik 299 except Exception, e:
4721 kaklik 300 # X.Float(hard=False) # release power
4575 kaklik 301 print >> sys.stderr, "Exception: %s" % str(e)
4714 jacho 302 sys.exit(1)