Fix all the climber signs and make them all work
Now that the HW exists, let's do it. Zero is all the way down against
the hard stop.
Change-Id: I8661c25e6ce8badab26a6d1b642bcc9d754d4878
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2022/joystick_reader.cc b/y2022/joystick_reader.cc
index 8423f47..f73058b 100644
--- a/y2022/joystick_reader.cc
+++ b/y2022/joystick_reader.cc
@@ -57,6 +57,8 @@
const ButtonLocation kTurret(4, 15);
const ButtonLocation kAutoAim(4, 2);
+const ButtonLocation kClimberExtend(4, 6);
+
const ButtonLocation kIntakeFrontOut(4, 10);
const ButtonLocation kIntakeBackOut(4, 9);
const ButtonLocation kSpit(3, 3);
@@ -169,6 +171,8 @@
double turret_pos = 0.0;
+ double climber_position = 0.01;
+
double catapult_pos = 0.03;
double catapult_speed = 18.0;
double catapult_return_pos = 0.0;
@@ -185,6 +189,12 @@
BlueResetLocalizer();
}
+ if (data.IsPressed(kClimberExtend)) {
+ climber_position = 0.50;
+ } else {
+ climber_position = 0.01;
+ }
+
if (data.IsPressed(kTurret)) {
if (setpoint_fetcher_.get()) {
turret_pos = setpoint_fetcher_->turret();
@@ -273,6 +283,11 @@
*builder.fbb(), catapult_return_pos,
frc971::CreateProfileParameters(*builder.fbb(), 9.0, 50.0));
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ climber_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), climber_position,
+ frc971::CreateProfileParameters(*builder.fbb(), 1.0, 5.0));
+
superstructure::CatapultGoal::Builder catapult_builder =
builder.MakeBuilder<superstructure::CatapultGoal>();
catapult_builder.add_return_position(catapult_return_offset);
@@ -287,6 +302,7 @@
superstructure_goal_builder.add_intake_front(intake_front_offset);
superstructure_goal_builder.add_intake_back(intake_back_offset);
superstructure_goal_builder.add_turret(turret_offset);
+ superstructure_goal_builder.add_climber(climber_offset);
superstructure_goal_builder.add_catapult(catapult_offset);
superstructure_goal_builder.add_fire(fire);