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