Add "disable_extend" bool to constants
Change-Id: Ibc839f913aa573655ad27aaeef9635ef2071d883
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2024/constants/common.json b/y2024/constants/common.json
index 25fae23..65fe20e 100644
--- a/y2024/constants/common.json
+++ b/y2024/constants/common.json
@@ -269,5 +269,6 @@
"ignore_targets": {
"red": [1, 2, 5, 6, 9, 10, 11, 12, 13, 14, 15, 16],
"blue": [1, 2, 5, 6, 9, 10, 11, 12, 13, 14, 15, 16]
- }
+ },
+ "disable_extend": false
}
diff --git a/y2024/constants/constants.fbs b/y2024/constants/constants.fbs
index c679128..1554f4c 100644
--- a/y2024/constants/constants.fbs
+++ b/y2024/constants/constants.fbs
@@ -197,6 +197,7 @@
altitude_avoid_extend_collision_position: double (id: 28);
autonomous_mode:AutonomousMode (id: 26);
ignore_targets:IgnoreTargets (id: 27);
+ disable_extend:bool (id: 29);
}
table Constants {
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)));
}
diff --git a/y2024/wpilib_interface.cc b/y2024/wpilib_interface.cc
index b67b07e..6b2acc4 100644
--- a/y2024/wpilib_interface.cc
+++ b/y2024/wpilib_interface.cc
@@ -492,10 +492,13 @@
7, true, "rio", &rio_signal_registry,
current_limits->climber_stator_current_limit(),
current_limits->climber_supply_current_limit());
- std::shared_ptr<TalonFX> extend = std::make_shared<TalonFX>(
- 12, false, "Drivetrain Bus", &canivore_signal_registry,
- current_limits->extend_stator_current_limit(),
- current_limits->extend_supply_current_limit());
+ std::shared_ptr<TalonFX> extend =
+ (robot_constants->common()->disable_extend())
+ ? nullptr
+ : std::make_shared<TalonFX>(
+ 12, false, "Drivetrain Bus", &canivore_signal_registry,
+ current_limits->extend_stator_current_limit(),
+ current_limits->extend_supply_current_limit());
std::shared_ptr<TalonFX> intake_roller = std::make_shared<TalonFX>(
8, false, "rio", &rio_signal_registry,
current_limits->intake_roller_stator_current_limit(),
@@ -549,7 +552,9 @@
for (auto talonfx :
{intake_pivot, turret, altitude, catapult_one, catapult_two, extend}) {
- canivore_talonfxs.push_back(talonfx);
+ if (talonfx != nullptr) {
+ canivore_talonfxs.push_back(talonfx);
+ }
}
for (auto talonfx : {intake_roller, transfer_roller, climber, extend_roller,
@@ -612,8 +617,10 @@
catapult_two->SerializePosition(
superstructure_can_builder->add_catapult_two(),
control_loops::superstructure::catapult::kOutputRatio);
- extend->SerializePosition(superstructure_can_builder->add_extend(),
- superstructure::extend::kOutputRatio);
+ if (extend != nullptr) {
+ extend->SerializePosition(superstructure_can_builder->add_extend(),
+ superstructure::extend::kOutputRatio);
+ }
superstructure_can_builder->set_status(static_cast<int>(status));
superstructure_can_builder.CheckOk(superstructure_can_builder.Send());
@@ -682,8 +689,10 @@
output.turret_voltage());
talonfx_map.find("climber")->second->WriteVoltage(
output.climber_voltage());
- talonfx_map.find("extend")->second->WriteVoltage(
- output.extend_voltage());
+ if (talonfx_map.find("extend") != talonfx_map.end()) {
+ talonfx_map.find("extend")->second->WriteVoltage(
+ output.extend_voltage());
+ }
talonfx_map.find("intake_roller")
->second->WriteVoltage(output.intake_roller_voltage());
talonfx_map.find("transfer_roller")
@@ -705,7 +714,9 @@
can_superstructure_writer.add_talonfx("catapult_two", catapult_two);
can_superstructure_writer.add_talonfx("turret", turret);
can_superstructure_writer.add_talonfx("climber", climber);
- can_superstructure_writer.add_talonfx("extend", extend);
+ if (!robot_constants->common()->disable_extend()) {
+ can_superstructure_writer.add_talonfx("extend", extend);
+ }
can_superstructure_writer.add_talonfx("intake_roller", intake_roller);
can_superstructure_writer.add_talonfx("transfer_roller", transfer_roller);
can_superstructure_writer.add_talonfx("extend_roller", extend_roller);