Add climber lift buttons for 2022
Change-Id: I96a7848504cb2b9b68cd0683ab3ca79257f7391c
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2022/joystick_reader.cc b/y2022/joystick_reader.cc
index cc2c6e9..a88e552 100644
--- a/y2022/joystick_reader.cc
+++ b/y2022/joystick_reader.cc
@@ -58,6 +58,7 @@
const ButtonLocation kAutoAim(4, 2);
const ButtonLocation kClimberExtend(4, 6);
+const ButtonLocation kClimberIntakes(4, 5);
const ButtonLocation kIntakeFrontOut(4, 10);
const ButtonLocation kIntakeBackOut(4, 9);
@@ -161,8 +162,9 @@
setpoint_fetcher_.Fetch();
// Default to the intakes in
- double intake_front_pos = 1.47;
- double intake_back_pos = 1.47;
+ constexpr double kIntakeUpPosition = 1.47;
+ double intake_front_pos = kIntakeUpPosition;
+ double intake_back_pos = kIntakeUpPosition;
double transfer_roller_front_speed = 0.0;
double transfer_roller_back_speed = 0.0;
std::optional<control_loops::superstructure::RequestedIntake>
@@ -262,6 +264,11 @@
turret_pos = 0.0;
}
+ if (data.IsPressed(kClimberIntakes)) {
+ intake_back_pos = kIntakeUpPosition;
+ intake_front_pos = kIntakePosition;
+ }
+
{
auto builder = superstructure_goal_sender_.MakeBuilder();