def main(): # # get the encoder counts # # address of the RoboClaw as set in Motion Studio # address = 0x80 print(address) # # Creating the RoboClaw object, serial port and baudrate passed # robo = Roboclaw("/dev/ttymxc2", 38400) # # Starting communication with the RoboClaw hardware # opened = robo.Open() if opened: # # Start motor 1 in the forward direction at half speed # counts = robo.ReadEncM1(address) print("motor 1 counts: ", counts) m1_count = counts[1] print("motor 1 count: ", m1_count) else: print("port did not open") return
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
#Windows comport name #rc = Roboclaw("COM3",115200) #Linux comport name rc = Roboclaw("/dev/ttyS0", 38400) rc.Open() address = 0x80 #rc.ResetEncoders(address) while (1): print("Pos 50000") rc.SpeedAccelDeccelPositionM1(address, 500, 2000, 500, -5000, 1) for i in range(0, 80): print("Position: %d, Setpoint: %d", rc.ReadEncM1(address), 0) #displayspeed() time.sleep(0.1) time.sleep(2) #rc.SetEncM1(address, -10000) print("Pos 0") rc.SpeedAccelDeccelPositionM1(address, 4000, 4000, 4000, 0, 1) for i in range(0, 80): print(rc.ReadEncM1(address)) #displayspeed() time.sleep(0.1) time.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)