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