Rev 4719 Rev 4723
Line 39... Line 39...
39 else: 39 else:
40 PORT = 0 40 PORT = 0
41 SPEED = 20 41 SPEED = 20
42 DISTANCE = 5000 42 DISTANCE = 5000
43   43  
-   44 # Begin of Class Axis --------------------------------------------------
44   45  
45 class axis: 46 class axis:
46 def __init__(self, SPI_CS, Direction, StepsPerUnit, MaxSpeed): 47 def __init__(self, SPI_CS, Direction, StepsPerUnit, MaxSpeed):
47 ' One axis of robot ' 48 ' One axis of robot '
48 self.CS = SPI_CS 49 self.CS = SPI_CS
Line 76... Line 77...
76 self.L6470_CONFIG =0x18 77 self.L6470_CONFIG =0x18
77 self.L6470_STATUS =0x19 78 self.L6470_STATUS =0x19
78   79  
79 self.Reset() 80 self.Reset()
80 self.Initialize() 81 self.Initialize()
-   82 self.MaxSpeed(self.maxspeed)
81   83  
82 def Reset(self): 84 def Reset(self):
83 'Reset the Axis' 85 'Reset the Axis'
84 spi.SPI_write_byte(self.CS, 0xC0) # reset 86 spi.SPI_write_byte(self.CS, 0xC0) # reset
85   87  
Line 150... Line 152...
150 while self.IsBusy(): 152 while self.IsBusy():
151 pass 153 pass
152 time.sleep(0.3) 154 time.sleep(0.3)
153 self.ReleaseSW() 155 self.ReleaseSW()
154   156  
-   157 def GetStatus(self):
-   158 #self.spi.xfer([0b11010000]) # Get status command from datasheet - does not work for uknown rasons
-   159 spi.SPI_write_byte(self.CS, 0x39) # Gotparam command on status register
-   160 spi.SPI_write_byte(self.CS, 0x00)
-   161 data = spi.SPI_read_byte()
-   162 spi.SPI_write_byte(self.CS, 0x00)
-   163 data = data + spi.SPI_read_byte()
-   164  
-   165 status = dict([('SCK_MOD',data[0] & 0x80 == 0x80), #The SCK_MOD bit is an active high flag indicating that the device is working in Step-clock mode. In this case the step-clock signal should be provided through the STCK input pin. The DIR bit indicates the current motor direction
-   166 ('STEP_LOSS_B',data[0] & 0x40 == 0x40),
-   167 ('STEP_LOSS_A',data[0] & 0x20 == 0x20),
-   168 ('OCD',data[0] & 0x10 == 0x10),
-   169 ('TH_SD',data[0] & 0x08 == 0x08),
-   170 ('TH_WRN',data[0] & 0x04 == 0x04),
-   171 ('UVLO',data[0] & 0x02 == 0x02),
-   172 ('WRONG_CMD',data[0] & 0x01 == 0x01), #The NOTPERF_CMD and WRONG_CMD flags are active high and indicate, respectively, that the command received by SPI cannot be performed or does not exist at all.
-   173 ('NOTPERF_CMD',data[1] & 0x80 == 0x80),
-   174 ('MOT_STATUS',data[1] & 0x60),
-   175 ('DIR',data[1] & 0x10 == 0x10),
-   176 ('SW_EVN',data[1] & 0x08 == 0x08),
-   177 ('SW_F',data[1] & 0x04 == 0x04), #The SW_F flag reports the SW input status (low for open and high for closed).
-   178 ('BUSY',data[1] & 0x02 != 0x02),
-   179 ('HIZ',data[1] & 0x01 == 0x01)])
-   180 return status
-   181  
-   182 def GetACC(self):
-   183 # self.spi.xfer([0x29]) # Gotparam command on status register
-   184 spi.SPI_write_byte(self.CS, self.L6470_ACC + 0x20) # TODO check register read address seting
-   185 spi.SPI_write_byte(self.CS, 0x00)
-   186 data = spi.SPI_read_byte()
-   187 spi.SPI_write_byte(self.CS, 0x00)
-   188 data = data + self.spi.readbytes(1)
-   189 print data # return speed in real units
-   190  
-   191  
155 def Move(self, units): 192 def Move(self, units):
156 ' Move some distance units from current position ' 193 ' Move some distance units from current position '
157 steps = units * self.SPU # translate units to steps 194 steps = units * self.SPU # translate units to steps
158 if steps > 0: # look for direction 195 if steps > 0: # look for direction
159 spi.SPI_write_byte(self.CS, 0x40 | (~self.Dir & 1)) 196 spi.SPI_write_byte(self.CS, 0x40 | (~self.Dir & 1))
Line 177... Line 214...
177 return (speed_value * 0.015) 214 return (speed_value * 0.015)
178   215  
179 def MoveWait(self, units): 216 def MoveWait(self, units):
180 ' Move some distance units from current position and wait for execution ' 217 ' Move some distance units from current position and wait for execution '
181 self.Move(units) 218 self.Move(units)
182 while self.IsBusy(): 219 while self.GetStatus()['BUSY']:
183 pass 220 time.sleep(0.1)
184   221  
185 def Float(self, hard = False): 222 def Float(self, hard = False):
186 ' switch H-bridge to High impedance state ' 223 ' switch H-bridge to High impedance state '
187 if (hard == False): 224 if (hard == False):
188 spi.SPI_write_byte(self.CS, 0xA0) 225 spi.SPI_write_byte(self.CS, 0xA0)
189 else: 226 else:
190 spi.SPI_write_byte(self.CS, 0xA8) 227 spi.SPI_write_byte(self.CS, 0xA8)
191   228  
192 def ReadStatusBit(self, bit): -  
193 ' Report given status bit ' -  
194 spi.SPI_write_byte(self.CS, 0x39) # Read from address 0x19 (STATUS) -  
195 spi.SPI_write_byte(self.CS, 0x00) -  
196 data0 = spi.SPI_read_byte() # 1st byte -  
197 spi.SPI_write_byte(self.CS, 0x00) -  
198 data1 = spi.SPI_read_byte() # 2nd byte -  
199 #print hex(data0), hex(data1) -  
200 if bit > 7: # extract requested bit -  
201 OutputBit = (data0 >> (bit - 8)) & 1 -  
202 else: -  
203 OutputBit = (data1 >> bit) & 1 -  
204 return OutputBit -  
205   -  
206 -  
207 def IsBusy(self): -  
208 """ Return True if tehre are motion """ -  
209 if self.ReadStatusBit(1) == 1: -  
210 return False -  
211 else: -  
212 return True -  
213   229  
214 # End Class axis -------------------------------------------------- 230 # End Class axis --------------------------------------------------
215   231  
216   232  
217   233  
Line 245... Line 261...
245 spi.SPI_config(spi.I2CSPI_MSB_FIRST| spi.I2CSPI_MODE_CLK_IDLE_HIGH_DATA_EDGE_TRAILING| spi.I2CSPI_CLK_461kHz) 261 spi.SPI_config(spi.I2CSPI_MSB_FIRST| spi.I2CSPI_MODE_CLK_IDLE_HIGH_DATA_EDGE_TRAILING| spi.I2CSPI_CLK_461kHz)
246 time.sleep(1) 262 time.sleep(1)
247   263  
248 print "Axis inicialization" 264 print "Axis inicialization"
249 X = axis(spi.I2CSPI_SS0, 0, 1, MaxSpeed = SPEED) # set Number of Steps per axis Unit and set Direction of Rotation 265 X = axis(spi.I2CSPI_SS0, 0, 1, MaxSpeed = SPEED) # set Number of Steps per axis Unit and set Direction of Rotation
250 X.MaxSpeed(SPEED) # set maximal motor speed 266 X.MaxSpeed(SPEED) # Reset maximal motor speed
251   267  
252 print "Axis is running" 268 print "Axis is running"
253   269  
254 for i in range(5): 270 for i in range(5):
255 print i 271 print i