Rev 4478 Rev 4575
Line 5... Line 5...
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 from pymlab import config 15 import spidev # SPI binding
16   -  
17   -  
-   16 import pylirc # infrared receiver binding
18   17  
19 #### Script Arguments ############################################### 18 #### Script Arguments ###############################################
20   19  
21 if len(sys.argv) < 2: 20 if len(sys.argv) == 2:
-   21 SPEED = eval(sys.argv[1])
-   22  
-   23 else:
22 sys.stderr.write("Invalid number of arguments.\n") 24 sys.stderr.write("Invalid number of arguments.\n")
23 sys.stderr.write("Usage: %s PORT ADDRESS SPEED MOVE_DISTANCE\n" % (sys.argv[0], )) 25 sys.stderr.write("Usage: %s BASE_SPEED (in steps/s)\n" % (sys.argv[0], ))
24 sys.exit(1) 26 sys.exit(1)
25   27  
26 elif len(sys.argv) == 2: -  
27 PORT = eval(sys.argv[1]) -  
28 SPEED = 5 -  
29 DISTANCE = 50 -  
30   -  
31 elif len(sys.argv) == 3: -  
32 SPEED = eval(sys.argv[2]) -  
33 DISTANCE = 100 -  
34   -  
35 elif len(sys.argv) == 4: -  
36 SPEED = eval(sys.argv[2]) -  
37 DISTANCE = eval(sys.argv[3]) -  
38   -  
39 else: -  
40 PORT = 0 -  
41 SPEED = 10 -  
42 DISTANCE = 50 -  
43   -  
44   28  
45 class axis: 29 class axis:
46 def __init__(self, SPI_CS, Direction, StepsPerUnit): 30 def __init__(self, SPI_handler, Direction, StepsPerUnit):
47 ' One axis of robot ' 31 ' One axis of robot '
48 self.CS = SPI_CS 32 self.spi = SPI_handler
49 self.Dir = Direction 33 self.Dir = Direction
50 self.SPU = StepsPerUnit 34 self.SPU = StepsPerUnit
51 self.Reset() 35 self.Reset()
-   36 self.Initialize()
52   37  
53 def Reset(self): 38 def Reset(self):
54 ' Reset Axis and set default parameters for H-bridge ' 39 'Reset the Axis'
55 spi.SPI_write_byte(self.CS, 0xC0) # reset 40 self.spi.xfer([0xC0]) # reset
-   41  
-   42 def Initialize(self):
-   43 'set default parameters for H-bridge '
56 # spi.SPI_write_byte(self.CS, 0x14) # Stall Treshold setup 44 # self.spi.xfer( 0x14) # Stall Treshold setup
57 # spi.SPI_write_byte(self.CS, 0xFF) 45 # self.spi.xfer( 0xFF)
58 # spi.SPI_write_byte(self.CS, 0x13) # Over Current Treshold setup 46 # self.spi.xfer( 0x13) # Over Current Treshold setup
59 # spi.SPI_write_byte(self.CS, 0xFF) 47 # self.spi.xfer( 0xFF)
60 spi.SPI_write_byte(self.CS, 0x15) # Full Step speed 48 self.spi.xfer([0x15]) # Full Step speed
61 spi.SPI_write_byte(self.CS, 0xFF) 49 self.spi.xfer([0xFF])
62 spi.SPI_write_byte(self.CS, 0xFF) 50 self.spi.xfer([0xFF])
63 spi.SPI_write_byte(self.CS, 0x05) # ACC 51 self.spi.xfer([0x05]) # ACC
64 spi.SPI_write_byte(self.CS, 0x00) 52 self.spi.xfer([0x00])
65 spi.SPI_write_byte(self.CS, 0x10) 53 self.spi.xfer([0x10])
66 spi.SPI_write_byte(self.CS, 0x06) # DEC 54 self.spi.xfer([0x06]) # DEC
67 spi.SPI_write_byte(self.CS, 0x00) 55 self.spi.xfer([0x00])
68 spi.SPI_write_byte(self.CS, 0x10) 56 self.spi.xfer([0x10])
69 spi.SPI_write_byte(self.CS, 0x0A) # KVAL_RUN 57 self.spi.xfer([0x0A]) # KVAL_RUN
70 spi.SPI_write_byte(self.CS, 0xFF) 58 self.spi.xfer([0x50])
71 spi.SPI_write_byte(self.CS, 0x0B) # KVAL_ACC 59 self.spi.xfer([0x0B]) # KVAL_ACC
72 spi.SPI_write_byte(self.CS, 0xFF) 60 self.spi.xfer([0x50])
73 spi.SPI_write_byte(self.CS, 0x0C) # KVAL_DEC 61 self.spi.xfer([0x0C]) # KVAL_DEC
74 spi.SPI_write_byte(self.CS, 0xFF) 62 self.spi.xfer([0x50])
75 spi.SPI_write_byte(self.CS, 0x18) # CONFIG 63 self.spi.xfer([0x18]) # CONFIG
76 spi.SPI_write_byte(self.CS, 0b00111000) 64 self.spi.xfer([0b00111000])
77 spi.SPI_write_byte(self.CS, 0b00000000) 65 self.spi.xfer([0b00000110])
78 66
79 def MaxSpeed(self, speed): 67 def MaxSpeed(self, speed):
80 ' Setup of maximum speed ' 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)
-   73  
-   74 data = [(speed_value >> i & 0xff) for i in (16,8,0)]
81 spi.SPI_write_byte(self.CS, 0x07) # Max Speed setup 75 self.spi.xfer([data[0]]) # Max Speed setup
82 spi.SPI_write_byte(self.CS, 0x00) 76 self.spi.xfer([data[1]])
-   77 self.spi.xfer([data[2]])
83 spi.SPI_write_byte(self.CS, speed) 78 return (speed_value * 15.25)
84   79  
85 def ReleaseSW(self): 80 def ReleaseSW(self):
86 ' Go away from Limit Switch ' 81 ' Go away from Limit Switch '
87 while self.ReadStatusBit(2) == 1: # is Limit Switch ON ? 82 while self.ReadStatusBit(2) == 1: # is Limit Switch ON ?
88 spi.SPI_write_byte(self.CS, 0x92 | (~self.Dir & 1)) # release SW 83 self.spi.xfer([0x92 | (~self.Dir & 1)]) # release SW
89 while self.IsBusy(): 84 while self.GetStatus()['BUSY']:
90 pass 85 pass
91 self.MoveWait(10) # move 10 units away 86 self.MoveWait(10) # move 10 units away
92 87
93 def GoZero(self, speed): 88 def GoZero(self, speed):
94 ' Go to Zero position ' 89 ' Go to Zero position '
95 self.ReleaseSW() 90 self.ReleaseSW()
96   -  
97 spi.SPI_write_byte(self.CS, 0x82 | (self.Dir & 1)) # Go to Zero 91 self.spi.xfer([0x82 | (self.Dir & 1)]) # Go to Zero
98 spi.SPI_write_byte(self.CS, 0x00) 92 self.spi.xfer([0x00])
99 spi.SPI_write_byte(self.CS, speed) 93 self.spi.xfer([speed])
100 while self.IsBusy(): 94 while self.GetStatus()['BUSY']:
101 pass 95 pass
102 time.sleep(0.3) 96 time.sleep(0.3)
103 self.ReleaseSW() 97 self.ReleaseSW()
104   98  
-   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  
105 def Move(self, units): 122 def Move(self, units):
106 ' Move some distance units from current position ' 123 ' Move some distance units from current position '
107 steps = units * self.SPU # translate units to steps 124 steps = units * self.SPU # translate units to steps
108 if steps > 0: # look for direction 125 if steps > 0: # look for direction
109 spi.SPI_write_byte(self.CS, 0x40 | (~self.Dir & 1)) 126 self.spi.xfer([0x40 | (~self.Dir & 1)])
110 else: 127 else:
111 spi.SPI_write_byte(self.CS, 0x40 | (self.Dir & 1)) 128 self.spi.xfer([0x40 | (self.Dir & 1)])
112 steps = int(abs(steps)) 129 steps = int(abs(steps))
113 spi.SPI_write_byte(self.CS, (steps >> 16) & 0xFF) 130 self.spi.xfer([(steps >> 16) & 0xFF])
114 spi.SPI_write_byte(self.CS, (steps >> 8) & 0xFF) 131 self.spi.xfer([(steps >> 8) & 0xFF])
115 spi.SPI_write_byte(self.CS, steps & 0xFF) 132 self.spi.xfer([steps & 0xFF])
-   133  
-   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)
116   145  
117 def MoveWait(self, units): 146 def MoveWait(self, units):
118 ' Move some distance units from current position and wait for execution ' 147 ' Move some distance units from current position and wait for execution '
119 self.Move(units) 148 self.Move(units)
120 while self.IsBusy(): 149 while self.GetStatus()['BUSY']:
121 pass 150 pass
-   151 time.sleep(0.8)
122   152  
123 def Float(self): 153 def Float(self, hard = False):
124 ' switch H-bridge to High impedance state ' 154 ' switch H-bridge to High impedance state '
125 spi.SPI_write_byte(self.CS, 0xA0) -  
126   -  
127 def ReadStatusBit(self, bit): -  
128 ' Report given status bit ' 155 if (hard == False):
129 spi.SPI_write_byte(self.CS, 0x39) # Read from address 0x19 (STATUS) -  
130 spi.SPI_write_byte(self.CS, 0x00) -  
131 data0 = spi.SPI_read_byte() # 1st byte -  
132 spi.SPI_write_byte(self.CS, 0x00) -  
133 data1 = spi.SPI_read_byte() # 2nd byte -  
134 #print hex(data0), hex(data1) 156 self.spi.xfer([0xA0])
135 if bit > 7: # extract requested bit -  
136 OutputBit = (data0 >> (bit - 8)) & 1 -  
137 else: 157 else:
138 OutputBit = (data1 >> bit) & 1 -  
139 return OutputBit 158 self.spi.xfer([0xA8])
140   159  
141 -  
142 def IsBusy(self): -  
143 """ Return True if tehre are motion """ -  
144 if self.ReadStatusBit(1) == 1: -  
145 return False -  
146 else: -  
147 return True -  
148   160  
149 # End Class axis -------------------------------------------------- 161 # End Class axis --------------------------------------------------
150   162  
-   163 print "Clock motor control script started. \r\n"
-   164 print "Requested speed is: %f steps/s" % SPEED
151   165  
152   -  
153 cfg = config.Config( -  
154 i2c = { -  
155 "port": 1, -  
156 }, -  
157   -  
158 bus = [ -  
159 { -  
160 "name":"spi", -  
161 "type":"i2cspi", -  
162 "address": 0x2e, -  
163 }, -  
164 ], -  
165 ) -  
166   -  
167   -  
168 cfg.initialize() -  
169   -  
170 print "Stepper motor control test started. \r\n" -  
171 print "Max motor speed: %d " % SPEED 166 pylirc.init("pylirc", "/home/odroid/conf")
172 print "Distance to run: %d " % DISTANCE -  
173   -  
174 spi = cfg.get_device("spi") -  
175   -  
176 spi.route() -  
177   167  
178 try: 168 try:
179 print "SPI configuration.." 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
180 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)
181 time.sleep(1) 178 time.sleep(1)
182   179  
183 print "Axis inicialization" 180 print "Configuring stepper motor.."
184 X = axis(spi.I2CSPI_SS0, 0, 641) # 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(200.0)
-   183 X.GetStatus()
-   184  
-   185 print "Motor speed limit is: %f steps/s" % maximum_speed
-   186  
-   187 print "Waiting for IR command.."
185 X.MaxSpeed(SPEED) # set maximal motor speed 188 while True: # set maximal motor speed
-   189 key = pylirc.nextcode() ## preccessing the IR remote control commands.
-   190  
-   191 if key == ['start']:
-   192 real_speed = X.Run(1, SPEED)
-   193 print "Motor running at: %f steps/s" % real_speed
-   194  
-   195 if key == ['faster']:
-   196 real_speed = X.Run(1, SPEED * 1.2) # runnig the motor at 120% of the base motor speed
-   197 print "Motor running at: %f steps/s" % real_speed
-   198  
-   199 if key == ['slower']:
-   200 real_speed = X.Run(1, SPEED * 0.8)
-   201 print "Motor running at: %f steps/s" % real_speed
186   202  
-   203 if key == ['stop']:
-   204 X.Float(hard=False) # release power
187 print "Axis is running" 205 print "Stopping the motor."
188   206  
189 for i in range(5): 207 except KeyboardInterrupt:
190 print i -  
191 X.MoveWait(DISTANCE) # move forward and wait for motor stop -  
192 print "Changing direction of rotation.." -  
193 X.MoveWait(-DISTANCE) # move backward and wait for motor stop -  
194 print "Changing direction of rotation.." -  
195   -  
196 X.Float() # release power -  
197   -  
198   -  
199 finally: -  
200 print "stop" 208 print "stop"
-   209 X.Float(hard=False) # release power
-   210 sys.exit(0)
-   211  
-   212 except Exception, e:
-   213 X.Float(hard=False) # release power
-   214 print >> sys.stderr, "Exception: %s" % str(e)
-   215 sys.exit(1)
201 216