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