Rev 4328 Rev 4346
1 #!/usr/bin/python 1 #!/usr/bin/python
2   2  
3 #uncomment for debbug purposes 3 #uncomment for debbug purposes
4 #import logging 4 #import logging
5 #logging.basicConfig(level=logging.DEBUG) 5 #logging.basicConfig(level=logging.DEBUG)
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 class axis: 11 class axis:
12 def __init__(self, SPI_CS, Direction, StepsPerUnit): 12 def __init__(self, SPI_CS, Direction, StepsPerUnit):
13 ' One axis of robot ' 13 ' One axis of robot '
14 self.CS = SPI_CS 14 self.CS = SPI_CS
15 self.Dir = Direction 15 self.Dir = Direction
16 self.SPU = StepsPerUnit 16 self.SPU = StepsPerUnit
17 self.Reset() 17 self.Reset()
18   18  
19 def Reset(self): 19 def Reset(self):
20 ' Reset Axis and set default parameters for H-bridge ' 20 ' Reset Axis and set default parameters for H-bridge '
21 spi.SPI_write_byte(self.CS, 0xC0) # reset 21 spi.SPI_write_byte(self.CS, 0xC0) # reset
22 spi.SPI_write_byte(self.CS, 0x14) # Stall Treshold setup 22 spi.SPI_write_byte(self.CS, 0x14) # Stall Treshold setup
23 spi.SPI_write_byte(self.CS, 0x7F) 23 spi.SPI_write_byte(self.CS, 0x7F)
24 spi.SPI_write_byte(self.CS, 0x14) # Over Current Treshold setup 24 spi.SPI_write_byte(self.CS, 0x14) # Over Current Treshold setup
25 spi.SPI_write_byte(self.CS, 0x0F) 25 spi.SPI_write_byte(self.CS, 0x0F)
26 #spi.SPI_write_byte(self.CS, 0x15) # Full Step speed 26 #spi.SPI_write_byte(self.CS, 0x15) # Full Step speed
27 #spi.SPI_write_byte(self.CS, 0x00) 27 #spi.SPI_write_byte(self.CS, 0x00)
28 #spi.SPI_write_byte(self.CS, 0x30) 28 #spi.SPI_write_byte(self.CS, 0x30)
29 #spi.SPI_write_byte(self.CS, 0x0A) # KVAL_RUN 29 #spi.SPI_write_byte(self.CS, 0x0A) # KVAL_RUN
30 #spi.SPI_write_byte(self.CS, 0x50) 30 #spi.SPI_write_byte(self.CS, 0x50)
31 31
32 def MaxSpeed(self, speed): 32 def MaxSpeed(self, speed):
33 ' Setup of maximum speed ' 33 ' Setup of maximum speed '
34 spi.SPI_write_byte(self.CS, 0x07) # Max Speed setup 34 spi.SPI_write_byte(self.CS, 0x07) # Max Speed setup
35 spi.SPI_write_byte(self.CS, 0x00) 35 spi.SPI_write_byte(self.CS, 0x00)
36 spi.SPI_write_byte(self.CS, speed) 36 spi.SPI_write_byte(self.CS, speed)
37   37  
38 def ReleaseSW(self): 38 def ReleaseSW(self):
39 ' Go away from Limit Switch ' 39 ' Go away from Limit Switch '
40 while self.ReadStatusBit(2) == 1: # is Limit Switch ON ? 40 while self.ReadStatusBit(2) == 1: # is Limit Switch ON ?
41 spi.SPI_write_byte(self.CS, 0x92 | (~self.Dir & 1)) # release SW 41 spi.SPI_write_byte(self.CS, 0x92 | (~self.Dir & 1)) # release SW
42 while self.IsBusy(): 42 while self.IsBusy():
43 pass 43 pass
44 self.MoveWait(10) # move 10 units away 44 self.MoveWait(10) # move 10 units away
45 45
46 def GoZero(self, speed): 46 def GoZero(self, speed):
47 ' Go to Zero position ' 47 ' Go to Zero position '
48 self.ReleaseSW() 48 self.ReleaseSW()
49   49  
50 spi.SPI_write_byte(self.CS, 0x82 | (self.Dir & 1)) # Go to Zero 50 spi.SPI_write_byte(self.CS, 0x82 | (self.Dir & 1)) # Go to Zero
51 spi.SPI_write_byte(self.CS, 0x00) 51 spi.SPI_write_byte(self.CS, 0x00)
52 spi.SPI_write_byte(self.CS, speed) 52 spi.SPI_write_byte(self.CS, speed)
53 while self.IsBusy(): 53 while self.IsBusy():
54 pass 54 pass
55 time.sleep(0.3) 55 time.sleep(0.3)
56 self.ReleaseSW() 56 self.ReleaseSW()
57   57  
58 def Move(self, units): 58 def Move(self, units):
59 ' Move some distance units from current position ' 59 ' Move some distance units from current position '
60 steps = units * self.SPU # translate units to steps 60 steps = units * self.SPU # translate units to steps
61 if steps > 0: # look for direction 61 if steps > 0: # look for direction
62 spi.SPI_write_byte(self.CS, 0x40 | (~self.Dir & 1)) 62 spi.SPI_write_byte(self.CS, 0x40 | (~self.Dir & 1))
63 else: 63 else:
64 spi.SPI_write_byte(self.CS, 0x40 | (self.Dir & 1)) 64 spi.SPI_write_byte(self.CS, 0x40 | (self.Dir & 1))
65 steps = int(abs(steps)) 65 steps = int(abs(steps))
66 spi.SPI_write_byte(self.CS, (steps >> 16) & 0xFF) 66 spi.SPI_write_byte(self.CS, (steps >> 16) & 0xFF)
67 spi.SPI_write_byte(self.CS, (steps >> 8) & 0xFF) 67 spi.SPI_write_byte(self.CS, (steps >> 8) & 0xFF)
68 spi.SPI_write_byte(self.CS, steps & 0xFF) 68 spi.SPI_write_byte(self.CS, steps & 0xFF)
69   69  
70 def MoveWait(self, units): 70 def MoveWait(self, units):
71 ' Move some distance units from current position and wait for execution ' 71 ' Move some distance units from current position and wait for execution '
72 self.Move(units) 72 self.Move(units)
73 while self.IsBusy(): 73 while self.IsBusy():
74 pass 74 pass
75   75  
76 def Float(self): 76 def Float(self):
77 ' switch H-bridge to High impedance state ' 77 ' switch H-bridge to High impedance state '
78 spi.SPI_write_byte(self.CS, 0xA0) 78 spi.SPI_write_byte(self.CS, 0xA0)
79   79  
80 def ReadStatusBit(self, bit): 80 def ReadStatusBit(self, bit):
81 ' Report given status bit ' 81 ' Report given status bit '
82 spi.SPI_write_byte(self.CS, 0x39) # Read from address 0x19 (STATUS) 82 spi.SPI_write_byte(self.CS, 0x39) # Read from address 0x19 (STATUS)
83 spi.SPI_write_byte(self.CS, 0x00) 83 spi.SPI_write_byte(self.CS, 0x00)
84 data0 = spi.SPI_read_byte() # 1st byte 84 data0 = spi.SPI_read_byte() # 1st byte
85 spi.SPI_write_byte(self.CS, 0x00) 85 spi.SPI_write_byte(self.CS, 0x00)
86 data1 = spi.SPI_read_byte() # 2nd byte 86 data1 = spi.SPI_read_byte() # 2nd byte
87 #print hex(data0), hex(data1) 87 #print hex(data0), hex(data1)
88 if bit > 7: # extract requested bit 88 if bit > 7: # extract requested bit
89 OutputBit = (data0 >> (bit - 8)) & 1 89 OutputBit = (data0 >> (bit - 8)) & 1
90 else: 90 else:
91 OutputBit = (data1 >> bit) & 1 91 OutputBit = (data1 >> bit) & 1
92 return OutputBit 92 return OutputBit
93   93  
94 94
95 def IsBusy(self): 95 def IsBusy(self):
96 """ Return True if tehre are motion """ 96 """ Return True if tehre are motion """
97 if self.ReadStatusBit(1) == 1: 97 if self.ReadStatusBit(1) == 1:
98 return False 98 return False
99 else: 99 else:
100 return True 100 return True
101   101  
102 # End Class axis -------------------------------------------------- 102 # End Class axis --------------------------------------------------
103   103  
104   104  
105   105  
106   106  
107   107  
108 cfg = config.Config( 108 cfg = config.Config(
109 i2c = { 109 i2c = {
110 "port": 1, 110 "port": 1,
111 }, 111 },
112   112  
113 bus = 113 bus =
114 [ 114 [
115 { 115 {
116 "type": "i2chub", 116 "type": "i2chub",
117 "address": 0x70, 117 "address": 0x70,
118 "children": 118 "children":
119 [ 119 [
120 { "name":"spi", "type":"i2cspi", "channel":7} 120 { "name":"spi", "type":"i2cspi", "channel":7}
121 ], 121 ],
122 }, 122 },
123 ], 123 ],
124 ) 124 )
125   125  
126   126  
127 cfg.initialize() 127 cfg.initialize()
128   128  
129 print "Irradiation unit. \r\n" 129 print "Irradiation unit. \r\n"
130   130  
131 spi = cfg.get_device("spi") 131 spi = cfg.get_device("spi")
132   132  
133 spi.route() 133 spi.route()
134   134  
135   135  
136 try: 136 try:
137   137  
138 while True: 138 while True:
139 print "SPI configuration.." 139 print "SPI configuration.."
140 spi.SPI_config(spi.I2CSPI_MSB_FIRST| spi.I2CSPI_MODE_CLK_IDLE_HIGH_DATA_EDGE_TRAILING| spi.I2CSPI_CLK_461kHz) 140 spi.SPI_config(spi.I2CSPI_MSB_FIRST| spi.I2CSPI_MODE_CLK_IDLE_HIGH_DATA_EDGE_TRAILING| spi.I2CSPI_CLK_461kHz)
141   141  
142 print "Robot inicialization" 142 print "Robot inicialization"
143 X = axis(spi.I2CSPI_SS1, 0, 641) 143 X = axis(spi.I2CSPI_SS1, 0, 641)
144 Y = axis(spi.I2CSPI_SS0, 1, 642) 144 Y = axis(spi.I2CSPI_SS0, 1, 642)
145 Z = axis(spi.I2CSPI_SS2, 1, 32256) 145 Z = axis(spi.I2CSPI_SS2, 1, 32256)
146   146  
147 #X.MaxSpeed(50) 147 #X.MaxSpeed(50)
148 #Y.MaxSpeed(50) 148 #Y.MaxSpeed(50)
149 #Z.MaxSpeed(30) 149 #Z.MaxSpeed(30)
150 X.MaxSpeed(35) 150 X.MaxSpeed(35)
151 Y.MaxSpeed(35) 151 Y.MaxSpeed(35)
152 Z.MaxSpeed(20) 152 Z.MaxSpeed(20)
153 153
154 Z.GoZero(100) 154 Z.GoZero(100)
155 Y.GoZero(20) 155 Y.GoZero(20)
156 X.GoZero(20) 156 X.GoZero(20)
157   157  
158 time.sleep(1) 158 time.sleep(1)
159   159  
160 X.Move(16) 160 X.Move(16)
161 Y.Move(150) 161 Y.Move(150)
162 Z.MoveWait(39) 162 Z.MoveWait(39)
163 time.sleep(1) 163 time.sleep(1)
164 Z.MoveWait(-5) 164 Z.MoveWait(-5)
165   165  
166 166
167 print "Robot is running" 167 print "Robot is running"
168   168  
169 xcorner = 72 169 xcorner = 72
170 xsteps = 9 170 xsteps = 9
171 ysteps = 6 # *2 + 1 line 171 ysteps = 6 # *2 + 1 line
172 space = 4 172 space = 4
173 grid = 8 173 grid = 8
174 delay = 40 174 delay = 120
175   175  
176 #Y.Move(-4) 176 #Y.Move(-4)
177 X.MoveWait(xcorner) 177 X.MoveWait(xcorner)
178   178  
179 for y in range(ysteps): 179 for y in range(ysteps):
180 for x in range(xsteps): 180 for x in range(xsteps):
-   181 print x, y
181 Z.MoveWait(space) 182 Z.MoveWait(space)
182 time.sleep(delay) 183 time.sleep(delay)
183 Z.MoveWait(-space) 184 Z.MoveWait(-space)
184 if x < (xsteps - 1): 185 if x < (xsteps - 1):
185 X.MoveWait(grid) 186 X.MoveWait(grid)
186 Y.MoveWait(-grid) 187 Y.MoveWait(-grid)
187 delay = delay - 2.5 188 #!!!!delay = delay - 20
188 for x in range(xsteps): 189 for x in range(xsteps):
-   190 print x, y
189 Z.MoveWait(space) 191 Z.MoveWait(space)
190 time.sleep(delay) 192 time.sleep(delay)
191 Z.MoveWait(-space) 193 Z.MoveWait(-space)
192 if x < (xsteps - 1): 194 if x < (xsteps - 1):
193 X.MoveWait(-grid) 195 X.MoveWait(-grid)
194 Y.MoveWait(-grid) 196 Y.MoveWait(-grid)
195 delay = delay - 2.5 197 delay = delay - 20
196   198  
-   199 delay = 20 #!!!!!!!!!!!!!!
197 for x in range(xsteps): 200 for x in range(xsteps):
-   201 print x, y
198 Z.MoveWait(space) 202 Z.MoveWait(space)
199 time.sleep(delay) 203 time.sleep(delay)
200 Z.MoveWait(-space) 204 Z.MoveWait(-space)
201 if x < (xsteps - 1): 205 if x < (xsteps - 1):
202 X.MoveWait(grid) 206 X.MoveWait(grid)
203 Y.MoveWait(-20) 207 Y.MoveWait(-20)
204   208  
205 X.MoveWait(-(xcorner+(xsteps-1)*grid)) 209 X.MoveWait(-(xcorner+(xsteps-1)*grid))
206 #Z.MoveWait(-10) 210 #Z.MoveWait(-10)
207 Y.MoveWait(ysteps*grid*2+20) 211 Y.MoveWait(ysteps*grid*2+20)
208 X.Float() 212 X.Float()
209 Y.Float() 213 Y.Float()
210 Z.Float() 214 Z.Float()
211 215
212   216  
213 while True: 217 while True:
214 print X.ReadStatusBit(2) 218 print X.ReadStatusBit(2)
215 time.sleep(1) 219 time.sleep(1)
216   220  
217 finally: 221 finally:
218 print "stop" 222 print "stop"