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 """
89 spi.SPI_write(self.CS, [0x39]) # Read from address 0x19 (STATUS)
90 time.sleep(0.2)
91 spi.SPI_write(self.CS, [0x00])
92 data = spi.SPI_read(1) # 1st byte
93 time.sleep(0.2)
94 spi.SPI_write(self.CS, [0x00])
95 data.extend(spi.SPI_read(1)) # 2nd byte
96 time.sleep(0.2)
97 if bit > 7: # extract requested bit
98 OutputBit = (data[0] >> (bit - 8)) & 1
99 else:
100 OutputBit = (data[1] >> bit) & 1
101 return OutputBit
102  
103 def IsBusy(self):
104 """ Return True if tehre are motion """
105 if self.ReadStatusBit(1) == 1:
106 return False
107 else:
108 return True
109  
110  
111 cfg = config.Config(
112 i2c = {
113 "port": 8,
114 },
115  
116 bus = [
117 { "name":"spi", "type":"i2cspi"},
118 ],
119 )
120  
121 cfg.initialize()
122  
123 print "Irradiation unit. \r\n"
124  
125 spi = cfg.get_device("spi")
126  
127  
128  
129 try:
130  
131 while True:
132 print "SPI configuration.."
133 spi.SPI_config(spi.I2CSPI_MSB_FIRST| spi.I2CSPI_MODE_CLK_IDLE_HIGH_DATA_EDGE_TRAILING| spi.I2CSPI_CLK_461kHz)
134  
135 print "Robot inicialization"
136 X = axis(spi.I2CSPI_SS1, 0)
137 Y = axis(spi.I2CSPI_SS0, 1)
138 Z = axis(spi.I2CSPI_SS2, 1)
139 X.MaxSpeed(60)
140 Y.MaxSpeed(60)
141 Z.MaxSpeed(38)
142  
143 Z.GoZero(100)
144 Z.Move(100000)
145 X.GoZero(20)
146 Y.GoZero(20)
147  
148 time.sleep(1)
149  
150 X.Move(50*XSMM)
151 Y.Move(50*YSMM)
152 Z.MoveWait(50*ZSMM)
153  
154 print "Robot is running"
155  
156 for y in range(5):
157 for x in range(5):
158 Z.MoveWait(8*ZSMM)
159 time.sleep(1)
160 Z.MoveWait(-8*ZSMM)
161 X.MoveWait(8*XSMM)
162 Y.MoveWait(8*YSMM)
163 for x in range(5):
164 Z.MoveWait(8*ZSMM)
165 time.sleep(1)
166 Z.MoveWait(-8*ZSMM)
167 X.MoveWait(-8*XSMM)
168 Y.MoveWait(8*YSMM)
169  
170 Z.MoveWait(-30*ZSMM)
171 X.Float()
172 Y.Float()
173 Z.Float()
174  
175  
176 '''
177 while True:
178 print X.ReadStatusBit(2)
179 time.sleep(1)
180 '''
181  
182 finally:
183 print "stop"