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  
44  
45 class axis:
4718 kaklik 46 def __init__(self, SPI_CS, Direction, StepsPerUnit, MaxSpeed):
4448 kaklik 47 ' One axis of robot '
48 self.CS = SPI_CS
49 self.Dir = Direction
50 self.SPU = StepsPerUnit
4718 kaklik 51 self.maxspeed = MaxSpeed
52  
53 self.L6470_ABS_POS =0x01
54 self.L6470_EL_POS =0x02
55 self.L6470_MARK =0x03
56 self.L6470_SPEED =0x04
57 self.L6470_ACC =0x05
58 self.L6470_DEC =0x06
59 self.L6470_MAX_SPEED =0x07
60 self.L6470_MIN_SPEED =0x08
61 self.L6470_FS_SPD =0x15
62 self.L6470_KVAL_HOLD =0x09
63 self.L6470_KVAL_RUN =0x0A
64 self.L6470_KVAL_ACC =0x0B
65 self.L6470_KVAL_DEC =0x0C
66 self.L6470_INT_SPEED =0x0D
67 self.L6470_ST_SLP =0x0E
68 self.L6470_FN_SLP_ACC =0x0F
69 self.L6470_FN_SLP_DEC =0x10
70 self.L6470_K_THERM =0x11
71 self.L6470_ADC_OUT =0x12
72 self.L6470_OCD_TH =0x13
73 self.L6470_STALL_TH =0x14
74 self.L6470_STEP_MODE =0x16
75 self.L6470_ALARM_EN =0x17
76 self.L6470_CONFIG =0x18
77 self.L6470_STATUS =0x19
78  
4448 kaklik 79 self.Reset()
4718 kaklik 80 self.Initialize()
4448 kaklik 81  
82 def Reset(self):
4718 kaklik 83 'Reset the Axis'
4448 kaklik 84 spi.SPI_write_byte(self.CS, 0xC0) # reset
4718 kaklik 85  
86 def Initialize(self):
87 'set default parameters for H-bridge '
4448 kaklik 88 # spi.SPI_write_byte(self.CS, 0x14) # Stall Treshold setup
89 # spi.SPI_write_byte(self.CS, 0xFF)
90 # spi.SPI_write_byte(self.CS, 0x13) # Over Current Treshold setup
91 # spi.SPI_write_byte(self.CS, 0xFF)
92 spi.SPI_write_byte(self.CS, 0x15) # Full Step speed
93 spi.SPI_write_byte(self.CS, 0xFF)
94 spi.SPI_write_byte(self.CS, 0xFF)
95 spi.SPI_write_byte(self.CS, 0x05) # ACC
96 spi.SPI_write_byte(self.CS, 0x00)
97 spi.SPI_write_byte(self.CS, 0x10)
98 spi.SPI_write_byte(self.CS, 0x06) # DEC
99 spi.SPI_write_byte(self.CS, 0x00)
100 spi.SPI_write_byte(self.CS, 0x10)
4718 kaklik 101 spi.SPI_write_byte(self.CS, self.L6470_KVAL_RUN) # KVAL_RUN
102 spi.SPI_write_byte(self.CS, 0x28)
103 spi.SPI_write_byte(self.CS, self.L6470_KVAL_ACC) # KVAL_ACC
104 spi.SPI_write_byte(self.CS, 0x28)
105 spi.SPI_write_byte(self.CS, self.L6470_KVAL_DEC) # KVAL_DEC
106 spi.SPI_write_byte(self.CS, 0x28)
107 # spi.SPI_write_byte(self.CS, 0x18) # CONFIG
108 # spi.SPI_write_byte(self.CS, 0b00111000)
109 # spi.SPI_write_byte(self.CS, 0b00000000)
110 self.MaxSpeed(self.maxspeed)
111  
112 def setKVAL(self, hold = 0.5, run = 0.5, acc = 0.5, dec = 0.5):
113 """ The available range is from 0 to 0.996 x VS with a resolution of 0.004 x VS """
114  
115 def setOverCurrentTH(self, hold = 0.5, run = 0.5, acc = 0.5, dec = 0.5):
116 """ The available range is from 375 mA to 6 A, in steps of 375 mA """
117  
118  
4448 kaklik 119  
120 def MaxSpeed(self, speed):
4718 kaklik 121 '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.'
122 speed_value = int(speed / 15.25)
123 if (speed_value <= 0):
124 speed_value = 1
125 elif (speed_value >= 1023):
126 speed_value = 1023
4448 kaklik 127  
4718 kaklik 128 data = [(speed_value >> i & 0xff) for i in (8,0)]
129 print data
130 spi.SPI_write_byte(self.CS, self.L6470_MAX_SPEED) # Max Speed setup
131 spi.SPI_write_byte(self.CS, data[0])
132 spi.SPI_write_byte(self.CS, data[1])
133 return (speed_value * 15.25)
134  
4448 kaklik 135 def ReleaseSW(self):
136 ' Go away from Limit Switch '
137 while self.ReadStatusBit(2) == 1: # is Limit Switch ON ?
138 spi.SPI_write_byte(self.CS, 0x92 | (~self.Dir & 1)) # release SW
139 while self.IsBusy():
140 pass
141 self.MoveWait(10) # move 10 units away
142  
143 def GoZero(self, speed):
144 ' Go to Zero position '
145 self.ReleaseSW()
146  
147 spi.SPI_write_byte(self.CS, 0x82 | (self.Dir & 1)) # Go to Zero
148 spi.SPI_write_byte(self.CS, 0x00)
149 spi.SPI_write_byte(self.CS, speed)
150 while self.IsBusy():
151 pass
152 time.sleep(0.3)
153 self.ReleaseSW()
154  
155 def Move(self, units):
156 ' Move some distance units from current position '
157 steps = units * self.SPU # translate units to steps
158 if steps > 0: # look for direction
159 spi.SPI_write_byte(self.CS, 0x40 | (~self.Dir & 1))
160 else:
161 spi.SPI_write_byte(self.CS, 0x40 | (self.Dir & 1))
162 steps = int(abs(steps))
163 spi.SPI_write_byte(self.CS, (steps >> 16) & 0xFF)
164 spi.SPI_write_byte(self.CS, (steps >> 8) & 0xFF)
165 spi.SPI_write_byte(self.CS, steps & 0xFF)
166  
4718 kaklik 167 def Run(self, direction, speed):
168 speed_value = int(speed / 0.015)
169 print hex(speed_value)
170  
171 command = 0b01010000 + int(direction)
172 data = [(speed_value >> i & 0xff) for i in (16,8,0)]
173 spi.SPI_write_byte(self.CS, command) # Max Speed setup
174 spi.SPI_write_byte(self.CS, data[0])
175 spi.SPI_write_byte(self.CS, data[1])
176 spi.SPI_write_byte(self.CS, data[2])
177 return (speed_value * 0.015)
178  
4448 kaklik 179 def MoveWait(self, units):
180 ' Move some distance units from current position and wait for execution '
181 self.Move(units)
182 while self.IsBusy():
183 pass
184  
4718 kaklik 185 def Float(self, hard = False):
4448 kaklik 186 ' switch H-bridge to High impedance state '
4718 kaklik 187 if (hard == False):
188 spi.SPI_write_byte(self.CS, 0xA0)
189 else:
190 spi.SPI_write_byte(self.CS, 0xA8)
4448 kaklik 191  
192 def ReadStatusBit(self, bit):
193 ' Report given status bit '
194 spi.SPI_write_byte(self.CS, 0x39) # Read from address 0x19 (STATUS)
195 spi.SPI_write_byte(self.CS, 0x00)
196 data0 = spi.SPI_read_byte() # 1st byte
197 spi.SPI_write_byte(self.CS, 0x00)
198 data1 = spi.SPI_read_byte() # 2nd byte
199 #print hex(data0), hex(data1)
200 if bit > 7: # extract requested bit
201 OutputBit = (data0 >> (bit - 8)) & 1
202 else:
203 OutputBit = (data1 >> bit) & 1
204 return OutputBit
205  
206  
207 def IsBusy(self):
208 """ Return True if tehre are motion """
209 if self.ReadStatusBit(1) == 1:
210 return False
211 else:
212 return True
213  
214 # End Class axis --------------------------------------------------
215  
216  
217  
218 cfg = config.Config(
219 i2c = {
220 "port": 1,
221 },
222  
223 bus = [
224 {
225 "name":"spi",
226 "type":"i2cspi",
227 "address": 0x2e,
228 },
229 ],
230 )
231  
232  
233 cfg.initialize()
234  
235 print "Stepper motor control test started. \r\n"
4450 kaklik 236 print "Max motor speed: %d " % SPEED
237 print "Distance to run: %d " % DISTANCE
4448 kaklik 238  
239 spi = cfg.get_device("spi")
240  
241 spi.route()
242  
243 try:
244 print "SPI configuration.."
245 spi.SPI_config(spi.I2CSPI_MSB_FIRST| spi.I2CSPI_MODE_CLK_IDLE_HIGH_DATA_EDGE_TRAILING| spi.I2CSPI_CLK_461kHz)
246 time.sleep(1)
247  
248 print "Axis inicialization"
4718 kaklik 249 X = axis(spi.I2CSPI_SS0, 0, 1, MaxSpeed = SPEED) # set Number of Steps per axis Unit and set Direction of Rotation
4448 kaklik 250 X.MaxSpeed(SPEED) # set maximal motor speed
251  
252 print "Axis is running"
253  
254 for i in range(5):
4450 kaklik 255 print i
4448 kaklik 256 X.MoveWait(DISTANCE) # move forward and wait for motor stop
4450 kaklik 257 print "Changing direction of rotation.."
4448 kaklik 258 X.MoveWait(-DISTANCE) # move backward and wait for motor stop
4450 kaklik 259 print "Changing direction of rotation.."
4448 kaklik 260  
4718 kaklik 261 time.sleep(1)
4448 kaklik 262  
4718 kaklik 263 X.Run(1, SPEED/2)
4448 kaklik 264  
4718 kaklik 265 time.sleep(3)
266  
267 X.Float(hard = False) # release power
268  
269  
4448 kaklik 270 finally:
271 print "stop"