def handle_spinner_inputs(self, joystick: wpilib.Joystick) -> None: if joystick.getRawButtonPressed(7): self.spinner_controller.run(test=True, task="position") print(f"Spinner Running") if joystick.getRawButtonPressed(9): self.spinner.piston_up() print("Spinner Piston Up") if joystick.getRawButtonPressed(10): self.spinner.piston_down() print("Spinner Piston Down") if joystick.getRawButtonPressed(8): print( f"Detected Colour: {self.spinner_controller.get_current_colour()}" ) print(f"Distance: {self.spinner_controller.get_wheel_dist()}")
def handle_shooter_inputs(self, joystick: wpilib.Joystick): if joystick.getTriggerPressed(): self.shooter_controller.driver_input(True) if joystick.getTriggerReleased(): self.shooter_controller.driver_input(False) if joystick.getRawButtonPressed(5): self.shooter_controller.spin_input()
def handle_chassis_inputs(self, joystick: wpilib.Joystick, gamepad: wpilib.XboxController) -> None: throttle = scale_value(joystick.getThrottle(), 1, -1, 0.1, 1) vx = 3 * throttle * rescale_js(-joystick.getY(), 0.1) vz = 3 * throttle * rescale_js(-joystick.getTwist(), 0.1) self.chassis.drive(vx, vz) if joystick.getRawButtonPressed(3): # TODO reset heading pass
class UltimateAscent(AsyncRobot): def __init__(self): super().__init__() # Create motors and stuff here def robotInit(self): self.driver = Joystick(0) CameraServer.launch('vision.py:main') def robotPeriodic(self): pass def autonomousInit(self): pass def autonomousPeriodic(self): pass def teleopInit(self): pass def teleopPeriodic(self): # Driving linear = -self.driver.getRawAxis(RobotMap.left_y) angular = self.driver.getRawAxis(RobotMap.right_x) RobotMap.driver_component.set_curve(linear, angular) # Square to toggle gear if self.driver.getRawButtonPressed(RobotMap.square): RobotMap.driver_component.toggle_gear() # Shooting (R2) launch_speed = (1.0 + self.driver.getRawAxis(RobotMap.r_2)) / 2 RobotMap.shooter_component.shoot(launch_speed) # X to fire if self.driver.getRawButtonPressed(RobotMap.x): print(RobotMap.shooter_component.is_busy()) if not RobotMap.shooter_component.is_busy(): self.start_command(Shoot()) # Circle to toggle height if self.driver.getRawButtonPressed(RobotMap.circle): RobotMap.shooter_component.toggle_lifter()
def handle_chassis_inputs(self, joystick: wpilib.Joystick, gamepad: wpilib.XboxController) -> None: throttle = scale_value(joystick.getThrottle(), 1, -1, 0.1, 1) vx = 3 * throttle * rescale_js(-joystick.getY(), 0.1) vz = 3 * throttle * rescale_js(-joystick.getTwist(), 0.1) self.chassis.drive(vx, vz) if joystick.getRawButtonPressed(3): self.chassis.reset_odometry( geometry.Pose2d(-3, 0, geometry.Rotation2d( math.pi))) # the starting position on the field self.target_estimator.reset()
def handle_intake_inputs( self, joystick: wpilib.Joystick, gamepad: wpilib.XboxController ) -> None: if joystick.getRawButtonPressed(2): if self.indexer.intaking: self.indexer.disable_intaking() self.indexer.raise_intake() else: self.indexer.enable_intaking() self.indexer.lower_intake() if gamepad.getAButton(): # Dump all balls out the intake to try to clear jam, etc self.indexer.clearing = True else: # Normal operation self.indexer.clearing = False
def handle_intake_inputs(self, joystick: wpilib.Joystick) -> None: if joystick.getRawButtonPressed(6): if self.indexer.intaking: self.indexer.disable_intaking() else: self.indexer.enable_intaking()
class Jessica(AsyncRobot): def __init__(self): super().__init__() # Create motors and stuff here def robotInit(self): self.controller = Joystick(0) self.joystick = Joystick(1) CameraServer.launch('vision.py:main') self.autoChooser = SendableChooser() self.autoChooser.addDefault("switch_scale", switch_scale) # self.autoChooser.addObject("drive_forward", drive_straight) SmartDashboard.putData("Autonomous Mode Chooser", self.autoChooser) self.autoSideChooser = SendableChooser() self.autoSideChooser.addDefault("left", "L") self.autoSideChooser.addObject("right", "R") self.autoSideChooser.addObject("middle", "M") SmartDashboard.putData("Side Chooser", self.autoSideChooser) RobotMap.driver_component.set_low_gear() def robotPeriodic(self): SmartDashboard.putNumber("driver/current_distance", RobotMap.driver_component.current_distance) SmartDashboard.putNumber("driver/left_encoder", RobotMap.driver_component.left_encoder_motor.getSelectedSensorPosition(0)) SmartDashboard.putNumber("driver/right_encoder", RobotMap.driver_component.right_encoder_motor.getSelectedSensorPosition(0)) SmartDashboard.putNumber("driver/gyro", RobotMap.driver_component.driver_gyro.getAngle()) SmartDashboard.putNumber("lifter/current_position", RobotMap.lifter_component.current_position) SmartDashboard.putNumber("lifter/current_elevator_position", RobotMap.lifter_component.current_elevator_position) SmartDashboard.putNumber("lifter/current_carriage_position", RobotMap.lifter_component.current_carriage_position) SmartDashboard.putBoolean("lifter/carriage_top_switch", RobotMap.lifter_component.carriage_top_switch.get()) SmartDashboard.putBoolean("lifter/carriage_bottom_switch", RobotMap.lifter_component.carriage_bottom_switch.get()) SmartDashboard.putBoolean("lifter/elevator_bottom_switch", RobotMap.lifter_component.elevator_bottom_switch.get()) SmartDashboard.putNumber("gripper/gripper_pot", RobotMap.gripper_component.pot.get()) def autonomousInit(self): # Insert decision tree logic here. game_data = self.ds.getGameSpecificMessage() switch_position = game_data[0] scale_position = game_data[1] start_position = self.autoSideChooser.getSelected() aut = self.autoChooser.getSelected() self.start_command(aut(scale_position, switch_position, start_position)) # self.start_command(Turn(45, 1)) def disabledInit(self): RobotMap.driver_component.moving_angular.clear() RobotMap.driver_component.moving_linear.clear() def autonomousPeriodic(self): pass def teleopInit(self): self.man_mode = False self.climb_mode = False # self.start_command(Reset()) def teleopPeriodic(self): # if self.joystick.getRawButtonPressed(1): # p1 left_y = -self.controller.getRawAxis(1) right_x = self.controller.getRawAxis(2) self.start_command(curve_drive(left_y, right_x)) if self.controller.getRawButtonPressed(3): self.start_command(Toggle()) if self.controller.getRawButtonPressed(14): RobotMap.driver_component.toggle_gear() # self.start_command(toggle_gear()) if self.controller.getRawButtonPressed(5): RobotMap.driver_component.set_low_gear() if self.controller.getRawButtonPressed(6): RobotMap.driver_component.set_high_gear() # vision = SmartDashboard.getNumber("vision", 0) # vision_min = SmartDashboard.getNumber("vision_min", 0) # vision_max = SmartDashboard.getNumber("vision_max", 0) # clamped_vision = normalize_range(vision, vision_min, vision_max, -1, 1) # left_vision = max(1 - abs(min(clamped_vision, 0)), 0) # right_vision = abs(max(clamped_vision, 0)) # clamped_vision = min(max(vision, -1), 1) # left_vision = 1 - abs(clamped_vision) # right_vision = abs(clamped_vision) # if not (vision_min == 0 and vision_max == 0): # self.controller.setRumble(Joystick.RumbleType.kLeftRumble, clamped_vision) # self.controller.setRumble(Joystick.RumbleType.kRightRumble, clamped_vision) # p2 # l2 = -normalize_range(self.joystick.getRawAxis(3), -1, 1, 0, 1) # r2 = normalize_range(self.joystick.getRawAxis(4), -1, 1, 0, 1) # speed = r2 + l2 # if self.man_mode: # self.start_command(move_lifter(speed)) right_y = -self.joystick.getRawAxis(5) #up if not self.climb_mode: if self.joystick.getPOV() == 0: self.start_command(move_lifter(1)) self.man_mode = True elif self.joystick.getPOV() == 180: self.start_command(move_lifter(-1)) self.man_mode = True elif self.man_mode: self.start_command(move_lifter(0)) else: if self.joystick.getPOV() == 0: self.start_command(lock_carriage_move_elevator(1)) self.man_mode = True elif self.joystick.getPOV() == 180: self.start_command(lock_carriage_move_elevator(right_y)) self.man_mode = True elif self.man_mode: self.start_command(lock_carriage_move_elevator(right_y)) l1 = 5 r1 = 6 square = 1 x = 2 circle = 3 triangle = 4 touchpad = 14 if self.joystick.getRawButtonPressed(touchpad): self.climb_mode = not self.climb_mode if self.climb_mode: self.man_mode = True self.start_command(LiftTo("up")) if self.joystick.getRawButton(7) and self.climb_mode: self.start_command(climb()) if not self.joystick.getRawButton(7) and self.climb_mode: self.start_command(stop()) g_speed = 0.0 if self.joystick.getRawButton(square): g_speed = -1.0 if self.joystick.getRawButton(x): g_speed = 1.0 if self.joystick.getRawButton(circle): g_speed = -0.50 self.start_command(move_left_right(g_speed)) if self.joystick.getRawButtonPressed(triangle): self.start_command(toggle_spread()) if self.joystick.getRawButtonPressed(r1) and not self.climb_mode: self.start_command(move_to_position_instant("scale_high")) self.man_mode = False if self.joystick.getRawButtonPressed(l1) and not self.climb_mode: self.start_command(move_to_position_instant("floor")) self.man_mode = False # if self.joystick.getRawButtonPressed(touchpad): # self.man_mode = not self.man_mode options = 10 if self.joystick.getRawButtonPressed(options): RobotMap.driver_component.reset_drive_sensors() share = 9 if self.joystick.getRawButtonPressed(share): self.start_command(Reset()) if self.joystick.getRawButton(8): self.start_command(LiftTo("up"))