| 0,0 → 1,199 |
|---|
| #!/usr/bin/python |
| |
| #uncomment for debbug purposes |
| #import logging |
| #logging.basicConfig(level=logging.DEBUG) |
| |
| import sys |
| import time |
| from pymlab import config |
| |
| XSMM = 641 |
| YSMM = 642 |
| ZSMM = 32256 |
| |
| |
| class axis: |
| def __init__(self, SPI_CS, Direction): |
| """ One axis of robot """ |
| self.CS = SPI_CS |
| self.Dir = Direction |
| self.Reset() |
| |
| def Reset(self): |
| """ Reset Axis an set default parameters for H-bridge """ |
| spi.SPI_write(self.CS, [0xC0]) # reset |
| spi.SPI_write(self.CS, [0x14]) # Stall Treshold setup |
| spi.SPI_write(self.CS, [0x7F]) |
| spi.SPI_write(self.CS, [0x14]) # Over Current Treshold setup |
| spi.SPI_write(self.CS, [0x0F]) |
| #spi.SPI_write(self.CS, [0x15]) # Full Step speed |
| #spi.SPI_write(self.CS, [0x00]) |
| #spi.SPI_write(self.CS, [0x30]) |
| #spi.SPI_write(self.CS, [0x0A]) # KVAL_RUN |
| #spi.SPI_write(self.CS, [0x50]) |
| |
| def MaxSpeed(self, speed): |
| """ Setup of maximum speed """ |
| spi.SPI_write(self.CS, [0x07]) # Max Speed setup |
| spi.SPI_write(self.CS, [0x00]) |
| spi.SPI_write(self.CS, [speed]) |
| |
| def ReleaseSW(self): |
| """ Go away from Limit Switch """ |
| while self.ReadStatusBit(2) == 1: # is Limit Switch ON ? |
| spi.SPI_write(self.CS, [0x92 | (~self.Dir & 1)]) # release SW |
| while self.IsBusy(): |
| pass |
| spi.SPI_write(self.CS, [0x40 | (~self.Dir & 1)]) # move 0x2000 steps away |
| spi.SPI_write(self.CS, [0x00]) |
| spi.SPI_write(self.CS, [0x20]) |
| spi.SPI_write(self.CS, [0x00]) |
| while self.IsBusy(): |
| pass |
| |
| def GoZero(self, speed): |
| """ Go to Zero position """ |
| self.ReleaseSW() |
| |
| spi.SPI_write(self.CS, [0x82 | (self.Dir & 1)]) # Go to Zero |
| spi.SPI_write(self.CS, [0x00]) |
| spi.SPI_write(self.CS, [speed]) |
| while self.IsBusy(): |
| pass |
| time.sleep(0.3) |
| self.ReleaseSW() |
| |
| def Move(self, steps): |
| """ Move some steps from current position """ |
| if steps > 0: # look for direction |
| spi.SPI_write(self.CS, [0x40 | (~self.Dir & 1)]) |
| else: |
| spi.SPI_write(self.CS, [0x40 | (self.Dir & 1)]) |
| steps = int(abs(steps)) |
| spi.SPI_write(self.CS, [(steps >> 16) & 0xFF]) |
| spi.SPI_write(self.CS, [(steps >> 8) & 0xFF]) |
| spi.SPI_write(self.CS, [steps & 0xFF]) |
| |
| def MoveWait(self, steps): |
| self.Move(steps) |
| while self.IsBusy(): |
| pass |
| |
| def Float(self): |
| """ switch H-bridge to High impedance state """ |
| spi.SPI_write(self.CS, [0xA0]) |
| |
| def ReadStatusBit(self, bit): |
| """ Report given status bit """ |
| try: |
| spi.SPI_write(self.CS, [0x39]) # Read from address 0x19 (STATUS) |
| spi.SPI_write(self.CS, [0x00]) |
| data = spi.SPI_read(1) # 1st byte |
| spi.SPI_write(self.CS, [0x00]) |
| data.extend(spi.SPI_read(1)) # 2nd byte |
| if bit > 7: # extract requested bit |
| OutputBit = (data[0] >> (bit - 8)) & 1 |
| else: |
| OutputBit = (data[1] >> bit) & 1 |
| return OutputBit |
| except IOError(): ### TODO |
| spi.SPI_write(self.CS, [0x39]) # Read from address 0x19 (STATUS) |
| spi.SPI_write(self.CS, [0x00]) |
| data = spi.SPI_read(1) # 1st byte |
| spi.SPI_write(self.CS, [0x00]) |
| data.extend(spi.SPI_read(1)) # 2nd byte |
| if bit > 7: # extract requested bit |
| OutputBit = (data[0] >> (bit - 8)) & 1 |
| else: |
| OutputBit = (data[1] >> bit) & 1 |
| return OutputBit |
| finally: |
| pass |
| |
| |
| def IsBusy(self): |
| """ Return True if tehre are motion """ |
| if self.ReadStatusBit(1) == 1: |
| return False |
| else: |
| return True |
| |
| |
| cfg = config.Config( |
| i2c = { |
| "port": 8, |
| }, |
| |
| bus = [ |
| { "name":"spi", "type":"i2cspi"}, |
| ], |
| ) |
| |
| cfg.initialize() |
| |
| print "Irradiation unit. \r\n" |
| |
| spi = cfg.get_device("spi") |
| |
| |
| |
| try: |
| |
| while True: |
| print "SPI configuration.." |
| spi.SPI_config(spi.I2CSPI_MSB_FIRST| spi.I2CSPI_MODE_CLK_IDLE_HIGH_DATA_EDGE_TRAILING| spi.I2CSPI_CLK_461kHz) |
| |
| print "Robot inicialization" |
| X = axis(spi.I2CSPI_SS1, 0) |
| Y = axis(spi.I2CSPI_SS0, 1) |
| Z = axis(spi.I2CSPI_SS2, 1) |
| X.MaxSpeed(60) |
| Y.MaxSpeed(60) |
| Z.MaxSpeed(38) |
| |
| Z.GoZero(100) |
| Z.Move(100000) |
| X.GoZero(20) |
| Y.GoZero(20) |
| |
| time.sleep(1) |
| |
| X.Move(30*XSMM) |
| Y.Move(50*YSMM) |
| Z.MoveWait(58*ZSMM) |
| X.Move(50*XSMM) |
| |
| print "Robot is running" |
| |
| for y in range(5): |
| for x in range(5): |
| Z.MoveWait(5*ZSMM) |
| time.sleep(1) |
| Z.MoveWait(-5*ZSMM) |
| if x < 4: |
| X.MoveWait(8*XSMM) |
| Y.MoveWait(8*YSMM) |
| for x in range(5): |
| Z.MoveWait(5*ZSMM) |
| time.sleep(1) |
| Z.MoveWait(-5*ZSMM) |
| if x < 4: |
| X.MoveWait(-8*XSMM) |
| Y.MoveWait(8*YSMM) |
| |
| X.MoveWait(-20*XSMM) |
| Z.MoveWait(-30*ZSMM) |
| X.Float() |
| Y.Float() |
| Z.Float() |
| |
| |
| ''' |
| while True: |
| print X.ReadStatusBit(2) |
| time.sleep(1) |
| ''' |
| |
| finally: |
| print "stop" |
| Property changes: |
|---|
| Added: svn:executable |
|---|
| +* |
|---|
| \ No newline at end of property |
|---|
