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