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"))
