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