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