Rev Author Line No. Line
4557 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
4570 kaklik 10 import logging
11 logging.basicConfig(level=logging.DEBUG)
4557 kaklik 12  
13 import sys
14 import time
15 import spidev
16  
17 #### Script Arguments ###############################################
18  
19 if len(sys.argv) < 2:
20 sys.stderr.write("Invalid number of arguments.\n")
21 sys.stderr.write("Usage: %s PORT ADDRESS SPEED MOVE_DISTANCE\n" % (sys.argv[0], ))
22 sys.exit(1)
23  
24 elif len(sys.argv) == 2:
25 PORT = eval(sys.argv[1])
4718 kaklik 26 SPEED = 50
4722 kaklik 27 DISTANCE = 500
4557 kaklik 28  
29 elif len(sys.argv) == 3:
30 SPEED = eval(sys.argv[2])
4718 kaklik 31 DISTANCE = 1000
4557 kaklik 32  
33 elif len(sys.argv) == 4:
34 SPEED = eval(sys.argv[2])
35 DISTANCE = eval(sys.argv[3])
36  
37 else:
38 PORT = 0
4718 kaklik 39 SPEED = 20
40 DISTANCE = 5000
4557 kaklik 41  
42 class axis:
4573 kaklik 43 def __init__(self, SPI_handler, Direction, StepsPerUnit, MaxSpeed):
4557 kaklik 44 ' One axis of robot '
45 self.spi = SPI_handler
46 self.Dir = Direction
47 self.SPU = StepsPerUnit
4718 kaklik 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  
4722 kaklik 76 # self.Reset()
77 # self.Initialize()
4557 kaklik 78  
79 def Reset(self):
4573 kaklik 80 'Reset the Axis'
4565 kaklik 81 self.spi.xfer([0xC0]) # reset
4573 kaklik 82  
83 def Initialize(self):
84 'set default parameters for H-bridge '
4557 kaklik 85 # self.spi.xfer( 0x14) # Stall Treshold setup
86 # self.spi.xfer( 0xFF)
87 # self.spi.xfer( 0x13) # Over Current Treshold setup
88 # self.spi.xfer( 0xFF)
4722 kaklik 89 # self.spi.xfer([0x15]) # Full Step speed
90 # self.spi.xfer([0xFF])
91 # self.spi.xfer([0xFF])
92 # self.spi.xfer([0xFF])
4565 kaklik 93 self.spi.xfer([0x05]) # ACC
94 self.spi.xfer([0x00])
95 self.spi.xfer([0x10])
96 self.spi.xfer([0x06]) # DEC
97 self.spi.xfer([0x00])
98 self.spi.xfer([0x10])
4718 kaklik 99 self.spi.xfer([self.L6470_KVAL_RUN]) # KVAL_RUN
4573 kaklik 100 self.spi.xfer([0x50])
4718 kaklik 101 self.spi.xfer([self.L6470_KVAL_ACC]) # KVAL_ACC
4573 kaklik 102 self.spi.xfer([0x50])
4718 kaklik 103 self.spi.xfer([self.L6470_KVAL_DEC]) # KVAL_DEC
4574 kaklik 104 self.spi.xfer([0x50])
4573 kaklik 105 # self.spi.xfer([0x18]) # CONFIG
106 # self.spi.xfer([0b00111000])
107 # self.spi.xfer([0b00000000])
4722 kaklik 108 # self.MaxSpeed(self.maxspeed)
4557 kaklik 109  
110 def MaxSpeed(self, speed):
4718 kaklik 111 '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.'
4573 kaklik 112 speed_value = int(speed / 15.25)
4718 kaklik 113 if (speed_value <= 0):
4573 kaklik 114 speed_value = 1
4718 kaklik 115 elif (speed_value >= 1023):
116 speed_value = 1023
4557 kaklik 117  
4718 kaklik 118 data = [(speed_value >> i & 0xff) for i in (8,0)]
119 self.spi.xfer([self.L6470_MAX_SPEED]) # Max Speed setup
120 self.spi.xfer([data[0]])
4573 kaklik 121 self.spi.xfer([data[1]])
122 return (speed_value * 15.25)
123  
4557 kaklik 124 def ReleaseSW(self):
125 ' Go away from Limit Switch '
126 while self.ReadStatusBit(2) == 1: # is Limit Switch ON ?
4565 kaklik 127 self.spi.xfer([0x92 | (~self.Dir & 1)]) # release SW
4573 kaklik 128 while self.GetStatus()['BUSY']:
4557 kaklik 129 pass
130 self.MoveWait(10) # move 10 units away
131  
132 def GoZero(self, speed):
133 ' Go to Zero position '
134 self.ReleaseSW()
4565 kaklik 135 self.spi.xfer([0x82 | (self.Dir & 1)]) # Go to Zero
136 self.spi.xfer([0x00])
137 self.spi.xfer([speed])
4573 kaklik 138 while self.GetStatus()['BUSY']:
4557 kaklik 139 pass
140 time.sleep(0.3)
141 self.ReleaseSW()
142  
4573 kaklik 143 def GetStatus(self):
144 #self.spi.xfer([0b11010000]) # Get status command from datasheet - does not work for uknown rasons
145 self.spi.xfer([0x39]) # Gotparam command on status register
146 data = self.spi.readbytes(1)
147 data = data + self.spi.readbytes(1)
148  
149 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
150 ('STEP_LOSS_B',data[0] & 0x40 == 0x40),
151 ('STEP_LOSS_A',data[0] & 0x20 == 0x20),
152 ('OCD',data[0] & 0x10 == 0x10),
153 ('TH_SD',data[0] & 0x08 == 0x08),
154 ('TH_WRN',data[0] & 0x04 == 0x04),
155 ('UVLO',data[0] & 0x02 == 0x02),
156 ('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.
157 ('NOTPERF_CMD',data[1] & 0x80 == 0x80),
158 ('MOT_STATUS',data[1] & 0x60),
159 ('DIR',data[1] & 0x10 == 0x10),
160 ('SW_EVN',data[1] & 0x08 == 0x08),
161 ('SW_F',data[1] & 0x04 == 0x04), #The SW_F flag reports the SW input status (low for open and high for closed).
162 ('BUSY',data[1] & 0x02 != 0x02),
163 ('HIZ',data[1] & 0x01 == 0x01)])
164 return status
165  
4722 kaklik 166 def GetACC(self):
167 # self.spi.xfer([0x29]) # Gotparam command on status register
168 self.spi.xfer([0x2D]) # Gotparam command on status register
169 data = self.spi.readbytes(1)
170 data = data + self.spi.readbytes(1)
171 print data
172  
4557 kaklik 173 def Move(self, units):
174 ' Move some distance units from current position '
175 steps = units * self.SPU # translate units to steps
176 if steps > 0: # look for direction
4565 kaklik 177 self.spi.xfer([0x40 | (~self.Dir & 1)])
4557 kaklik 178 else:
4565 kaklik 179 self.spi.xfer([0x40 | (self.Dir & 1)])
4557 kaklik 180 steps = int(abs(steps))
4565 kaklik 181 self.spi.xfer([(steps >> 16) & 0xFF])
182 self.spi.xfer([(steps >> 8) & 0xFF])
183 self.spi.xfer([steps & 0xFF])
4557 kaklik 184  
4573 kaklik 185 def Run(self, direction, speed):
186 speed_value = int(speed / 0.015)
187 print hex(speed_value)
188  
189 data = [0b01010000 + direction]
190 data = data +[(speed_value >> i & 0xff) for i in (16,8,0)]
191 self.spi.xfer([data[0]]) # Max Speed setup
192 self.spi.xfer([data[1]])
193 self.spi.xfer([data[2]])
194 self.spi.xfer([data[3]])
195 return (speed_value * 0.015)
196  
4557 kaklik 197 def MoveWait(self, units):
198 ' Move some distance units from current position and wait for execution '
199 self.Move(units)
4573 kaklik 200 while self.GetStatus()['BUSY']:
4557 kaklik 201 pass
4722 kaklik 202 print self.GetStatus()
4570 kaklik 203 time.sleep(0.8)
4557 kaklik 204  
4722 kaklik 205  
4573 kaklik 206 def Float(self, hard = False):
4557 kaklik 207 ' switch H-bridge to High impedance state '
4573 kaklik 208 if (hard == False):
209 self.spi.xfer([0xA0])
4557 kaklik 210 else:
4573 kaklik 211 self.spi.xfer([0xA8])
4557 kaklik 212  
4570 kaklik 213  
4557 kaklik 214 # End Class axis --------------------------------------------------
215  
216 print "Stepper motor control test started. \r\n"
4573 kaklik 217 print "Max motor speed: %f " % SPEED
218 print "Distance to run: %f " % DISTANCE
4557 kaklik 219  
220 try:
221 print "SPI configuration.."
222 spi = spidev.SpiDev() # create a spi object
4561 kaklik 223 spi.open(0, 0) # open spi port 0, device (CS) 0
4570 kaklik 224 spi.mode = 0b01
4563 kaklik 225 spi.lsbfirst = False
226 spi.bits_per_word = 8
227 spi.cshigh = False
4573 kaklik 228 spi.max_speed_hz = 100000
4563 kaklik 229 #spi.SPI_config(spi.I2CSPI_MSB_FIRST| spi.I2CSPI_MODE_CLK_IDLE_HIGH_DATA_EDGE_TRAILING| spi.I2CSPI_CLK_461kHz)
4557 kaklik 230 time.sleep(1)
231  
232 print "Axis inicialization"
4718 kaklik 233 X = axis(spi, 0, 1, MaxSpeed = SPEED) # set Number of Steps per axis Unit and set Direction of Rotation
4557 kaklik 234  
4722 kaklik 235 #X.Reset()
236  
237 X.GetACC()
238  
239 exit()
240  
241 print X.GetStatus()
242 time.sleep(1)
243  
244 print "Setting maximal speed"
4573 kaklik 245 print X.MaxSpeed(SPEED) # set maximal motor speed
4722 kaklik 246 print X.GetStatus()
247 time.sleep(1)
4573 kaklik 248  
4722 kaklik 249 print X.Run(1, 20.456431)
250 print X.GetStatus()
251 time.sleep(5)
4574 kaklik 252  
4557 kaklik 253 print "Axis is running"
254 for i in range(5):
255 print i
256 X.MoveWait(DISTANCE) # move forward and wait for motor stop
257 print "Changing direction of rotation.."
4570 kaklik 258 time.sleep(1.1)
4557 kaklik 259 X.MoveWait(-DISTANCE) # move backward and wait for motor stop
260 print "Changing direction of rotation.."
4570 kaklik 261 time.sleep(1.1)
4574 kaklik 262  
4573 kaklik 263 X.Float(hard=False) # release power
4557 kaklik 264  
265 finally:
266 print "stop"