import time from roboclaw_3 import Roboclaw #Windows comport name #Linux comport name rc = Roboclaw("/dev/ttyACM0", 115200) rc.Open() address = 0x80 for i in range(1): rc.ForwardM1(address, 127) #1/4 power forward time.sleep(2) rc.BackwardM1(address, 0) #Stopped rc.ForwardM2(address, 0) #Stopped time.sleep(2)
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 MotorController(object): def __init__(self): # Initialize addresses = [0x80, 0x81] self.addresses = [0x80, 0x81] # Connect to roboclaw serial_port = "/dev/ttyS0" baudrate = 38400 self.roboclaw = Roboclaw(serial_port, baudrate) self.roboclaw.Open() self.robotspeed = 10 def forward(self, drivetime): for address in self.addresses: self.roboclaw.ForwardMixed(address, self.robotspeed) sleep(drivetime) for address in self.addresses: self.roboclaw.ForwardMixed(address, 0) def backward(self, drivetime): for address in self.addresses: self.roboclaw.BackwardMixed(address, self.robotspeed) sleep(drivetime) for address in self.addresses: self.roboclaw.BackwardMixed(address, 0) def right(self, drivetime): self.roboclaw.TurnRightMixed(self.addresses[0], self.robotspeed) self.roboclaw.TurnLeftMixed(self.addresses[1], self.robotspeed) sleep(drivetime) self.roboclaw.TurnRightMixed(self.addresses[0], 0) self.roboclaw.TurnLeftMixed(self.addresses[1], 0) def left(self, drivetime): self.roboclaw.TurnLeftMixed(self.addresses[0], self.robotspeed) self.roboclaw.TurnRightMixed(self.addresses[1], self.robotspeed) sleep(drivetime) self.roboclaw.TurnLeftMixed(self.addresses[0], 0) self.roboclaw.TurnRightMixed(self.addresses[1], 0) def diagonals(self, direction, drivetime): if direction == 'northeast' or direction == "wd": for address in self.addresses: self.roboclaw.ForwardM1(address, self.robotspeed) sleep(drivetime) elif direction == 'northwest' or direction == "wa": for address in self.addresses: self.roboclaw.ForwardM2(address, self.robotspeed) sleep(drivetime) elif direction == 'southeast' or direction == "sd": for address in self.addresses: self.roboclaw.BackwardM2(address, self.robotspeed) sleep(drivetime) elif direction == 'southwest' or direction == "sa": for address in self.addresses: self.roboclaw.BackwardM1(address, self.robotspeed) sleep(drivetime) def rotate(self, direction, drivetime): if direction == 'clockwise' or direction == 'e': for address in self.addresses: self.roboclaw.TurnLeftMixed(address, self.robotspeed) sleep(drivetime) elif direction == 'counter-clockwise' or direction == 'q': for address in self.addresses: self.roboclaw.TurnRightMixed(address, self.robotspeed) sleep(drivetime)
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)
class Launcher: def __init__(self): #setup variables #Linux comport name #self.rc = Roboclaw("/dev/ttyACM1",115200) #Windows com-port name self.rc = Roboclaw("COM8", 115200) self.rc.Open() #Declare variables self.address = 0x80 #Controller 1, M1=Pitch, M2=Rotation self.address_2 = 0x81 #Controller 2, M1=Lift, M2=Launch self.pitch_pulses = 355000 #Encoder pulses from the linear actuator self.pitch_length = 90.0 #Degrees self.pitch_speed_pulses = 7000 #Pulses per second self.pitch_speed_manual = 75 #From 0 to 127 self.pitch_ready = 70.0 #Pitch degrees for the launch (temporary) self.rotation_pulses = 950000 #Encoder pulses from the rotation motor self.rotation_length = 180.0 #Degrees self.rotation_speed_pulses = 16000 #Pulses per second self.rotation_speed_manual = 127 #From 0 to 127 self.rotation_ready = 10.0 #Rotation degress for the launch (temporary) self.lift_pulses = 19000 #Encoder pulses from the lifting colum self.lift_length = 130.0 #cm self.lift_speed_pulses = 420 #Pulses per second self.lift_speed_manual = 127 #From 0 to 127 - 7 bits self.lift_ready = self.lift_length #Lift lenght for the launch (temporary) self.launch_pulses = 14800 #Encoder pulses from the launch motor self.launch_length = 111.0 #cm self.launch_speed_pulses = 6 * 13400 #Pulses per second during launch (145000 max) (13400 pulses/m) self.launch_speed_pulses_slow = 2500 #Pulses per second during preparation self.launch_speed_manual = 12 #From 0 to 127 self.launch_acceleration = ( self.launch_speed_pulses** 2) / 13400 #Acceleration during launch (pulses/second2) self.launch_max_speed = 10 #Maximum launch speed self.launch_min_speed = 1 #Minimum launch speed self.launch_max_acceleration = 48 #Maximum launch acceleration self.launch_min_acceleration = 1 #Minimum launch acceleration self.launch_standby = 8000 #Drone position during stand-by self.launch_mount = 17000 #Drone position during mounting self.launch_break = 21000 #Belt position during breaking self.launch_bottom = 0 #Drone position at the back part of the capsule self.launch_connect = 2190 #Belt position for touching the upper part self.encoders_ready = 0 #At the beggining, the encoders are not ready def encoder_ready_check(self): ''' Checks if encoder i ready. return: True or False ''' if self.encoders_ready == 0: return False else: return True # --------------------------------------------------------------------------------- # ----------------- Pitch Functions ----------------------------------------------- # --------------------------------------------------------------------------------- def set_pitch_position(self, pitch_position): ''' sets the pitch position of the launcher by the given pitch_position parameter. Args: pitch_position (float): desired pitch position for pitch motors in degrees ''' if self.encoder_ready_check(): # Checks conditions if pitch_position > self.pitch_length or pitch_position < 0: raise ValueError("out of bounds") elif pitch_position == 0: pitch_objective = 0 else: pitch_objective = int(self.pitch_pulses / (self.pitch_length / pitch_position)) pitch_increment = pitch_objective - self.rc.ReadEncM1( self.address)[1] if pitch_increment >= 0: self.rc.SpeedDistanceM1( self.address, self.pitch_speed_pulses, pitch_increment, 1 ) #(address, +-speed, pulses, buffer(0=buffered, 1=Execute immediately)) self.rc.SpeedDistanceM1(self.address, 0, 0, 0) #To avoid deceleration else: self.rc.SpeedDistanceM1(self.address, -self.pitch_speed_pulses, -pitch_increment, 1) self.rc.SpeedDistanceM1(self.address, 0, 0, 0) #To avoid deceleration else: raise EncoderError('Encoder Not Ready') def pitch_control(self, cmd: PitchCMD): ''' Takes in a command (up, down or stop) and controlls the pitch accordingly Args: cmd (Pitch.CMD): desired movement command for pitch motors (PitchCMD.up, PitchCMD.down, PitchCMD.stop) ''' if cmd == PitchCMD.up: self.rc.BackwardM1(self.address, self.pitch_speed_manual) if cmd == PitchCMD.down: self.rc.ForwardM1(self.address, self.pitch_speed_manual) if cmd == PitchCMD.stop: self.rc.ForwardM1(self.address, 0) # --------------------------------------------------------------------------------- # ------------------------ Rotation functions-------------------------------------- # --------------------------------------------------------------------------------- def set_rotation_position(self, rotation_position): ''' sets the rotation position of the launcher by the given rotation_position parameter. Args: rotation_position (float): desired rotation position for rotation motors in degrees ''' if self.encoder_ready_check(): # Checks conditions if rotation_position > self.lift_length or rotation_position < 0: raise ValueError("out of bounds") elif rotation_position == 0: rotation_objective = 0 else: rotation_objective = int( (self.rotation_pulses / (self.rotation_length / rotation_position)) / 2) rotation_increment = rotation_objective - self.rc.ReadEncM2( self.address)[1] if rotation_increment >= 0: self.rc.SpeedDistanceM2( self.address, self.rotation_speed_pulses, rotation_increment, 1 ) #(address, +-speed, pulses, buffer(0=buffered, 1=Execute immediately)) self.rc.SpeedDistanceM2(self.address, 0, 0, 0) #To avoid deceleration else: self.rc.SpeedDistanceM2(self.address, -self.rotation_speed_pulses, -rotation_increment, 1) self.rc.SpeedDistanceM2(self.address, 0, 0, 0) #To avoid deceleration else: raise EncoderError('Encoder Not Ready') def rotation_control(self, cmd: RotationCMD): ''' Takes in a command (right, left or stop) and controlls the rotation accordingly Args: cmd (Rotation.CMD): desired movement command for lift motors (Rotation.right, Rotation.left, Rotation.stop) ''' if cmd == RotationCMD.right: self.rc.ForwardM1(self.address_2, self.rotation_speed_manual) self.rc.ForwardM2(self.address_2, self.rotation_speed_manual) if cmd == RotationCMD.left: self.rc.BackwardM1(self.address_2, self.rotation_speed_manual) self.rc.BackwardM2(self.address_2, self.rotation_speed_manual) if cmd == RotationCMD.stop: self.rc.ForwardM1(self.address_2, 0) self.rc.ForwardM2(self.address_2, 0) # --------------------------------------------------------------------------------- # ------------------------ Lift functions-------------------------------------- # --------------------------------------------------------------------------------- def set_lift_position(self, lift_position): ''' sets the lift position of the launcher by the given lift_position parameter. Args: lift_position (float): desired lift position on lift motors in centimeters ''' if self.encoder_ready_check(): # Checks conditions if lift_position > self.lift_length or lift_position < 0: raise ValueError("out of bounds") elif lift_position == 0: lift_objective = 0 else: lift_objective = int(self.lift_pulses / (self.lift_length / lift_position)) lift_increment = lift_objective - self.rc.ReadEncM1( self.address_2)[1] if lift_increment >= 0: self.rc.SpeedDistanceM1( self.address_2, self.lift_speed_pulses, lift_increment, 1 ) #(address, +-speed, pulses, buffer(0=buffered, 1=Execute immediately)) self.rc.SpeedDistanceM1(self.address_2, 0, 0, 0) #To avoid deceleration # set position cases else: self.rc.SpeedDistanceM1(self.address_2, -self.lift_speed_pulses, -lift_increment, 1) self.rc.SpeedDistanceM1(self.address_2, 0, 0, 0) #To avoid deceleration else: raise EncoderError('Encoder Not Ready') def lift_control(self, cmd: LiftCMD): ''' Takes in a command (up, down or stop) and controlls the lift accordingly Args: cmd (LiftCMD): desired movement command for lift motors (LiftCMD.up, LiftCMD.down, LiftCMD.stop) ''' if cmd == LiftCMD.up: self.rc.ForwardM1(self.address_2, self.lift_speed_manual) if cmd == LiftCMD.down: self.rc.BackwardM1(self.address_2, self.lift_speed_manual) if cmd == LiftCMD.stop: self.rc.ForwardM1(self.address_2, 0) # --------------------------------------------------------------------------------- # ------------------------ Launch functions-------------------------------------- # --------------------------------------------------------------------------------- def set_launch_position(self, launch_position): ''' sets the launch position of the launcher by the given launch_position parameter. Args: launch_position (float): desired launch position in centimeters ''' if self.encoder_ready_check(): # Checks conditions if launch_position > self.launch_length or launch_position < 0: raise ValueError("out of bounds") else: launch_objective = self.launch_bottom launch_increment = launch_objective - self.rc.ReadEncM2( self.address_2)[1] if launch_increment >= 0: self.rc.SpeedDistanceM2( self.address_2, self.launch_speed_pulses_slow, launch_increment, 1 ) #(address, +-speed, pulses, buffer(0=buffered, 1=Execute immediately)) self.rc.SpeedDistanceM2(self.address_2, 0, 0, 0) #To avoid deceleration else: self.rc.SpeedDistanceM2(self.address_2, -self.launch_speed_pulses_slow, -launch_increment, 1) self.rc.SpeedDistanceM2(self.address_2, 0, 0, 0) #To avoid deceleration buffer_2 = (0, 0, 0) while (buffer_2[2] != 0x80): #Loop until all movements are completed buffer_2 = self.rc.ReadBuffers(self.address_2) if launch_position == 0: launch_objective = 0 else: launch_objective = int(self.launch_pulses / (self.launch_length / launch_position)) launch_increment = launch_objective - self.rc.ReadEncM2( self.address_2)[1] + self.launch_connect if launch_increment >= 0: self.rc.SpeedDistanceM2( self.address_2, self.launch_speed_pulses_slow, launch_increment, 0 ) #(address, +-speed, pulses, buffer(0=buffered, 1=Execute immediately)) self.rc.SpeedDistanceM2(self.address_2, 0, 0, 0) #To avoid deceleration else: self.rc.SpeedDistanceM2(self.address_2, -self.launch_speed_pulses_slow, -launch_increment, 0) self.rc.SpeedDistanceM2(self.address_2, 0, 0, 0) #To avoid deceleration else: raise EncoderError('Encoder Not Ready') def launch_control(self, cmd: LaunchCMD): ''' Takes in a command (forwards, backwards or stop) and controlls the launch accordingly Args: cmd (LaunchCMD): desired launch movement for launch motors, (for example: Launch.CMD.forwards) ''' if cmd == LaunchCMD.up: self.rc.ForwardM2(self.address_2, self.launch_speed_manual) if cmd == LaunchCMD.down: self.rc.BackwardM2(self.address_2, self.launch_speed_manual) if cmd == LaunchCMD.stop: self.rc.ForwardM2(self.address_2, 0) def stop(self): ''' stops all motors ''' self.rc.ForwardM1(self.address, 0) self.rc.ForwardM2(self.address, 0) self.rc.ForwardM1(self.address_2, 0) self.rc.ForwardM2(self.address_2, 0) def max_pitch(self): if self.encoder_ready_check(): pitch_increment = self.pitch_pulses - self.rc.ReadEncM1( self.address)[1] if pitch_increment >= 0: self.rc.SpeedDistanceM1( self.address, self.pitch_speed_pulses, pitch_increment, 1 ) #(address, +-speed, pulses, buffer(0=buffered, 1=Execute immediately)) self.rc.SpeedDistanceM1(self.address, 0, 0, 0) #To avoid deceleration else: self.rc.SpeedDistanceM1(self.address, -self.pitch_speed_pulses, -pitch_increment, 1) self.rc.SpeedDistanceM1(self.address, 0, 0, 0) #To avoid deceleration def min_pitch(self): pass def min_lift(self): pass def max_lift(self): pass def home(self): pass def reset_encoders(self): #TODO: reset encoder. Either set to "1" (ready) OR make the inverse of what it is now. #example 1: self.encoders_ready = 1 #example 2: pass def battery_voltage(self): ''' Calculates and returns battery voltage param: none return: voltage (float): represents battery voltage ''' voltage = round(0.1 * self.rc.ReadMainBatteryVoltage(self.address)[1], 2) return voltage # --------------------------------------------------------------------------------- # ------------------------ Automated functions-------------------------------------- # --------------------------------------------------------------------------------- def standby(self): ''' Sets pitch, rotation, lift and launch to zero, which is the home position. ''' self.set_pitch_position(0) self.set_rotation_position(0) self.set_lift_position(0) self.set_launch_position(0) def set_launch_variables(self, pitch_position, rotation_position, lift_position): ''' Sets the variables before preparing the launch. Args: pitch_position (int): desired pitch position between values x and y rotation_position (int): desired rotation position between x and y lift_position (int): desired lift position in between values x and y launch_position (int): desired launch position in between values x and y ''' self.change_pitch(pitch_position) #Updates self.pitch_ready self.change_rotation(rotation_position) #Updates self.rotation_ready self.change_lift(lift_position) #Updates self.lift_ready def prepare_launch(self): ''' Configures the launchers pitch, rotation and lift according to the variables set in set_launch_variables(). also sets launch position to zero, since that is the start position before launch ''' self.set_pitch_position(self.pitch_ready) self.set_rotation_position(self.rotation_ready) # Changed self.lift_position to self.lift_ready since there is no variable named lift_position self.set_lift_position(self.lift_ready) self.set_launch_position(0) def launch(self): ''' launch operates the launch motor + belt. when this runs, the drone launches into the air. ''' pass def mount(self): pass def automatic_launch(self): pass # --------------------------------------------------------------------------------- # ------------------------ Variable update functions------------------------------- # --------------------------------------------------------------------------------- def change_pitch(self, pitch_position): if pitch_position > self.pitch_length or pitch_position < 0: raise ValueError("Out of Bounds") else: self.pitch_ready = pitch_position def change_lift(self, lift_position): if lift_position > self.lift_length or lift_position < 0: raise ValueError("Out of Bounds") else: self.lift_ready = lift_position def change_rotation(self, rotation_position): if rotation_position > self.rotation_length or rotation_position < 0: raise ValueError("Out of Bounds") else: self.rotation_ready = rotation_position def change_speed(self, speed): if speed > self.launch_max_speed or speed < self.launch_min_speed: raise ValueError("Out of Bounds") else: if speed > 7: self.launch_speed_pulses = speed * 13400 self.launch_acceleration = 655360 #maximum value else: self.launch_speed_pulses = speed * 13400 self.launch_acceleration = (self.launch_speed_pulses** 2) / 13400 def change_acceleration(self, acceleration): if acceleration > self.launch_max_acceleration or acceleration < self.launch_min_acceleration: raise ValueError("Out of Bounds") else: self.launch_acceleration = acceleration * 13400 def disable_buttons(self): pass
from roboclaw_3 import Roboclaw #Windows comport name #rc = Roboclaw("COM11",115200) #Linux comport name rc = Roboclaw("/dev/ttyACM0",115200) rc.Open() address = 0x80 while(1): rc.ForwardM1(address,32) #1/4 power forward rc.BackwardM2(address,32) #1/4 power backward time.sleep(2) rc.BackwardM1(address,32) #1/4 power backward rc.ForwardM2(address,32) #1/4 power forward time.sleep(2) rc.BackwardM1(address,0) #Stopped rc.ForwardM2(address,0) #Stopped time.sleep(2) m1duty = 16 m2duty = -16 rc.ForwardBackwardM1(address,64+m1duty) #1/4 power forward rc.ForwardBackwardM2(address,64+m2duty) #1/4 power backward time.sleep(2) m1duty = -16 m2duty = 16
from roboclaw_3 import Roboclaw from time import sleep #Windows comport name rc = Roboclaw("COM6", 115200) #Linux comport name #rc = Roboclaw("/dev/ttyACM0",115200) rc.Open() while (True): rc.ForwardM1(0x80, 64) sleep(2) rc.ForwardM1(0x80, 0) sleep(2) rc.BackwardM1(0x80, 64) sleep(2) rc.BackwardM1(0x80, 0) sleep(2)
class Footballmachine: def __init__(self, address=0x80, baudrate=38400, port="/dev/ttyS0"): self.address = address self.rc = Roboclaw(port, baudrate) self.rc.Open() version = self.rc.ReadVersion(self.address) if version[0] == False: print("GETVERSION Failed - check power supply and conections") return else: print(repr(version[1])) def has_angle_motor_stopped_moving(self): interval = 1 first = self.rc.ReadEncM1(self.address) sleep(interval) second = self.rc.ReadEncM1(self.address) while (first != second): first = self.rc.ReadEncM1(self.address) sleep(interval) second = self.rc.ReadEncM1(self.address) def init_motors(self): print("Initializing all motors...") self.rc.BackwardM1(self.address, 126) self.has_angle_motor_stopped_moving() self.rc.BackwardM1(self.address, 0) self.rc.ResetEncoders(self.address) print("Angle encoder:", self.rc.ReadEncM1(self.address)[1]) def displayspeed(self): enc1 = self.rc.ReadEncM1(self.address) enc2 = self.rc.ReadEncM2(self.address) speed1 = self.rc.ReadSpeedM1(self.address) speed2 = self.rc.ReadSpeedM2(self.address) print(("Encoder1:"), end=' ') if (enc1[0] == 1): print(enc1[1], end=' ') print(format(enc1[2], '02x'), end=' ') else: print("failed", end=' ') print("Encoder2:", end=' ') if (enc2[0] == 1): print(enc2[1], end=' ') print(format(enc2[2], '02x'), end=' ') else: print("failed ", end=' ') print("Speed1:", end=' ') if (speed1[0]): print(speed1[1], end=' ') else: print("failed", end=' ') print(("Speed2:"), end=' ') if (speed2[0]): print(speed2[1]) else: print("failed ") def speed_to_QPPS(self, speed): radius = 0.1 encoder_pulses_per_rad = 1024 / 2 angular_speed = speed / (2 * pi * radius) QPPS = encoder_pulses_per_rad * angular_speed return int(QPPS) def angle_to_QP(self, angle): range_min = 0 range_max = 225 angle_min = 0 angle_max = 55 a1 = int(angle) - angle_min a2 = range_max - range_min a3 = angle_max - angle_min angle = int((a1 * a2) / a3 + range_min) return angle def set_angle(self, angle): print("Set_angle: ", angle) angle = self.angle_to_QP(angle) print("Target position M1:", angle) self.rc.SpeedAccelDeccelPositionM1(self.address, 10, 10, 10, angle, 0) self.has_angle_motor_stopped_moving() print("Angle encoder:", self.rc.ReadEncM1(self.address)[1]) def set_speed_then_stop(self, speed): print("Set_speed: ", speed) speed = self.speed_to_QPPS(int(speed)) self.rc.SpeedAccelM2(self.address, 22000, speed) sleep(4) self.rc.SpeedAccelM2(self.address, 22000, 0) def set_speed(self, speed): print("Set_speed: ", speed) speed = self.speed_to_QPPS(int(speed)) self.rc.SpeedAccelM2(self.address, 22000, speed) for i in range(0, 50): print(("{} # ".format(i)), end=' ') self.displayspeed() sleep(0.1) def check_encoders(self, seconds): for i in range(0, seconds): print(("{} # ".format(i)), end=' ') self.displayspeed() sleep(0.1)