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