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