Add "disable_extend" bool to constants
Change-Id: Ibc839f913aa573655ad27aaeef9635ef2071d883
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2024/control_loops/superstructure/shooter.cc b/y2024/control_loops/superstructure/shooter.cc
index 4421e15..79da980 100644
--- a/y2024/control_loops/superstructure/shooter.cc
+++ b/y2024/control_loops/superstructure/shooter.cc
@@ -181,18 +181,15 @@
const bool disabled = turret_.Correct(turret_goal, position->turret(),
turret_output == nullptr);
+ // Zero out extend goal and position if "disable_extend" is true
collision_avoidance->UpdateGoal(
{.intake_pivot_position = intake_pivot_position,
.turret_position = turret_.estimated_position(),
- .extend_position = extend_position},
- turret_goal->unsafe_goal(), extend_goal);
-
- if (!CatapultRetracted()) {
- altitude_.set_min_position(
- robot_constants_->common()->min_altitude_shooting_angle());
- } else {
- altitude_.clear_min_position();
- }
+ .extend_position =
+ ((!robot_constants_->common()->disable_extend()) ? extend_position
+ : 0.0)},
+ turret_goal->unsafe_goal(),
+ ((!robot_constants_->common()->disable_extend()) ? extend_goal : 0.0));
if (!CatapultRetracted()) {
altitude_.set_min_position(
diff --git a/y2024/control_loops/superstructure/superstructure.cc b/y2024/control_loops/superstructure/superstructure.cc
index d3e98ab..5727546 100644
--- a/y2024/control_loops/superstructure/superstructure.cc
+++ b/y2024/control_loops/superstructure/superstructure.cc
@@ -159,9 +159,12 @@
robot_constants_->common()->extend_set_points();
// Checks if the extend is close enough to the retracted position to be
- // considered ready to accept note from the transfer rollers.
- const bool extend_at_retracted = PositionNear(
- extend_.position(), extend_set_points->retracted(), kExtendThreshold);
+ // considered ready to accept note from the transfer rollers. If disable
+ // extend is triggered, this will autoatically be false.
+ const bool extend_at_retracted =
+ (!robot_constants_->common()->disable_extend() &&
+ PositionNear(extend_.position(), extend_set_points->retracted(),
+ kExtendThreshold));
// Check if the turret is at the position to accept the note from extend
const bool turret_ready_for_load =
@@ -525,10 +528,13 @@
drivetrain_status_fetcher_.Fetch();
+ // Zero out extend position if "disable_extend" is true
const bool collided = collision_avoidance_.IsCollided({
.intake_pivot_position = intake_pivot_.estimated_position(),
.turret_position = shooter_.turret().estimated_position(),
- .extend_position = extend_.estimated_position(),
+ .extend_position = ((!robot_constants_->common()->disable_extend())
+ ? extend_.estimated_position()
+ : 0.0),
});
aos::FlatbufferFixedAllocatorArray<
@@ -635,6 +641,11 @@
output != nullptr ? &output_struct.extend_voltage : nullptr,
status->fbb());
+ // Zero out extend voltage if "disable_extend" is true
+ if (robot_constants_->common()->disable_extend()) {
+ output_struct.extend_voltage = 0.0;
+ }
+
if (output) {
output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
}