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