Rev Author Line No. Line
4247 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:
4250 kakl 12 def __init__(self, SPI_CS, Direction, StepsPerUnit):
13 ' One axis of robot '
4247 kakl 14 self.CS = SPI_CS
15 self.Dir = Direction
4250 kakl 16 self.SPU = StepsPerUnit
4247 kakl 17 self.Reset()
18  
19 def Reset(self):
4250 kakl 20 ' Reset Axis and set default parameters for H-bridge '
4265 kakl 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)
4247 kakl 31  
32 def MaxSpeed(self, speed):
4250 kakl 33 ' Setup of maximum speed '
4265 kakl 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)
4247 kakl 37  
38 def ReleaseSW(self):
4250 kakl 39 ' Go away from Limit Switch '
4247 kakl 40 while self.ReadStatusBit(2) == 1: # is Limit Switch ON ?
4265 kakl 41 spi.SPI_write_byte(self.CS, 0x92 | (~self.Dir & 1)) # release SW
4247 kakl 42 while self.IsBusy():
43 pass
4265 kakl 44 self.MoveWait(10) # move 10 units away
4247 kakl 45  
46 def GoZero(self, speed):
4250 kakl 47 ' Go to Zero position '
4247 kakl 48 self.ReleaseSW()
49  
4265 kakl 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)
4247 kakl 53 while self.IsBusy():
54 pass
55 time.sleep(0.3)
56 self.ReleaseSW()
57  
4250 kakl 58 def Move(self, units):
59 ' Move some distance units from current position '
60 steps = units * self.SPU # translate units to steps
4247 kakl 61 if steps > 0: # look for direction
4265 kakl 62 spi.SPI_write_byte(self.CS, 0x40 | (~self.Dir & 1))
4247 kakl 63 else:
4265 kakl 64 spi.SPI_write_byte(self.CS, 0x40 | (self.Dir & 1))
4247 kakl 65 steps = int(abs(steps))
4265 kakl 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)
4247 kakl 69  
4250 kakl 70 def MoveWait(self, units):
71 ' Move some distance units from current position and wait for execution '
72 self.Move(units)
4247 kakl 73 while self.IsBusy():
74 pass
75  
76 def Float(self):
4250 kakl 77 ' switch H-bridge to High impedance state '
4265 kakl 78 spi.SPI_write_byte(self.CS, 0xA0)
4247 kakl 79  
80 def ReadStatusBit(self, bit):
4250 kakl 81 ' Report given status bit '
4265 kakl 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)
4250 kakl 88 if bit > 7: # extract requested bit
89 OutputBit = (data0 >> (bit - 8)) & 1
90 else:
91 OutputBit = (data1 >> bit) & 1
92 return OutputBit
4247 kakl 93  
4248 kakl 94  
4247 kakl 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  
4265 kakl 102 # End Class axis --------------------------------------------------
4247 kakl 103  
4265 kakl 104  
105  
106  
107  
4247 kakl 108 cfg = config.Config(
109 i2c = {
4265 kakl 110 "port": 1,
4247 kakl 111 },
112  
4265 kakl 113 bus =
114 [
115 {
116 "type": "i2chub",
117 "address": 0x70,
118 "children":
119 [
120 { "name":"spi", "type":"i2cspi", "channel":7}
121 ],
122 },
4247 kakl 123 ],
124 )
125  
4265 kakl 126  
4247 kakl 127 cfg.initialize()
128  
129 print "Irradiation unit. \r\n"
130  
131 spi = cfg.get_device("spi")
132  
4265 kakl 133 spi.route()
4247 kakl 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"
4250 kakl 143 X = axis(spi.I2CSPI_SS1, 0, 641)
144 Y = axis(spi.I2CSPI_SS0, 1, 642)
145 Z = axis(spi.I2CSPI_SS2, 1, 32256)
4265 kakl 146  
4309 kakl 147 #X.MaxSpeed(50)
148 #Y.MaxSpeed(50)
149 #Z.MaxSpeed(30)
150 X.MaxSpeed(40)
151 Y.MaxSpeed(40)
152 Z.MaxSpeed(20)
4247 kakl 153  
154 Z.GoZero(100)
4265 kakl 155 Y.GoZero(20)
4247 kakl 156 X.GoZero(20)
157  
158 time.sleep(1)
159  
4309 kakl 160 X.Move(15)
161 Y.Move(150)
162 Z.MoveWait(37)
163 time.sleep(1)
164 Z.MoveWait(-5)
4311 kakl 165  
4247 kakl 166 print "Robot is running"
167  
4311 kakl 168 corner = 73
169 xsteps = 9
170 ysteps = 6 # *2 + 1 line
4309 kakl 171 space = 5
172 grid = 8
173  
4311 kakl 174 #Y.Move(-4)
175 X.MoveWait(corner)
176  
4309 kakl 177 for y in range(ysteps):
178 for x in range(xsteps):
179 Z.MoveWait(space)
4247 kakl 180 time.sleep(1)
4309 kakl 181 Z.MoveWait(-space)
182 if x < (xsteps - 1):
183 X.MoveWait(grid)
184 Y.MoveWait(-grid)
185 for x in range(xsteps):
186 Z.MoveWait(space)
4247 kakl 187 time.sleep(1)
4309 kakl 188 Z.MoveWait(-space)
189 if x < (xsteps - 1):
190 X.MoveWait(-grid)
191 Y.MoveWait(-grid)
4247 kakl 192  
4311 kakl 193 for x in range(xsteps):
194 Z.MoveWait(space)
195 time.sleep(1)
196 Z.MoveWait(-space)
197 if x < (xsteps - 1):
198 X.MoveWait(grid)
199 Y.MoveWait(-20)
200  
201 X.MoveWait(-(corner+(xsteps-1)*grid))
4309 kakl 202 #Z.MoveWait(-10)
4311 kakl 203 Y.MoveWait(ysteps*grid*2+20)
4247 kakl 204 X.Float()
205 Y.Float()
206 Z.Float()
4250 kakl 207  
4247 kakl 208  
209 while True:
210 print X.ReadStatusBit(2)
211 time.sleep(1)
212  
213 finally:
214 print "stop"