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)
4328 kakl 150 X.MaxSpeed(35)
151 Y.MaxSpeed(35)
4309 kakl 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  
4328 kakl 160 X.Move(17)
4309 kakl 161 Y.Move(150)
4328 kakl 162 Z.MoveWait(39)
4309 kakl 163 time.sleep(1)
164 Z.MoveWait(-5)
4328 kakl 165  
4311 kakl 166  
4247 kakl 167 print "Robot is running"
168  
4328 kakl 169 xcorner = 72
4311 kakl 170 xsteps = 9
171 ysteps = 6 # *2 + 1 line
4328 kakl 172 space = 4
4309 kakl 173 grid = 8
4328 kakl 174 delay = 1
4309 kakl 175  
4311 kakl 176 #Y.Move(-4)
4328 kakl 177 X.MoveWait(xcorner)
4311 kakl 178  
4309 kakl 179 for y in range(ysteps):
180 for x in range(xsteps):
181 Z.MoveWait(space)
4328 kakl 182 time.sleep(delay)
4309 kakl 183 Z.MoveWait(-space)
184 if x < (xsteps - 1):
185 X.MoveWait(grid)
186 Y.MoveWait(-grid)
187 for x in range(xsteps):
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)
4247 kakl 194  
4311 kakl 195 for x in range(xsteps):
196 Z.MoveWait(space)
4328 kakl 197 time.sleep(delay)
4311 kakl 198 Z.MoveWait(-space)
199 if x < (xsteps - 1):
200 X.MoveWait(grid)
201 Y.MoveWait(-20)
202  
4328 kakl 203 X.MoveWait(-(xcorner+(xsteps-1)*grid))
4309 kakl 204 #Z.MoveWait(-10)
4311 kakl 205 Y.MoveWait(ysteps*grid*2+20)
4247 kakl 206 X.Float()
207 Y.Float()
208 Z.Float()
4250 kakl 209  
4247 kakl 210  
211 while True:
212 print X.ReadStatusBit(2)
213 time.sleep(1)
214  
215 finally:
216 print "stop"