class Clawbot: # drive base configs LEFT_MOTOR_PORT = Ports.PORT1 LEFT_MOTOR_REVERSE_POLARITY = False RIGHT_MOTOR_PORT = Ports.PORT6 RIGHT_MOTOR_REVERSE_POLARITY = True WHEEL_TRAVEL = 200 TRACK_WIDTH = 176 DISTANCE_UNIT = DistanceUnits.MM GEAR_RATIO = 1 # actuator configs ARM_MOTOR_PORT = Ports.PORT10 ARM_MOTOR_REVERSE_POLARITY = False ARM_MOTOR_VELOCITY = 30 # % CLAW_MOTOR_PORT = Ports.PORT11 CLAW_MOTOR_REVERSE_POLARITY = False CLAW_MOTOR_TIMEOUT_SECS = 3 CLAW_MOTOR_VELOCITY = 60 # % def __init__(self): self.drivetrain = \ Drivetrain( Motor( self.LEFT_MOTOR_PORT, # index self.LEFT_MOTOR_REVERSE_POLARITY # reverse ), # left_motor Motor( self.RIGHT_MOTOR_PORT, # index self.RIGHT_MOTOR_REVERSE_POLARITY # reverse ), # right_motor self.WHEEL_TRAVEL, # wheel_travel self.TRACK_WIDTH, # track_width self.DISTANCE_UNIT, # distanceUnit self.GEAR_RATIO # gear_ratio ) self.arm_motor = \ Motor( self.ARM_MOTOR_PORT, # index self.ARM_MOTOR_REVERSE_POLARITY # reverse ) self.claw_motor = \ Motor( self.CLAW_MOTOR_PORT, # index self.CLAW_MOTOR_REVERSE_POLARITY # reverse ) self.claw_motor.set_timeout( self.CLAW_MOTOR_TIMEOUT_SECS, # time TimeUnits.SEC # timeUnit )
DistanceUnits.MM, # distanceUnit 1 # gear_ratio ) # Arm motor configs ARM_MOTOR = Motor(Ports.PORT10, # index False # reverse ) ARM_MOTOR_VELOCITY_PERCENT = 30 # Claw motor configs CLAW_MOTOR = Motor(Ports.PORT11, # index False # reverse ) CLAW_MOTOR.set_timeout(3, # time TimeUnits.SEC # timeUnit ) CLAW_MOTOR_VELOCITY_PERCENT = 60 # sensor configs TOUCH_LED = Touchled(Ports.PORT2) COLOR_SENSOR = Colorsensor(Ports.PORT3, # index False, # is_grayscale 700 # proximity ) GYRO_SENSOR = Gyro(Ports.PORT4, # index True # calibrate ) DISTANCE_SENSOR = Sonar(Ports.PORT7)
class Armbot: # Base Pivot motor configs BASE_PIVOT_MOTOR_PORT = Ports.PORT10 BASE_PIVOT_REVERSE_POLARITY = False BASE_PIVOT_VELOCITY_PERCENT = 100 # Shoulder motor configs SHOULDER_MOTOR_PORT = Ports.PORT6 SHOULDER_REVERSE_POLARITY = True # reverse polarity # Elbow motor configs ELBOW_MOTOR_PORT = Ports.PORT1 ELBOW_REVERSE_POLARITY = False # Claw motor configs CLAW_MOTOR_PORT = Ports.PORT4 CLAW_MOTOR_REVERSE_POLARITY = False CLAW_MOTOR_VELOCITY_PERCENT = 100 CLAW_MOTOR_TIMEOUT_SECS = 3 # sensor configs BASE_BUMPER_SWITCH_PORT = Ports.PORT2 ELBOW_BUMPER_SWITCH_PORT = Ports.PORT9 DISTANCE_SENSOR_PORT = Ports.PORT8 COLOR_SENSOR_PORT = Ports.PORT12 LEFT_TOUCH_LED_PORT = Ports.PORT5 RIGHT_TOUCH_LED_PORT = Ports.PORT11 # controller configs CONTROLLER_DEADBAND = 3 # seconds def __init__(self): self.brain = Brain() self.base_pivot_motor = \ Motor( self.BASE_PIVOT_MOTOR_PORT, # index self.BASE_PIVOT_REVERSE_POLARITY # reverse polarity? ) self.shoulder_motor = \ Motor( self.SHOULDER_MOTOR_PORT, # index self.SHOULDER_REVERSE_POLARITY # reverse polarity? ) self.elbow_motor = \ Motor( self.ELBOW_MOTOR_PORT, # index self.ELBOW_REVERSE_POLARITY # reverse polarity? ) self.claw_motor = \ Motor( self.CLAW_MOTOR_PORT, # index self.CLAW_MOTOR_REVERSE_POLARITY # reverse polarity? ) self.claw_motor.set_timeout( self.CLAW_MOTOR_TIMEOUT_SECS, # time TimeUnits.SEC # timeUnit ) self.base_bumper_switch = Bumper(self.BASE_BUMPER_SWITCH_PORT) self.elbow_bumper_switch = Bumper(self.ELBOW_BUMPER_SWITCH_PORT) self.distance_sensor = Sonar(self.DISTANCE_SENSOR_PORT) self.color_sensor = \ Colorsensor( self.COLOR_SENSOR_PORT, # index False, # is_grayscale 700 # proximity ) self.left_touch_led = Touchled(self.LEFT_TOUCH_LED_PORT) self.right_touch_led = Touchled(self.RIGHT_TOUCH_LED_PORT) self.controller = Controller() self.controller.set_deadband(self.CONTROLLER_DEADBAND) def pivot_base_by_controller_left_buttons(self): # pivot base right if self.controller.buttonLDown.pressing(): self.base_pivot_motor.spin( DirectionType.REV, # dir self.BASE_PIVOT_VELOCITY_PERCENT, # velocity VelocityUnits.PCT # velocityUnit ) # pivot base left elif self.controller.buttonLUp.pressing(): self.base_pivot_motor.spin( DirectionType.FWD, # dir self.BASE_PIVOT_VELOCITY_PERCENT, # velocity VelocityUnits.PCT # velocityUnit ) else: self.base_pivot_motor.stop(BrakeType.HOLD) def keep_pivoting_base_by_controller_left_buttons(self): while True: self.pivot_base_by_controller_left_buttons() def control_shoulder_by_controller_axis_d(self): controller_axis_d_position = self.controller.axisD.position() if ((controller_axis_d_position > 0) and (not self.base_bumper_switch.pressing())) or \ (controller_axis_d_position < 0): self.shoulder_motor.spin( DirectionType.FWD, # dir controller_axis_d_position, # velocity VelocityUnits.PCT # velocityUnit ) else: self.shoulder_motor.stop(BrakeType.HOLD) def keep_controlling_shoulder_by_controller_axis_d(self): while True: self.control_shoulder_by_controller_axis_d() def control_elbow_by_controller_axis_a(self): controller_axis_a_position = self.controller.axisA.position() if ((controller_axis_a_position > 0) and (not self.elbow_bumper_switch.pressing())) or \ (controller_axis_a_position < 0): self.elbow_motor.spin( DirectionType.FWD, # dir controller_axis_a_position, # velocity VelocityUnits.PCT # velocityUnit ) else: self.elbow_motor.stop(BrakeType.HOLD) def keep_controlling_elbow_by_controller_axis_a(self): while True: self.control_elbow_by_controller_axis_a() def control_claw_by_controller_right_buttons(self): # open claw if self.controller.buttonRDown.pressing(): self.claw_motor.spin( DirectionType.REV, # dir self.CLAW_MOTOR_VELOCITY_PERCENT, # velocity VelocityUnits.PCT # velocityUnit ) # close claw elif self.controller.buttonRUp.pressing(): self.claw_motor.spin( DirectionType.FWD, # dir self.CLAW_MOTOR_VELOCITY_PERCENT, # velocity VelocityUnits.PCT # velocityUnit ) else: self.claw_motor.stop(BrakeType.HOLD) def keep_controlling_claw_by_controller_right_buttons(self): while True: self.control_claw_by_controller_right_buttons() def main(self): while True: self.pivot_base_by_controller_left_buttons() self.control_shoulder_by_controller_axis_d() self.control_elbow_by_controller_axis_a() self.control_claw_by_controller_right_buttons()
class Clawbot: # Drive Base configs LEFT_MOTOR_PORT = Ports.PORT1 LEFT_MOTOR_REVERSE_POLARITY = False RIGHT_MOTOR_PORT = Ports.PORT6 RIGHT_MOTOR_REVERSE_POLARITY = True WHEEL_TRAVEL = 200 TRACK_WIDTH = 176 DISTANCE_UNIT = DistanceUnits.MM GEAR_RATIO = 1 # Arm motor configs ARM_MOTOR_PORT = Ports.PORT10 ARM_MOTOR_REVERSE_POLARITY = False ARM_MOTOR_VELOCITY_PERCENT = 30 # Claw motor configs CLAW_MOTOR_PORT = Ports.PORT11 CLAW_MOTOR_REVERSE_POLARITY = False CLAW_MOTOR_TIMEOUT_SECS = 3 CLAW_MOTOR_VELOCITY_PERCENT = 60 # sensor configs TOUCH_LED_PORT = Ports.PORT2 COLOR_SENSOR_PORT = Ports.PORT3 GYRO_SENSOR_PORT = Ports.PORT4 DISTANCE_SENSOR_PORT = Ports.PORT7 BUMPER_SWITCH_PORT = Ports.PORT8 # Controller configs CONTROLLER_DEADBAND = 3 def __init__(self): self.brain = Brain() self.left_motor = \ Motor( self.LEFT_MOTOR_PORT, # index self.LEFT_MOTOR_REVERSE_POLARITY # reverse polarity? ) self.right_motor = \ Motor( self.RIGHT_MOTOR_PORT, # index self.RIGHT_MOTOR_REVERSE_POLARITY # reverse polarity? ) self.drivetrain = \ Drivetrain( self.left_motor, # left_motor self.right_motor, # right_motor self.WHEEL_TRAVEL, # wheel_travel self.TRACK_WIDTH, # track_width self.DISTANCE_UNIT, # distanceUnit self.GEAR_RATIO # gear_ratio ) self.arm_motor = \ Motor( self.ARM_MOTOR_PORT, # index self.ARM_MOTOR_REVERSE_POLARITY # reverse polarity? ) self.claw_motor = \ Motor( self.CLAW_MOTOR_PORT, # index self.CLAW_MOTOR_REVERSE_POLARITY # reverse polarity? ) self.claw_motor.set_timeout( self.CLAW_MOTOR_TIMEOUT_SECS, # time TimeUnits.SEC # timeUnit ) self.touch_led = Touchled(self.TOUCH_LED_PORT) self.color_sensor = \ Colorsensor( self.COLOR_SENSOR_PORT, # index False, # is_grayscale 700 # proximity ) self.gyro_sensor = \ Gyro( self.GYRO_SENSOR_PORT, # index True # calibrate ) self.distance_sensor = Sonar(self.DISTANCE_SENSOR_PORT) self.bumper_switch = Bumper(self.BUMPER_SWITCH_PORT) self.controller = Controller() self.controller.set_deadband(self.CONTROLLER_DEADBAND) def drive_by_controller(self): self.left_motor.spin( DirectionType.FWD, # dir self.controller.axisA.position(), # velocity VelocityUnits.PCT # velocityUnit ) self.right_motor.spin( DirectionType.FWD, # dir self.controller.axisD.position(), # velocity VelocityUnits.PCT # velocityUnit ) def keep_driving_by_controller(self): while True: self.drive_by_controller() def lower_or_raise_arm_by_controller(self): if self.controller.buttonLDown.pressing(): self.arm_motor.spin( DirectionType.REV, # dir self.ARM_MOTOR_VELOCITY_PERCENT, # velocity VelocityUnits.PCT # velocityUnit ) elif self.controller.buttonLUp.pressing(): self.arm_motor.spin( DirectionType.FWD, # dir self.ARM_MOTOR_VELOCITY_PERCENT, # velocity VelocityUnits.PCT # velocityUnit ) else: self.arm_motor.stop(BrakeType.HOLD) def keep_lowering_or_raising_arm_by_controller(self): while True: self.lower_or_raise_arm_by_controller() def grab_or_release_object_by_controller(self): if self.controller.buttonRDown.pressing(): self.claw_motor.spin( DirectionType.REV, # dir self.CLAW_MOTOR_VELOCITY_PERCENT, # velocity VelocityUnits.PCT # velocityUnit ) elif self.controller.buttonRUp.pressing(): self.claw_motor.spin( DirectionType.FWD, # dir self.CLAW_MOTOR_VELOCITY_PERCENT, # velocity VelocityUnits.PCT # velocityUnit ) else: self.claw_motor.stop(BrakeType.HOLD) def keep_grabbing_or_releasing_objects_by_controller(self): while True: self.grab_or_release_object_by_controller() def main(self): while True: self.drive_by_controller() self.lower_or_raise_arm_by_controller() self.grab_or_release_object_by_controller()
Ports.PORT6, # index True # reverse polarity? ) # Elbow motor configs ELBOW_MOTOR = Motor( Ports.PORT1, # index False # reverse polarity? ) # Claw motor configs CLAW_MOTOR = Motor( Ports.PORT4, # index False # reverse polarity? ) CLAW_MOTOR.set_timeout(3, TimeUnits.SEC) CLAW_MOTOR_VELOCITY_PERCENT = 100 # sensor configs BASE_BUMPER_SWITCH = Bumper(Ports.PORT2) ELBOW_BUMPER_SWITCH = Bumper(Ports.PORT9) DISTANCE_SENSOR = Sonar(Ports.PORT8) COLOR_SENSOR = Colorsensor( Ports.PORT12, # index False, # is_grayscale 700 # proximity ) LEFT_TOUCH_LED = Touchled(Ports.PORT5)
# Base Pivot motor BASE_PIVOT_MOTOR = Motor(Ports.PORT10) # Shoulder motor SHOULDER_MOTOR = Motor( Ports.PORT6, True # reverse polarity? ) # Elbow motor ELBOW_MOTOR = Motor(Ports.PORT1) # Claw motor CLAW_MOTOR = Motor(Ports.PORT4) CLAW_MOTOR.set_timeout(3, SECONDS) # Controller CONTROLLER = Controller() CONTROLLER.set_deadband(3) # FUNCTIONS # ========= # function for pivoting the Base def pivot_base_by_controller_left_buttons(): # pivot base right if CONTROLLER.buttonLDown.pressing(): BASE_PIVOT_MOTOR.spin(REVERSE, 100, PERCENT)