Rev Author Line No. Line
4328 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  
138 while True:
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)
141  
142 print "Robot inicialization"
143 X = axis(spi.I2CSPI_SS1, 0, 641)
144 Y = axis(spi.I2CSPI_SS0, 1, 642)
145 Z = axis(spi.I2CSPI_SS2, 1, 32256)
146  
147 #X.MaxSpeed(50)
148 #Y.MaxSpeed(50)
149 #Z.MaxSpeed(30)
150 X.MaxSpeed(35)
151 Y.MaxSpeed(35)
152 Z.MaxSpeed(20)
153  
154 Z.GoZero(100)
155 Y.GoZero(20)
156 X.GoZero(20)
157  
158 time.sleep(1)
159  
160 X.Move(16)
161 Y.Move(150)
162 Z.MoveWait(39)
163 time.sleep(1)
164 Z.MoveWait(-5)
165  
166  
167 print "Robot is running"
168  
169 xcorner = 72
170 xsteps = 9
171 ysteps = 6 # *2 + 1 line
172 space = 4
173 grid = 8
4346 kakl 174 delay = 120
4328 kakl 175  
176 #Y.Move(-4)
177 X.MoveWait(xcorner)
178  
179 for y in range(ysteps):
180 for x in range(xsteps):
4346 kakl 181 print x, y
4328 kakl 182 Z.MoveWait(space)
183 time.sleep(delay)
184 Z.MoveWait(-space)
185 if x < (xsteps - 1):
186 X.MoveWait(grid)
187 Y.MoveWait(-grid)
4346 kakl 188 #!!!!delay = delay - 20
4328 kakl 189 for x in range(xsteps):
4346 kakl 190 print x, y
4328 kakl 191 Z.MoveWait(space)
192 time.sleep(delay)
193 Z.MoveWait(-space)
194 if x < (xsteps - 1):
195 X.MoveWait(-grid)
196 Y.MoveWait(-grid)
4346 kakl 197 delay = delay - 20
4328 kakl 198  
4346 kakl 199 delay = 20 #!!!!!!!!!!!!!!
4328 kakl 200 for x in range(xsteps):
4346 kakl 201 print x, y
4328 kakl 202 Z.MoveWait(space)
203 time.sleep(delay)
204 Z.MoveWait(-space)
205 if x < (xsteps - 1):
206 X.MoveWait(grid)
207 Y.MoveWait(-20)
208  
209 X.MoveWait(-(xcorner+(xsteps-1)*grid))
210 #Z.MoveWait(-10)
211 Y.MoveWait(ysteps*grid*2+20)
212 X.Float()
213 Y.Float()
214 Z.Float()
215  
216  
217 while True:
218 print X.ReadStatusBit(2)
219 time.sleep(1)
220  
221 finally:
222 print "stop"