/Modules/H_Bridge/HBSTEP01B/SW/I2CSPI_HBSTEP_test.py
25,12 → 25,12
 
elif len(sys.argv) == 2:
PORT = eval(sys.argv[1])
SPEED = 5
DISTANCE = 50
SPEED = 50
DISTANCE = 5000
 
elif len(sys.argv) == 3:
SPEED = eval(sys.argv[2])
DISTANCE = 100
DISTANCE = 1000
 
elif len(sys.argv) == 4:
SPEED = eval(sys.argv[2])
38,21 → 38,53
 
else:
PORT = 0
SPEED = 10
DISTANCE = 50
SPEED = 20
DISTANCE = 5000
 
 
class axis:
def __init__(self, SPI_CS, Direction, StepsPerUnit):
def __init__(self, SPI_CS, Direction, StepsPerUnit, MaxSpeed):
' One axis of robot '
self.CS = SPI_CS
self.Dir = Direction
self.SPU = StepsPerUnit
self.maxspeed = MaxSpeed
 
self.L6470_ABS_POS =0x01
self.L6470_EL_POS =0x02
self.L6470_MARK =0x03
self.L6470_SPEED =0x04
self.L6470_ACC =0x05
self.L6470_DEC =0x06
self.L6470_MAX_SPEED =0x07
self.L6470_MIN_SPEED =0x08
self.L6470_FS_SPD =0x15
self.L6470_KVAL_HOLD =0x09
self.L6470_KVAL_RUN =0x0A
self.L6470_KVAL_ACC =0x0B
self.L6470_KVAL_DEC =0x0C
self.L6470_INT_SPEED =0x0D
self.L6470_ST_SLP =0x0E
self.L6470_FN_SLP_ACC =0x0F
self.L6470_FN_SLP_DEC =0x10
self.L6470_K_THERM =0x11
self.L6470_ADC_OUT =0x12
self.L6470_OCD_TH =0x13
self.L6470_STALL_TH =0x14
self.L6470_STEP_MODE =0x16
self.L6470_ALARM_EN =0x17
self.L6470_CONFIG =0x18
self.L6470_STATUS =0x19
 
self.Reset()
self.Initialize()
 
def Reset(self):
' Reset Axis and set default parameters for H-bridge '
'Reset the Axis'
spi.SPI_write_byte(self.CS, 0xC0) # reset
 
def Initialize(self):
'set default parameters for H-bridge '
# spi.SPI_write_byte(self.CS, 0x14) # Stall Treshold setup
# spi.SPI_write_byte(self.CS, 0xFF)
# spi.SPI_write_byte(self.CS, 0x13) # Over Current Treshold setup
66,22 → 98,40
spi.SPI_write_byte(self.CS, 0x06) # DEC
spi.SPI_write_byte(self.CS, 0x00)
spi.SPI_write_byte(self.CS, 0x10)
spi.SPI_write_byte(self.CS, 0x0A) # KVAL_RUN
spi.SPI_write_byte(self.CS, 0xFF)
spi.SPI_write_byte(self.CS, 0x0B) # KVAL_ACC
spi.SPI_write_byte(self.CS, 0xFF)
spi.SPI_write_byte(self.CS, 0x0C) # KVAL_DEC
spi.SPI_write_byte(self.CS, 0xFF)
spi.SPI_write_byte(self.CS, 0x18) # CONFIG
spi.SPI_write_byte(self.CS, 0b00111000)
spi.SPI_write_byte(self.CS, 0b00000000)
spi.SPI_write_byte(self.CS, self.L6470_KVAL_RUN) # KVAL_RUN
spi.SPI_write_byte(self.CS, 0x28)
spi.SPI_write_byte(self.CS, self.L6470_KVAL_ACC) # KVAL_ACC
spi.SPI_write_byte(self.CS, 0x28)
spi.SPI_write_byte(self.CS, self.L6470_KVAL_DEC) # KVAL_DEC
spi.SPI_write_byte(self.CS, 0x28)
# spi.SPI_write_byte(self.CS, 0x18) # CONFIG
# spi.SPI_write_byte(self.CS, 0b00111000)
# spi.SPI_write_byte(self.CS, 0b00000000)
self.MaxSpeed(self.maxspeed)
 
def setKVAL(self, hold = 0.5, run = 0.5, acc = 0.5, dec = 0.5):
""" The available range is from 0 to 0.996 x VS with a resolution of 0.004 x VS """
 
def setOverCurrentTH(self, hold = 0.5, run = 0.5, acc = 0.5, dec = 0.5):
""" The available range is from 375 mA to 6 A, in steps of 375 mA """
 
 
def MaxSpeed(self, speed):
' Setup of maximum speed '
spi.SPI_write_byte(self.CS, 0x07) # Max Speed setup
spi.SPI_write_byte(self.CS, 0x00)
spi.SPI_write_byte(self.CS, speed)
'Setup of maximum speed in steps/s. The available range is from 15.25 to 15610 step/s with a resolution of 15.25 step/s.'
speed_value = int(speed / 15.25)
if (speed_value <= 0):
speed_value = 1
elif (speed_value >= 1023):
speed_value = 1023
 
data = [(speed_value >> i & 0xff) for i in (8,0)]
print data
spi.SPI_write_byte(self.CS, self.L6470_MAX_SPEED) # Max Speed setup
spi.SPI_write_byte(self.CS, data[0])
spi.SPI_write_byte(self.CS, data[1])
return (speed_value * 15.25)
 
def ReleaseSW(self):
' Go away from Limit Switch '
while self.ReadStatusBit(2) == 1: # is Limit Switch ON ?
114,6 → 164,18
spi.SPI_write_byte(self.CS, (steps >> 8) & 0xFF)
spi.SPI_write_byte(self.CS, steps & 0xFF)
 
def Run(self, direction, speed):
speed_value = int(speed / 0.015)
print hex(speed_value)
 
command = 0b01010000 + int(direction)
data = [(speed_value >> i & 0xff) for i in (16,8,0)]
spi.SPI_write_byte(self.CS, command) # Max Speed setup
spi.SPI_write_byte(self.CS, data[0])
spi.SPI_write_byte(self.CS, data[1])
spi.SPI_write_byte(self.CS, data[2])
return (speed_value * 0.015)
 
def MoveWait(self, units):
' Move some distance units from current position and wait for execution '
self.Move(units)
120,9 → 182,12
while self.IsBusy():
pass
 
def Float(self):
def Float(self, hard = False):
' switch H-bridge to High impedance state '
spi.SPI_write_byte(self.CS, 0xA0)
if (hard == False):
spi.SPI_write_byte(self.CS, 0xA0)
else:
spi.SPI_write_byte(self.CS, 0xA8)
 
def ReadStatusBit(self, bit):
' Report given status bit '
181,7 → 246,7
time.sleep(1)
 
print "Axis inicialization"
X = axis(spi.I2CSPI_SS0, 0, 641) # set Number of Steps per axis Unit and set Direction of Rotation
X = axis(spi.I2CSPI_SS0, 0, 1, MaxSpeed = SPEED) # set Number of Steps per axis Unit and set Direction of Rotation
X.MaxSpeed(SPEED) # set maximal motor speed
 
print "Axis is running"
193,8 → 258,14
X.MoveWait(-DISTANCE) # move backward and wait for motor stop
print "Changing direction of rotation.."
 
X.Float() # release power
time.sleep(1)
 
X.Run(1, SPEED/2)
 
time.sleep(3)
 
X.Float(hard = False) # release power
 
 
finally:
print "stop"