Rev 4248 Rev 4250
Line 6... Line 6...
6   6  
7 import sys 7 import sys
8 import time 8 import time
9 from pymlab import config 9 from pymlab import config
10   10  
11 XSMM = 641 -  
12 YSMM = 642 -  
13 ZSMM = 32256 -  
14   -  
15   -  
16 class axis: 11 class axis:
17 def __init__(self, SPI_CS, Direction): 12 def __init__(self, SPI_CS, Direction, StepsPerUnit):
18 """ One axis of robot """ 13 ' One axis of robot '
19 self.CS = SPI_CS 14 self.CS = SPI_CS
20 self.Dir = Direction 15 self.Dir = Direction
-   16 self.SPU = StepsPerUnit
21 self.Reset() 17 self.Reset()
22   18  
23 def Reset(self): 19 def Reset(self):
24 """ Reset Axis an set default parameters for H-bridge """ 20 ' Reset Axis and set default parameters for H-bridge '
25 spi.SPI_write(self.CS, [0xC0]) # reset 21 spi.SPI_write(self.CS, 0xC0) # reset
26 spi.SPI_write(self.CS, [0x14]) # Stall Treshold setup 22 spi.SPI_write(self.CS, 0x14) # Stall Treshold setup
27 spi.SPI_write(self.CS, [0x7F]) 23 spi.SPI_write(self.CS, 0x7F)
28 spi.SPI_write(self.CS, [0x14]) # Over Current Treshold setup 24 spi.SPI_write(self.CS, 0x14) # Over Current Treshold setup
29 spi.SPI_write(self.CS, [0x0F]) 25 spi.SPI_write(self.CS, 0x0F)
30 #spi.SPI_write(self.CS, [0x15]) # Full Step speed 26 #spi.SPI_write(self.CS, 0x15) # Full Step speed
31 #spi.SPI_write(self.CS, [0x00]) 27 #spi.SPI_write(self.CS, 0x00)
32 #spi.SPI_write(self.CS, [0x30]) 28 #spi.SPI_write(self.CS, 0x30)
33 #spi.SPI_write(self.CS, [0x0A]) # KVAL_RUN 29 #spi.SPI_write(self.CS, 0x0A) # KVAL_RUN
34 #spi.SPI_write(self.CS, [0x50]) 30 #spi.SPI_write(self.CS, 0x50)
35 31
36 def MaxSpeed(self, speed): 32 def MaxSpeed(self, speed):
37 """ Setup of maximum speed """ 33 ' Setup of maximum speed '
38 spi.SPI_write(self.CS, [0x07]) # Max Speed setup 34 spi.SPI_write(self.CS, 0x07) # Max Speed setup
39 spi.SPI_write(self.CS, [0x00]) 35 spi.SPI_write(self.CS, 0x00)
40 spi.SPI_write(self.CS, [speed]) 36 spi.SPI_write(self.CS, speed)
41   37  
42 def ReleaseSW(self): 38 def ReleaseSW(self):
43 """ Go away from Limit Switch """ 39 ' Go away from Limit Switch '
44 while self.ReadStatusBit(2) == 1: # is Limit Switch ON ? 40 while self.ReadStatusBit(2) == 1: # is Limit Switch ON ?
45 spi.SPI_write(self.CS, [0x92 | (~self.Dir & 1)]) # release SW 41 spi.SPI_write(self.CS, 0x92 | (~self.Dir & 1)) # release SW
46 while self.IsBusy(): 42 while self.IsBusy():
47 pass 43 pass
-   44 self.MoveWait(10) # move 10 units awey
-   45 '''
48 spi.SPI_write(self.CS, [0x40 | (~self.Dir & 1)]) # move 0x2000 steps away 46 spi.SPI_write(self.CS, 0x40 | (~self.Dir & 1)) # move 0x2000 steps away
49 spi.SPI_write(self.CS, [0x00]) 47 spi.SPI_write(self.CS, 0x00)
50 spi.SPI_write(self.CS, [0x20]) 48 spi.SPI_write(self.CS, 0x20)
51 spi.SPI_write(self.CS, [0x00]) 49 spi.SPI_write(self.CS, 0x00)
52 while self.IsBusy(): 50 while self.IsBusy():
53 pass 51 pass
-   52 '''
54 53
55 def GoZero(self, speed): 54 def GoZero(self, speed):
56 """ Go to Zero position """ 55 ' Go to Zero position '
57 self.ReleaseSW() 56 self.ReleaseSW()
58   57  
59 spi.SPI_write(self.CS, [0x82 | (self.Dir & 1)]) # Go to Zero 58 spi.SPI_write(self.CS, 0x82 | (self.Dir & 1)) # Go to Zero
60 spi.SPI_write(self.CS, [0x00]) 59 spi.SPI_write(self.CS, 0x00)
61 spi.SPI_write(self.CS, [speed]) 60 spi.SPI_write(self.CS, speed)
62 while self.IsBusy(): 61 while self.IsBusy():
63 pass 62 pass
64 time.sleep(0.3) 63 time.sleep(0.3)
65 self.ReleaseSW() 64 self.ReleaseSW()
66   65  
67 def Move(self, steps): 66 def Move(self, units):
68 """ Move some steps from current position """ 67 ' Move some distance units from current position '
-   68 steps = units * self.SPU # translate units to steps
69 if steps > 0: # look for direction 69 if steps > 0: # look for direction
70 spi.SPI_write(self.CS, [0x40 | (~self.Dir & 1)]) 70 spi.SPI_write(self.CS, 0x40 | (~self.Dir & 1))
71 else: 71 else:
72 spi.SPI_write(self.CS, [0x40 | (self.Dir & 1)]) 72 spi.SPI_write(self.CS, 0x40 | (self.Dir & 1))
73 steps = int(abs(steps)) 73 steps = int(abs(steps))
74 spi.SPI_write(self.CS, [(steps >> 16) & 0xFF]) 74 spi.SPI_write(self.CS, (steps >> 16) & 0xFF)
75 spi.SPI_write(self.CS, [(steps >> 8) & 0xFF]) 75 spi.SPI_write(self.CS, (steps >> 8) & 0xFF)
76 spi.SPI_write(self.CS, [steps & 0xFF]) 76 spi.SPI_write(self.CS, steps & 0xFF)
77   77  
78 def MoveWait(self, steps): 78 def MoveWait(self, units):
-   79 ' Move some distance units from current position and wait for execution '
79 self.Move(steps) 80 self.Move(units)
80 while self.IsBusy(): 81 while self.IsBusy():
81 pass 82 pass
82   83  
83 def Float(self): 84 def Float(self):
84 """ switch H-bridge to High impedance state """ 85 ' switch H-bridge to High impedance state '
85 spi.SPI_write(self.CS, [0xA0]) 86 spi.SPI_write(self.CS, 0xA0)
86   87  
87 def ReadStatusBit(self, bit): 88 def ReadStatusBit(self, bit):
88 """ Report given status bit """ 89 ' Report given status bit '
89 try: -  
90 spi.SPI_write(self.CS, [0x39]) # Read from address 0x19 (STATUS) 90 spi.SPI_write(self.CS, 0x39) # Read from address 0x19 (STATUS)
91 spi.SPI_write(self.CS, [0x00]) 91 spi.SPI_write(self.CS, 0x00)
92 data = spi.SPI_read(1) # 1st byte 92 data0 = spi.SPI_read() # 1st byte
93 spi.SPI_write(self.CS, [0x00]) 93 spi.SPI_write(self.CS, 0x00)
94 data.extend(spi.SPI_read(1)) # 2nd byte 94 data1 = spi.SPI_read() # 2nd byte
-   95 print hex(data0), hex(data1)
95 if bit > 7: # extract requested bit 96 if bit > 7: # extract requested bit
96 OutputBit = (data[0] >> (bit - 8)) & 1 97 OutputBit = (data0 >> (bit - 8)) & 1
97 else: 98 else:
98 OutputBit = (data[1] >> bit) & 1 99 OutputBit = (data1 >> bit) & 1
99 return OutputBit 100 return OutputBit
100 except IOError(): ### TODO -  
101 spi.SPI_write(self.CS, [0x39]) # Read from address 0x19 (STATUS) -  
102 spi.SPI_write(self.CS, [0x00]) -  
103 data = spi.SPI_read(1) # 1st byte -  
104 spi.SPI_write(self.CS, [0x00]) -  
105 data.extend(spi.SPI_read(1)) # 2nd byte -  
106 if bit > 7: # extract requested bit -  
107 OutputBit = (data[0] >> (bit - 8)) & 1 -  
108 else: -  
109 OutputBit = (data[1] >> bit) & 1 -  
110 return OutputBit -  
111 finally: -  
112 pass -  
113   101  
114 102
115 def IsBusy(self): 103 def IsBusy(self):
116 """ Return True if tehre are motion """ 104 """ Return True if tehre are motion """
117 if self.ReadStatusBit(1) == 1: 105 if self.ReadStatusBit(1) == 1:
Line 143... Line 131...
143 while True: 131 while True:
144 print "SPI configuration.." 132 print "SPI configuration.."
145 spi.SPI_config(spi.I2CSPI_MSB_FIRST| spi.I2CSPI_MODE_CLK_IDLE_HIGH_DATA_EDGE_TRAILING| spi.I2CSPI_CLK_461kHz) 133 spi.SPI_config(spi.I2CSPI_MSB_FIRST| spi.I2CSPI_MODE_CLK_IDLE_HIGH_DATA_EDGE_TRAILING| spi.I2CSPI_CLK_461kHz)
146   134  
147 print "Robot inicialization" 135 print "Robot inicialization"
148 X = axis(spi.I2CSPI_SS1, 0) 136 X = axis(spi.I2CSPI_SS1, 0, 641)
149 Y = axis(spi.I2CSPI_SS0, 1) 137 Y = axis(spi.I2CSPI_SS0, 1, 642)
150 Z = axis(spi.I2CSPI_SS2, 1) 138 Z = axis(spi.I2CSPI_SS2, 1, 32256)
151 X.MaxSpeed(60) 139 X.MaxSpeed(60)
152 Y.MaxSpeed(60) 140 Y.MaxSpeed(60)
153 Z.MaxSpeed(38) 141 Z.MaxSpeed(38)
154 142
155 Z.GoZero(100) 143 Z.GoZero(100)
156 Z.Move(100000) 144 #Y.GoZero(20)
157 X.GoZero(20) 145 X.GoZero(20)
158 Y.GoZero(20) -  
159   146  
160 time.sleep(1) 147 time.sleep(1)
161   148  
162 X.Move(30*XSMM) 149 X.Move(30)
163 Y.Move(50*YSMM) 150 Y.Move(50)
164 Z.MoveWait(58*ZSMM) 151 Z.MoveWait(58)
165 X.Move(50*XSMM) 152 X.Move(50)
-   153
166 154
167 print "Robot is running" 155 print "Robot is running"
168   156  
169 for y in range(5): 157 for y in range(2):
170 for x in range(5): 158 for x in range(5):
171 Z.MoveWait(5*ZSMM) 159 Z.MoveWait(5)
172 time.sleep(1) 160 time.sleep(1)
173 Z.MoveWait(-5*ZSMM) 161 Z.MoveWait(-5)
174 if x < 4: 162 if x < 4:
175 X.MoveWait(8*XSMM) 163 X.MoveWait(8)
176 Y.MoveWait(8*YSMM) 164 Y.MoveWait(8)
177 for x in range(5): 165 for x in range(5):
178 Z.MoveWait(5*ZSMM) 166 Z.MoveWait(5)
179 time.sleep(1) 167 time.sleep(1)
180 Z.MoveWait(-5*ZSMM) 168 Z.MoveWait(-5)
181 if x < 4: 169 if x < 4:
182 X.MoveWait(-8*XSMM) 170 X.MoveWait(-8)
183 Y.MoveWait(8*YSMM) 171 Y.MoveWait(8)
184   172  
185 X.MoveWait(-20*XSMM) 173 X.Move(-50)
186 Z.MoveWait(-30*ZSMM) 174 Z.MoveWait(-30)
187 X.Float() 175 X.Float()
188 Y.Float() 176 Y.Float()
189 Z.Float() 177 Z.Float()
190 178
191   179