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 XSMM = 641
12 YSMM = 642
13 ZSMM = 32256
14  
15  
16 class axis:
17 def __init__(self, SPI_CS, Direction):
18 """ One axis of robot """
19 self.CS = SPI_CS
20 self.Dir = Direction
21 self.Reset()
22  
23 def Reset(self):
24 """ Reset Axis an set default parameters for H-bridge """
25 spi.SPI_write(self.CS, [0xC0]) # reset
26 spi.SPI_write(self.CS, [0x14]) # Stall Treshold setup
27 spi.SPI_write(self.CS, [0x7F])
28 spi.SPI_write(self.CS, [0x14]) # Over Current Treshold setup
29 spi.SPI_write(self.CS, [0x0F])
30 #spi.SPI_write(self.CS, [0x15]) # Full Step speed
31 #spi.SPI_write(self.CS, [0x00])
32 #spi.SPI_write(self.CS, [0x30])
33 #spi.SPI_write(self.CS, [0x0A]) # KVAL_RUN
34 #spi.SPI_write(self.CS, [0x50])
35  
36 def MaxSpeed(self, speed):
37 """ Setup of maximum speed """
38 spi.SPI_write(self.CS, [0x07]) # Max Speed setup
39 spi.SPI_write(self.CS, [0x00])
40 spi.SPI_write(self.CS, [speed])
41  
42 def ReleaseSW(self):
43 """ Go away from Limit Switch """
44 while self.ReadStatusBit(2) == 1: # is Limit Switch ON ?
45 spi.SPI_write(self.CS, [0x92 | (~self.Dir & 1)]) # release SW
46 while self.IsBusy():
47 pass
48 spi.SPI_write(self.CS, [0x40 | (~self.Dir & 1)]) # move 0x2000 steps away
49 spi.SPI_write(self.CS, [0x00])
50 spi.SPI_write(self.CS, [0x20])
51 spi.SPI_write(self.CS, [0x00])
52 while self.IsBusy():
53 pass
54  
55 def GoZero(self, speed):
56 """ Go to Zero position """
57 self.ReleaseSW()
58  
59 spi.SPI_write(self.CS, [0x82 | (self.Dir & 1)]) # Go to Zero
60 spi.SPI_write(self.CS, [0x00])
61 spi.SPI_write(self.CS, [speed])
62 while self.IsBusy():
63 pass
64 time.sleep(0.3)
65 self.ReleaseSW()
66  
67 def Move(self, steps):
68 """ Move some steps from current position """
69 if steps > 0: # look for direction
70 spi.SPI_write(self.CS, [0x40 | (~self.Dir & 1)])
71 else:
72 spi.SPI_write(self.CS, [0x40 | (self.Dir & 1)])
73 steps = int(abs(steps))
74 spi.SPI_write(self.CS, [(steps >> 16) & 0xFF])
75 spi.SPI_write(self.CS, [(steps >> 8) & 0xFF])
76 spi.SPI_write(self.CS, [steps & 0xFF])
77  
78 def MoveWait(self, steps):
79 self.Move(steps)
80 while self.IsBusy():
81 pass
82  
83 def Float(self):
84 """ switch H-bridge to High impedance state """
85 spi.SPI_write(self.CS, [0xA0])
86  
87 def ReadStatusBit(self, bit):
88 """ Report given status bit """
4248 kakl 89 try:
90 spi.SPI_write(self.CS, [0x39]) # Read from address 0x19 (STATUS)
91 spi.SPI_write(self.CS, [0x00])
92 data = spi.SPI_read(1) # 1st byte
93 spi.SPI_write(self.CS, [0x00])
94 data.extend(spi.SPI_read(1)) # 2nd byte
95 if bit > 7: # extract requested bit
96 OutputBit = (data[0] >> (bit - 8)) & 1
97 else:
98 OutputBit = (data[1] >> bit) & 1
99 return OutputBit
100 except IOError(): ### TODO
101 spi.SPI_write(self.CS, [0x39]) # Read from address 0x19 (STATUS)
102 spi.SPI_write(self.CS, [0x00])
103 data = spi.SPI_read(1) # 1st byte
104 spi.SPI_write(self.CS, [0x00])
105 data.extend(spi.SPI_read(1)) # 2nd byte
106 if bit > 7: # extract requested bit
107 OutputBit = (data[0] >> (bit - 8)) & 1
108 else:
109 OutputBit = (data[1] >> bit) & 1
110 return OutputBit
111 finally:
112 pass
4247 kakl 113  
4248 kakl 114  
4247 kakl 115 def IsBusy(self):
116 """ Return True if tehre are motion """
117 if self.ReadStatusBit(1) == 1:
118 return False
119 else:
120 return True
121  
122  
123 cfg = config.Config(
124 i2c = {
125 "port": 8,
126 },
127  
128 bus = [
129 { "name":"spi", "type":"i2cspi"},
130 ],
131 )
132  
133 cfg.initialize()
134  
135 print "Irradiation unit. \r\n"
136  
137 spi = cfg.get_device("spi")
138  
139  
140  
141 try:
142  
143 while True:
144 print "SPI configuration.."
145 spi.SPI_config(spi.I2CSPI_MSB_FIRST| spi.I2CSPI_MODE_CLK_IDLE_HIGH_DATA_EDGE_TRAILING| spi.I2CSPI_CLK_461kHz)
146  
147 print "Robot inicialization"
148 X = axis(spi.I2CSPI_SS1, 0)
149 Y = axis(spi.I2CSPI_SS0, 1)
150 Z = axis(spi.I2CSPI_SS2, 1)
151 X.MaxSpeed(60)
152 Y.MaxSpeed(60)
153 Z.MaxSpeed(38)
154  
155 Z.GoZero(100)
156 Z.Move(100000)
157 X.GoZero(20)
158 Y.GoZero(20)
159  
160 time.sleep(1)
161  
4248 kakl 162 X.Move(30*XSMM)
163 Y.Move(50*YSMM)
164 Z.MoveWait(58*ZSMM)
4247 kakl 165 X.Move(50*XSMM)
166  
167 print "Robot is running"
168  
169 for y in range(5):
170 for x in range(5):
4248 kakl 171 Z.MoveWait(5*ZSMM)
4247 kakl 172 time.sleep(1)
4248 kakl 173 Z.MoveWait(-5*ZSMM)
174 if x < 4:
175 X.MoveWait(8*XSMM)
4247 kakl 176 Y.MoveWait(8*YSMM)
177 for x in range(5):
4248 kakl 178 Z.MoveWait(5*ZSMM)
4247 kakl 179 time.sleep(1)
4248 kakl 180 Z.MoveWait(-5*ZSMM)
181 if x < 4:
182 X.MoveWait(-8*XSMM)
4247 kakl 183 Y.MoveWait(8*YSMM)
184  
4248 kakl 185 X.MoveWait(-20*XSMM)
4247 kakl 186 Z.MoveWait(-30*ZSMM)
187 X.Float()
188 Y.Float()
189 Z.Float()
190  
191  
192 '''
193 while True:
194 print X.ReadStatusBit(2)
195 time.sleep(1)
196 '''
197  
198 finally:
199 print "stop"