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
4723 kaklik 202 time.sleep(0.1)
4557 kaklik 203  
4722 kaklik 204  
4573 kaklik 205 def Float(self, hard = False):
4557 kaklik 206 ' switch H-bridge to High impedance state '
4573 kaklik 207 if (hard == False):
208 self.spi.xfer([0xA0])
4557 kaklik 209 else:
4573 kaklik 210 self.spi.xfer([0xA8])
4557 kaklik 211  
4570 kaklik 212  
4557 kaklik 213 # End Class axis --------------------------------------------------
214  
215 print "Stepper motor control test started. \r\n"
4573 kaklik 216 print "Max motor speed: %f " % SPEED
217 print "Distance to run: %f " % DISTANCE
4557 kaklik 218  
219 try:
220 print "SPI configuration.."
221 spi = spidev.SpiDev() # create a spi object
4561 kaklik 222 spi.open(0, 0) # open spi port 0, device (CS) 0
4570 kaklik 223 spi.mode = 0b01
4563 kaklik 224 spi.lsbfirst = False
225 spi.bits_per_word = 8
226 spi.cshigh = False
4573 kaklik 227 spi.max_speed_hz = 100000
4563 kaklik 228 #spi.SPI_config(spi.I2CSPI_MSB_FIRST| spi.I2CSPI_MODE_CLK_IDLE_HIGH_DATA_EDGE_TRAILING| spi.I2CSPI_CLK_461kHz)
4557 kaklik 229 time.sleep(1)
230  
231 print "Axis inicialization"
4718 kaklik 232 X = axis(spi, 0, 1, MaxSpeed = SPEED) # set Number of Steps per axis Unit and set Direction of Rotation
4557 kaklik 233  
4722 kaklik 234 #X.Reset()
235  
236 X.GetACC()
237  
238 exit()
239  
240 print X.GetStatus()
241 time.sleep(1)
242  
243 print "Setting maximal speed"
4573 kaklik 244 print X.MaxSpeed(SPEED) # set maximal motor speed
4722 kaklik 245 print X.GetStatus()
246 time.sleep(1)
4573 kaklik 247  
4722 kaklik 248 print X.Run(1, 20.456431)
249 print X.GetStatus()
250 time.sleep(5)
4574 kaklik 251  
4557 kaklik 252 print "Axis is running"
253 for i in range(5):
254 print i
255 X.MoveWait(DISTANCE) # move forward and wait for motor stop
256 print "Changing direction of rotation.."
4570 kaklik 257 time.sleep(1.1)
4557 kaklik 258 X.MoveWait(-DISTANCE) # move backward and wait for motor stop
259 print "Changing direction of rotation.."
4570 kaklik 260 time.sleep(1.1)
4574 kaklik 261  
4573 kaklik 262 X.Float(hard=False) # release power
4557 kaklik 263  
264 finally:
265 print "stop"