class BeltControl:
def __init__(self, addr):
self.act = Roboclaw(addr, 115200)
self.currents = [0,0]
while self.act.Open()==0:
print("Failed to open actuator comms, trying again.")
time.sleep(1)
print("Opened Belt roboclaw to ",addr)
############# public methods #############
def updateCurrents(self):
con1 = self.act.ReadCurrents(0x80)
digCurr = float(con1[1]) / 100
offCurr = float(con1[2]) / 100
self.currents = [digCurr, offCurr]
# method for reading the blet currents
def readCurrents(self, i):
return self.currents[i]
# control the offload belt
def offload(self, speed):
speed = self.verify_speed(speed)
direction = "s"
if speed <= 1800:
# move actuator forward
adjusted_speed = self.translate_value(speed, 1800,0,0,127)
direction = "b"
elif speed >= 2200:
#move actuator backward
adjusted_speed = self.translate_value(speed, 2200, 4095,0,127)
direction = "f"
else :
#do not move actuator
direction = "s"
adjusted_speed = 0
if direction == "f" :
self.act.ForwardM1(0x80, int(adjusted_speed))
elif direction == "b":
self.act.BackwardM1(0x80, int(adjusted_speed))
else:
self.act.ForwardM1(0x80, 0)
self.updateCurrents()
# control the digging belt
def dig(self, speed):
speed = self.verify_speed(speed)
direction = "s"
if speed <= 1800:
# move actuator forward
adjusted_speed = self.translate_value(speed, 1800,0,0,127)
direction = "b"
elif speed >= 2200:
#move actuator backward
adjusted_speed = self.translate_value(speed, 2200, 4095,0,127)
direction = "f"
else :
#do not move actuator
direction = "s"
adjusted_speed = 0
if direction == "f" :
self.act.ForwardM2(0x80, int(adjusted_speed))
elif direction == "b":
self.act.BackwardM2(0x80, int(adjusted_speed))
else:
self.act.ForwardM2(0x80, 0)
self.updateCurrents()
############# private methods #############
# verifies speed and position
def verify_speed(self, speed):
# set maximum speed that the actuator can go
maximum = 4095
# cap the speed at the max in either direction
if speed > maximum:
speed = maximum
elif speed < 0:
speed = 0
else:
speed = speed
return speed
# translates vale from left range to right range
def translate_value(self, value, leftMin, leftMax, rightMin, rightMax):
leftSpan = leftMax - leftMin
rightSpan = rightMax - rightMin
valueScaled = float(value-leftMin)/float(leftSpan)
return rightMin + (valueScaled * rightSpan)
class DriveControl:
def __init__(self):
self.rc1 = Roboclaw('/dev/roboclaw1', 115200)
self.rc2 = Roboclaw('/dev/roboclaw2', 115200)
self.currents = [0, 0, 0, 0]
while self.rc1.Open() == 0:
print('OPEN ROBOCLAW 1 COMMS FAILED, RETRYING...')
time.sleep(1)
print('OPENED ROBOCLAW 1 COMMS')
while self.rc2.Open() == 0:
print('OPEN ROBOCLAW 2 COMMS FAILED, RETRYING...')
time.sleep(1)
print('OPENED ROBOCLAW 2 COMMS')
print('STARTING CURRENT MONITOR LOOP')
def updateCurrents(self):
con1 = self.rc1.ReadCurrents(0x80)
con2 = self.rc2.ReadCurrents(0x80)
frontLeftCurr = float(con1[1]) / 100
frontRightCurr = float(con1[2]) / 100
backLeftCurr = float(con2[1]) / 100
backRightCurr = float(con2[2]) / 100
self.currents = [
frontLeftCurr, backLeftCurr, frontRightCurr, backRightCurr
]
print(self.currents)
def readCurrents(self, i):
print(self.currents[i])
return self.currents[i]
def moveLeftSide(self, speed):
self.drive(self.rc1, 'm1', speed)
self.drive(self.rc1, 'm2', speed)
def moveRightSide(self, speed):
self.drive(self.rc2, 'm1', speed)
self.drive(self.rc2, 'm2', speed)
def moveM1(self, speed):
self.drive(self.rc1, 'm1', speed)
def moveM2(self, speed):
self.drive(self.rc1, 'm2', speed)
def moveM3(self, speed):
self.drive(self.rc2, 'm1', speed)
def moveM4(self, speed):
self.drive(self.rc2, 'm2', speed)
def drive(self, claw, motor, speed):
speed = self.ensureValidSpeed(speed)
direction = 's' # needs to be either f or b to run
scaledValue = 0
if speed <= 1800:
scaledValue = self.translateValue(speed, 1800, 0, 0, 127)
direction = 'b'
elif speed >= 2200:
scaledValue = self.translateValue(speed, 2200, 4095, 0, 127)
direction = 'f'
else:
direction = 'f'
scaledValue = 0
# forward and backward go the same direction here becuase the
# motors are reversed in the wiring
if motor == 'm1':
if direction == 'f':
claw.ForwardM1(0x80, int(scaledValue))
elif direction == 'b':
claw.BackwardM1(0x80, int(scaledValue))
else:
print('bad direction value')
elif motor == 'm2':
if direction == 'f':
claw.ForwardM2(0x80, int(scaledValue))
elif direction == 'b':
claw.BackwardM2(0x80, int(scaledValue))
else:
print('bad direction value')
else:
print('bad motor index')
self.updateCurrents()
def ensureValidSpeed(self, speed):
if speed < 0:
speed = 0
if speed > 4095:
speed = 4095
return speed
def translateValue(self, value, leftMin, leftMax, rightMin, rightMax):
leftSpan = leftMax - leftMin
rightSpan = rightMax - rightMin
valueScaled = float(value - leftMin) / float(leftSpan)
return rightMin + (valueScaled * rightSpan)
class ActuatorControl:
# max and min values that the ADCInterface will send
MAX_POS = 100
MIN_POS = 0
def __init__(self, addr1, addr2):
self.act1 = Roboclaw(addr1, 115200)
self.act2 = Roboclaw(addr2, 115200)
while self.act1.Open() == 0:
print("Failed to open actuator comms, trying again.")
time.sleep(1)
while self.act2.Open() == 0:
print("Failed to open actuator 2 comms, trying again")
time.sleep(1)
print("Opened Actuator roboclaw to ", addr1, "and ", addr2)
############# public methods #############
# set adc interface for reading the current position of the actuator
#def setInterface(self, adcObj):
#expects an adc interface object
#self.act_interface = adcObj
#self.pos = self.act_interface.readADC()
# move the actuator letting the move command specify the direction
def readCurrents(self, i):
con1 = self.act1.ReadCurrents(0x80)
con2 = self.act2.ReadCurrents(0x80)
digCurrent = (con1[1] + con2[1]) / 100
raiseCurrent = (con1[2] + con2[2]) / 100
currents = [digCurrent, raiseCurrent]
return currents[i]
def moveDig(self, speed=False):
self.moveActBinary('dig', speed)
def moveRaise(self, speed=False):
self.moveActBinary('raise', speed)
# move up with the option of specifying speed
def moveUp(self, speed=False):
#refresh position
#self.pos = self.act_interface.readADC()
if not speed:
self.moveActBinary(1799)
else:
self.moveActScalar(speed)
# move down with the option of specifying speed
def moveDown(self, speed=False):
#refresh position
#self.pos = self.act_interface.readADC()
if not speed:
self.moveActBinary(2201)
else:
self.moveActScalar(speed)
def stop(self):
self.moveActBinary(2000)
############# private methods #############
# single speed actuator movement
def moveActBinary(self, actChoice, speed):
speed = self.verify_speed(speed)
if actChoice == 'dig':
if speed <= 1800:
self.act1.ForwardM1(0x80, 118)
self.act2.ForwardM1(0x80, 127)
elif speed >= 2200:
self.act1.BackwardM1(0x80, 127)
self.act2.BackwardM1(0x80, 127)
else:
self.act1.ForwardM1(0x80, 0)
self.act2.ForwardM1(0x80, 0)
elif actChoice == 'raise':
if speed <= 1800:
self.act1.ForwardM2(0x80, 127)
self.act2.ForwardM2(0x80, 127)
elif speed >= 2200:
self.act1.BackwardM2(0x80, 127)
self.act2.BackwardM2(0x80, 127)
else:
self.act1.ForwardM2(0x80, 0)
self.act2.ForwardM2(0x80, 0)
else:
print("bad act choice, stopping both")
self.act1.ForwardM1(0x80, 0)
self.act1.ForwardM1(0x80, 0)
self.act2.ForwardM2(0x80, 0)
self.act2.ForwardM2(0x80, 0)
# variable speed actuator movement
def moveActScalar(self, actChoice, speed):
speed = self.verify_speed(speed)
if speed <= 1800:
# move actuator forward
adjusted_speed = translate_value(speed, 1800, 0, 0, 127)
direction = "b"
elif speed >= 2200:
#move actuator backward
adjusted_speed = translate_value(speed, 2200, 4095, 0, 127)
direction = "f"
else:
#do not move actuator
direction = "s"
adjusted_speed = 0
if direction == "f":
if actChoice == 'dig':
self.act1.ForwardM1(0x80, adjusted_speed)
self.act2.ForwardM1(0x80, adjusted_speed)
elif actChoice == 'raise':
self.act1.ForwardM2(0x80, adjusted_speed)
self.act2.ForwardM2(0x80, adjusted_speed)
else:
print("bad actuator choice")
elif direction == "b":
if actChoice == 'dig':
self.act1.BackwardM1(0x80, adjusted_speed)
self.act2.BackwardM1(0x80, adjusted_speed)
if actChoice == 'raise':
self.act1.BackwardM2(0x80, adjusted_speed)
self.act2.BackwardM2(0x80, adjusted_speed)
else:
print("bad actuator choice")
else:
self.act1.ForwardM1(0x80, 0)
self.act1.ForwardM2(0x80, 0)
self.act2.ForwardM1(0x80, 0)
self.act2.ForwardM2(0x80, 0)
# verifies speed and position
def verify_speed(self, speed):
# set maximum speed that the actuator can go
maximum = 4095
# cap the speed at the max in either direction
if speed > maximum:
speed = maximum
elif speed < 0:
speed = 0
else:
speed = speed
# make sure not to drive the actuators with no more left
#if self.pos >= MAX_POS and speed > 2000:
# speed = 2000
#elif self.pos <= MIN_POS and speed < 2000:
# speed = 2000
#else:
# speed = speed
return speed
# translates vale from left range to right range
def translate_value(self, value, leftMin, leftMax, rightMin, rightMax):
leftSpan = leftMax - leftMin
rightSpan = rightMax - rightMin
valueScaled = float(value - leftMin) / float(leftSpan)
return rightMin + (valueScaled * rightSpan)
