No changes between revisions
/Designs/IRRAD01A/SW/irrad.py/irrad.py
0,0 → 1,183
#!/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 """
spi.SPI_write(self.CS, [0x39]) # Read from address 0x19 (STATUS)
time.sleep(0.2)
spi.SPI_write(self.CS, [0x00])
data = spi.SPI_read(1) # 1st byte
time.sleep(0.2)
spi.SPI_write(self.CS, [0x00])
data.extend(spi.SPI_read(1)) # 2nd byte
time.sleep(0.2)
if bit > 7: # extract requested bit
OutputBit = (data[0] >> (bit - 8)) & 1
else:
OutputBit = (data[1] >> bit) & 1
return OutputBit
 
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(50*XSMM)
Y.Move(50*YSMM)
Z.MoveWait(50*ZSMM)
print "Robot is running"
 
for y in range(5):
for x in range(5):
Z.MoveWait(8*ZSMM)
time.sleep(1)
Z.MoveWait(-8*ZSMM)
X.MoveWait(8*XSMM)
Y.MoveWait(8*YSMM)
for x in range(5):
Z.MoveWait(8*ZSMM)
time.sleep(1)
Z.MoveWait(-8*ZSMM)
X.MoveWait(-8*XSMM)
Y.MoveWait(8*YSMM)
 
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