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