Problem with comparison.
/Designs/ROBOTS/IRRAD01A/SW/irrad.py |
---|
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 |