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