Rev 4715 Rev 4716
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 # SPI binding 15 import spidev # SPI binding
16 import pylirc # infrared receiver binding 16 import pylirc # infrared receiver binding
17   17  
18 #### Script Arguments ############################################### 18 #### Script Arguments ###############################################
19   19  
20 if len(sys.argv) == 2: 20 if len(sys.argv) == 2:
21 SPEED = eval(sys.argv[1]) 21 SPEED = eval(sys.argv[1])
22   22  
23 else: 23 else:
24 sys.stderr.write("Invalid number of arguments.\n") 24 sys.stderr.write("Invalid number of arguments.\n")
25 sys.stderr.write("Usage: %s BASE_SPEED (in steps/s)\n" % (sys.argv[0], )) 25 sys.stderr.write("Usage: %s BASE_SPEED (in steps/s)\n" % (sys.argv[0], ))
26 sys.exit(1) 26 sys.exit(1)
27   27  
28   28  
29 class axis: 29 class axis:
30 def __init__(self, SPI_handler, Direction, StepsPerUnit): 30 def __init__(self, SPI_handler, Direction, StepsPerUnit):
31 ' One axis of robot ' 31 ' One axis of robot '
32 self.spi = SPI_handler 32 self.spi = SPI_handler
33 self.Dir = Direction 33 self.Dir = Direction
34 self.SPU = StepsPerUnit 34 self.SPU = StepsPerUnit
35 self.Reset() 35 self.Reset()
36 self.Initialize() 36 self.Initialize()
37   37  
38 def Reset(self): 38 def Reset(self):
39 'Reset the Axis' 39 'Reset the Axis'
40 self.spi.xfer([0xC0]) # reset 40 self.spi.xfer([0xC0]) # reset
41   41  
42 def Initialize(self): 42 def Initialize(self):
43 'set default parameters for H-bridge ' 43 'set default parameters for H-bridge '
44 # self.spi.xfer( 0x14) # Stall Treshold setup 44 # self.spi.xfer( 0x14) # Stall Treshold setup
45 # self.spi.xfer( 0xFF) 45 # self.spi.xfer( 0xFF)
46 # self.spi.xfer( 0x13) # Over Current Treshold setup 46 # self.spi.xfer( 0x13) # Over Current Treshold setup
47 # self.spi.xfer( 0xFF) 47 # self.spi.xfer( 0xFF)
48 self.spi.xfer([0x15]) # Full Step speed 48 self.spi.xfer([0x15]) # Full Step speed
49 self.spi.xfer([0xFF]) 49 self.spi.xfer([0xFF])
50 self.spi.xfer([0xFF]) 50 self.spi.xfer([0xFF])
51 self.spi.xfer([0x05]) # ACC 51 self.spi.xfer([0x05]) # ACC
52 self.spi.xfer([0x00]) 52 self.spi.xfer([0x00])
53 self.spi.xfer([0x10]) 53 self.spi.xfer([0x10])
54 self.spi.xfer([0x06]) # DEC 54 self.spi.xfer([0x06]) # DEC
55 self.spi.xfer([0x00]) 55 self.spi.xfer([0x00])
56 self.spi.xfer([0x10]) 56 self.spi.xfer([0x10])
57 self.spi.xfer([0x0A]) # KVAL_RUN 57 self.spi.xfer([0x0A]) # KVAL_RUN
58 self.spi.xfer([0x20]) 58 self.spi.xfer([0x20])
59 self.spi.xfer([0x0B]) # KVAL_ACC 59 self.spi.xfer([0x0B]) # KVAL_ACC
60 self.spi.xfer([0x20]) 60 self.spi.xfer([0x20])
61 self.spi.xfer([0x0C]) # KVAL_DEC 61 self.spi.xfer([0x0C]) # KVAL_DEC
62 self.spi.xfer([0x20]) 62 self.spi.xfer([0x20])
63 self.spi.xfer([0x18]) # CONFIG 63 self.spi.xfer([0x18]) # CONFIG
64 self.spi.xfer([0b00111000]) 64 self.spi.xfer([0b00111010])
65 self.spi.xfer([0b00000110]) 65 self.spi.xfer([0b10001000])
66 66
67 def MaxSpeed(self, speed): 67 def MaxSpeed(self, speed):
68 'Setup of maximum speed in steps/s' 68 'Setup of maximum speed in steps/s'
69 speed_value = int(speed / 15.25) 69 speed_value = int(speed / 15.25)
70 if (speed_value == 0): 70 if (speed_value == 0):
71 speed_value = 1 71 speed_value = 1
72 print hex(speed_value) 72 print hex(speed_value)
73   73  
74 data = [(speed_value >> i & 0xff) for i in (16,8,0)] 74 data = [(speed_value >> i & 0xff) for i in (16,8,0)]
75 self.spi.xfer([data[0]]) # Max Speed setup 75 self.spi.xfer([data[0]]) # Max Speed setup
76 self.spi.xfer([data[1]]) 76 self.spi.xfer([data[1]])
77 self.spi.xfer([data[2]]) 77 self.spi.xfer([data[2]])
78 return (speed_value * 15.25) 78 return (speed_value * 15.25)
79   79  
80 def ReleaseSW(self): 80 def ReleaseSW(self):
81 ' Go away from Limit Switch ' 81 ' Go away from Limit Switch '
82 while self.ReadStatusBit(2) == 1: # is Limit Switch ON ? 82 while self.ReadStatusBit(2) == 1: # is Limit Switch ON ?
83 self.spi.xfer([0x92 | (~self.Dir & 1)]) # release SW 83 self.spi.xfer([0x92 | (~self.Dir & 1)]) # release SW
84 while self.GetStatus()['BUSY']: 84 while self.GetStatus()['BUSY']:
85 pass 85 pass
86 self.MoveWait(10) # move 10 units away 86 self.MoveWait(10) # move 10 units away
87 87
88 def GoZero(self, speed): 88 def GoZero(self, speed):
89 ' Go to Zero position ' 89 ' Go to Zero position '
90 self.ReleaseSW() 90 self.ReleaseSW()
91 self.spi.xfer([0x82 | (self.Dir & 1)]) # Go to Zero 91 self.spi.xfer([0x82 | (self.Dir & 1)]) # Go to Zero
92 self.spi.xfer([0x00]) 92 self.spi.xfer([0x00])
93 self.spi.xfer([speed]) 93 self.spi.xfer([speed])
94 while self.GetStatus()['BUSY']: 94 while self.GetStatus()['BUSY']:
95 pass 95 pass
96 time.sleep(0.3) 96 time.sleep(0.3)
97 self.ReleaseSW() 97 self.ReleaseSW()
98   98  
99 def GetStatus(self): 99 def GetStatus(self):
100 #self.spi.xfer([0b11010000]) # Get status command from datasheet - does not work for uknown rasons 100 #self.spi.xfer([0b11010000]) # Get status command from datasheet - does not work for uknown rasons
101 self.spi.xfer([0x39]) # Gotparam command on status register 101 self.spi.xfer([0x39]) # Gotparam command on status register
102 data = self.spi.readbytes(1) 102 data = self.spi.readbytes(1)
103 data = data + self.spi.readbytes(1) 103 data = data + self.spi.readbytes(1)
104   104  
105 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 105 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
106 ('STEP_LOSS_B',data[0] & 0x40 == 0x40), 106 ('STEP_LOSS_B',data[0] & 0x40 == 0x40),
107 ('STEP_LOSS_A',data[0] & 0x20 == 0x20), 107 ('STEP_LOSS_A',data[0] & 0x20 == 0x20),
108 ('OCD',data[0] & 0x10 == 0x10), 108 ('OCD',data[0] & 0x10 == 0x10),
109 ('TH_SD',data[0] & 0x08 == 0x08), 109 ('TH_SD',data[0] & 0x08 == 0x08),
110 ('TH_WRN',data[0] & 0x04 == 0x04), 110 ('TH_WRN',data[0] & 0x04 == 0x04),
111 ('UVLO',data[0] & 0x02 == 0x02), 111 ('UVLO',data[0] & 0x02 == 0x02),
112 ('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. 112 ('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.
113 ('NOTPERF_CMD',data[1] & 0x80 == 0x80), 113 ('NOTPERF_CMD',data[1] & 0x80 == 0x80),
114 ('MOT_STATUS',data[1] & 0x60), 114 ('MOT_STATUS',data[1] & 0x60),
115 ('DIR',data[1] & 0x10 == 0x10), 115 ('DIR',data[1] & 0x10 == 0x10),
116 ('SW_EVN',data[1] & 0x08 == 0x08), 116 ('SW_EVN',data[1] & 0x08 == 0x08),
117 ('SW_F',data[1] & 0x04 == 0x04), #The SW_F flag reports the SW input status (low for open and high for closed). 117 ('SW_F',data[1] & 0x04 == 0x04), #The SW_F flag reports the SW input status (low for open and high for closed).
118 ('BUSY',data[1] & 0x02 != 0x02), 118 ('BUSY',data[1] & 0x02 != 0x02),
119 ('HIZ',data[1] & 0x01 == 0x01)]) 119 ('HIZ',data[1] & 0x01 == 0x01)])
120 return status 120 return status
121   121  
122 def Move(self, units): 122 def Move(self, units):
123 ' Move some distance units from current position ' 123 ' Move some distance units from current position '
124 steps = units * self.SPU # translate units to steps 124 steps = units * self.SPU # translate units to steps
125 if steps > 0: # look for direction 125 if steps > 0: # look for direction
126 self.spi.xfer([0x40 | (~self.Dir & 1)]) 126 self.spi.xfer([0x40 | (~self.Dir & 1)])
127 else: 127 else:
128 self.spi.xfer([0x40 | (self.Dir & 1)]) 128 self.spi.xfer([0x40 | (self.Dir & 1)])
129 steps = int(abs(steps)) 129 steps = int(abs(steps))
130 self.spi.xfer([(steps >> 16) & 0xFF]) 130 self.spi.xfer([(steps >> 16) & 0xFF])
131 self.spi.xfer([(steps >> 8) & 0xFF]) 131 self.spi.xfer([(steps >> 8) & 0xFF])
132 self.spi.xfer([steps & 0xFF]) 132 self.spi.xfer([steps & 0xFF])
133   133  
134 def Run(self, direction, speed): 134 def Run(self, direction, speed):
135 speed_value = int(speed / 0.015) 135 speed_value = int(speed / 0.015)
136 print speed_value 136 print speed_value
137   137  
138 data = [0b01010000 + direction] 138 data = [0b01010000 + direction]
139 data = data +[(speed_value >> i & 0xff) for i in (16,8,0)] 139 data = data +[(speed_value >> i & 0xff) for i in (16,8,0)]
140 self.spi.xfer([data[0]]) # Max Speed setup 140 self.spi.xfer([data[0]]) # Max Speed setup
141 self.spi.xfer([data[1]]) 141 self.spi.xfer([data[1]])
142 self.spi.xfer([data[2]]) 142 self.spi.xfer([data[2]])
143 self.spi.xfer([data[3]]) 143 self.spi.xfer([data[3]])
144 return (speed_value * 0.015) 144 return (speed_value * 0.015)
145   145  
146 def MoveWait(self, units): 146 def MoveWait(self, units):
147 ' Move some distance units from current position and wait for execution ' 147 ' Move some distance units from current position and wait for execution '
148 self.Move(units) 148 self.Move(units)
149 while self.GetStatus()['BUSY']: 149 while self.GetStatus()['BUSY']:
150 pass 150 pass
151 time.sleep(0.8) 151 time.sleep(0.8)
152   152  
153 def Float(self, hard = False): 153 def Float(self, hard = False):
154 ' switch H-bridge to High impedance state ' 154 ' switch H-bridge to High impedance state '
155 if (hard == False): 155 if (hard == False):
156 self.spi.xfer([0xA0]) 156 self.spi.xfer([0xA0])
157 else: 157 else:
158 self.spi.xfer([0xA8]) 158 self.spi.xfer([0xA8])
159   159  
160   160  
161 # End Class axis -------------------------------------------------- 161 # End Class axis --------------------------------------------------
162   162  
163 print "Clock motor control script started. \r\n" 163 print "Clock motor control script started. \r\n"
164 print "Requested speed is: %f steps/s" % SPEED 164 print "Requested speed is: %f steps/s" % SPEED
165   165  
166 pylirc.init("pylirc", "/home/odroid/conf") 166 pylirc.init("pylirc", "/home/odroid/conf")
167   167  
168 try: 168 try:
169 print "Configuring SPI.." 169 print "Configuring SPI.."
170 spi = spidev.SpiDev() # create a spi object 170 spi = spidev.SpiDev() # create a spi object
171 spi.open(0, 0) # open spi port 0, device (CS) 0 171 spi.open(0, 0) # open spi port 0, device (CS) 0
172 spi.mode = 0b01 172 spi.mode = 0b01
173 spi.lsbfirst = False 173 spi.lsbfirst = False
174 spi.bits_per_word = 8 174 spi.bits_per_word = 8
175 spi.cshigh = False 175 spi.cshigh = False
176 spi.max_speed_hz = 100000 176 spi.max_speed_hz = 100000
177 #spi.SPI_config(spi.I2CSPI_MSB_FIRST| spi.I2CSPI_MODE_CLK_IDLE_HIGH_DATA_EDGE_TRAILING| spi.I2CSPI_CLK_461kHz) 177 #spi.SPI_config(spi.I2CSPI_MSB_FIRST| spi.I2CSPI_MODE_CLK_IDLE_HIGH_DATA_EDGE_TRAILING| spi.I2CSPI_CLK_461kHz)
178 time.sleep(1) 178 time.sleep(1)
179   179  
180 print "Configuring stepper motor.." 180 print "Configuring stepper motor.."
181 X = axis(spi, 0, 1) # set Number of Steps per axis Unit and set Direction of Rotation 181 X = axis(spi, 0, 1) # set Number of Steps per axis Unit and set Direction of Rotation
182 maximum_speed = X.MaxSpeed(100.0) 182 maximum_speed = X.MaxSpeed(100.0)
183 X.GetStatus() 183 X.GetStatus()
184   184  
185 print "Motor speed limit is: %f steps/s" % maximum_speed 185 print "Motor speed limit is: %f steps/s" % maximum_speed
186 running = False 186 running = False
187   187  
188 print "Waiting for IR command.." 188 print "Waiting for IR command.."
189 while True: # set maximal motor speed 189 while True: # set maximal motor speed
190 key = pylirc.nextcode() ## preccessing the IR remote control commands. 190 key = pylirc.nextcode() ## preccessing the IR remote control commands.
191   191  
192 if key == ['start']: 192 if key == ['start']:
193 running = True 193 running = True
194 direction = True 194 direction = True
195 requested_speed = SPEED 195 requested_speed = SPEED
196   196  
197 if key == ['faster']: 197 if key == ['faster']:
198 running = True 198 running = True
199 direction = True 199 direction = True
200 requested_speed = SPEED * 1.2 # runnig the motor at 120% of the base motor speed 200 requested_speed = SPEED * 1.2 # runnig the motor at 120% of the base motor speed
201   201  
202 if key == ['slower']: 202 if key == ['slower']:
203 running = True 203 running = True
204 direction = False 204 direction = False
205 requested_speed = SPEED * 0.2 205 requested_speed = SPEED * 0.2
206   206  
207 if key == ['stop']: 207 if key == ['stop']:
208 running = False 208 running = False
209   209  
210 time.sleep(0.1) 210 time.sleep(0.1)
211   211  
212 if running == True: 212 if running == True:
213 real_speed = X.Run(direction, requested_speed) 213 real_speed = X.Run(direction, requested_speed)
214 print "Motor running at: %f steps/s" % real_speed 214 print "Motor running at: %f steps/s" % real_speed
215 else: 215 else:
216 X.Reset() 216 X.Reset()
217 X.Initialize() 217 X.Initialize()
218 X.Float(hard=False) # release power 218 X.Float(hard=False) # release power
219 print "Stopping the motor." 219 print "Stopping the motor."
220   220  
221 except KeyboardInterrupt: 221 except KeyboardInterrupt:
222 print "stop" 222 print "stop"
223 X.Float(hard=False) # release power 223 X.Float(hard=False) # release power
224 sys.exit(0) 224 sys.exit(0)
225   225  
226 except Exception, e: 226 except Exception, e:
227 X.Float(hard=False) # release power 227 X.Float(hard=False) # release power
228 print >> sys.stderr, "Exception: %s" % str(e) 228 print >> sys.stderr, "Exception: %s" % str(e)
229 sys.exit(1) 229 sys.exit(1)