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)