/Designs/ROBOTS/IRRAD01A/SW/irrad.py
8,108 → 8,96
import time
from pymlab import config
 
XSMM = 641
YSMM = 642
ZSMM = 32256
 
 
class axis:
def __init__(self, SPI_CS, Direction):
""" One axis of robot """
def __init__(self, SPI_CS, Direction, StepsPerUnit):
' One axis of robot '
self.CS = SPI_CS
self.Dir = Direction
self.SPU = StepsPerUnit
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])
' Reset Axis and 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])
' 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 """
' 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
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])
self.MoveWait(10) # move 10 units awey
'''
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 """
' 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])
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 """
def Move(self, units):
' Move some distance units from current position '
steps = units * self.SPU # translate units to steps
if steps > 0: # look for direction
spi.SPI_write(self.CS, [0x40 | (~self.Dir & 1)])
spi.SPI_write(self.CS, 0x40 | (~self.Dir & 1))
else:
spi.SPI_write(self.CS, [0x40 | (self.Dir & 1)])
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])
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)
def MoveWait(self, units):
' Move some distance units from current position and wait for execution '
self.Move(units)
while self.IsBusy():
pass
 
def Float(self):
""" switch H-bridge to High impedance state """
spi.SPI_write(self.CS, [0xA0])
' 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
' Report given status bit '
spi.SPI_write(self.CS, 0x39) # Read from address 0x19 (STATUS)
spi.SPI_write(self.CS, 0x00)
data0 = spi.SPI_read() # 1st byte
spi.SPI_write(self.CS, 0x00)
data1 = spi.SPI_read() # 2nd byte
print hex(data0), hex(data1)
if bit > 7: # extract requested bit
OutputBit = (data0 >> (bit - 8)) & 1
else:
OutputBit = (data1 >> bit) & 1
return OutputBit
 
def IsBusy(self):
145,49 → 133,49
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 = axis(spi.I2CSPI_SS1, 0, 641)
Y = axis(spi.I2CSPI_SS0, 1, 642)
Z = axis(spi.I2CSPI_SS2, 1, 32256)
X.MaxSpeed(60)
Y.MaxSpeed(60)
Z.MaxSpeed(38)
Z.GoZero(100)
Z.Move(100000)
#Y.GoZero(20)
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)
X.Move(30)
Y.Move(50)
Z.MoveWait(58)
X.Move(50)
print "Robot is running"
 
for y in range(5):
for y in range(2):
for x in range(5):
Z.MoveWait(5*ZSMM)
Z.MoveWait(5)
time.sleep(1)
Z.MoveWait(-5*ZSMM)
Z.MoveWait(-5)
if x < 4:
X.MoveWait(8*XSMM)
Y.MoveWait(8*YSMM)
X.MoveWait(8)
Y.MoveWait(8)
for x in range(5):
Z.MoveWait(5*ZSMM)
Z.MoveWait(5)
time.sleep(1)
Z.MoveWait(-5*ZSMM)
Z.MoveWait(-5)
if x < 4:
X.MoveWait(-8*XSMM)
Y.MoveWait(8*YSMM)
X.MoveWait(-8)
Y.MoveWait(8)
 
X.MoveWait(-20*XSMM)
Z.MoveWait(-30*ZSMM)
X.Move(-50)
Z.MoveWait(-30)
X.Float()
Y.Float()
Z.Float()
 
'''
while True: