Make new climber fully work
Change-Id: Id7884fdf22a180a4a33d174638c188dc1e01ae48
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2024/control_loops/superstructure/superstructure.cc b/y2024/control_loops/superstructure/superstructure.cc
index 6412c27..e22dc80 100644
--- a/y2024/control_loops/superstructure/superstructure.cc
+++ b/y2024/control_loops/superstructure/superstructure.cc
@@ -544,6 +544,18 @@
const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
*extend_goal = &extend_goal_buffer.message();
+ // Ignore climber voltage goal if "disable_climber" is true
+ output_struct.climber_voltage =
+ (!robot_constants_->robot()->disable_climber() && unsafe_goal != nullptr)
+ ? unsafe_goal->climber_goal_voltage()
+ : 0.0;
+
+ if (output) {
+ if (output_struct.climber_voltage != 0.0) {
+ ++climbing_;
+ }
+ }
+
// TODO(max): Change how we handle the collision with the turret and
// intake to be clearer
const flatbuffers::Offset<ShooterStatus> shooter_status_offset =
@@ -563,7 +575,7 @@
&max_extend_position, &min_extend_position,
intake_pivot_.estimated_position(), &max_intake_pivot_position,
&min_intake_pivot_position, requested_note_goal_, status->fbb(),
- timestamp);
+ timestamp, climbing_ > 50);
intake_pivot_.set_min_position(min_intake_pivot_position);
intake_pivot_.set_max_position(max_intake_pivot_position);
@@ -595,12 +607,6 @@
output_struct.extend_voltage = 0.0;
}
- // Ignore climber voltage goal if "disable_climber" is true
- output_struct.climber_voltage =
- (!robot_constants_->robot()->disable_climber() && unsafe_goal != nullptr)
- ? unsafe_goal->climber_goal_voltage()
- : 0.0;
-
if (output) {
output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
}