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