Rev 4247 Rev 4248
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"