Remove Encoder from Climber
Signed-off-by: Niko Sohmers <nikolai@sohmers.com>
Change-Id: Ia219b303a500c46fdd72aad592053fe81e807f2b
diff --git a/y2024/control_loops/superstructure/superstructure.cc b/y2024/control_loops/superstructure/superstructure.cc
index daede87..6412c27 100644
--- a/y2024/control_loops/superstructure/superstructure.cc
+++ b/y2024/control_loops/superstructure/superstructure.cc
@@ -43,9 +43,6 @@
event_loop->MakeFetcher<aos::JoystickState>("/aos")),
intake_pivot_(robot_constants_->common()->intake_pivot(),
robot_constants_->robot()->intake_constants()),
- climber_(
- robot_constants_->common()->climber(),
- robot_constants_->robot()->climber_constants()->zeroing_constants()),
shooter_(event_loop, robot_constants_),
extend_(
robot_constants_->common()->extend(),
@@ -71,7 +68,6 @@
if (WasReset()) {
AOS_LOG(ERROR, "WPILib reset, restarting\n");
intake_pivot_.Reset();
- climber_.Reset();
shooter_.Reset();
extend_.Reset();
}
@@ -84,45 +80,6 @@
transfer_debouncer_.Update(position->transfer_beambreak(), timestamp);
const bool transfer_beambreak = transfer_debouncer_.state();
- // Handle Climber Goal separately from main superstructure state machine
- double climber_position =
- robot_constants_->common()->climber_set_points()->retract();
-
- double climber_velocity = robot_constants_->common()
- ->climber()
- ->default_profile_params()
- ->max_velocity();
- const double climber_accel = robot_constants_->common()
- ->climber()
- ->default_profile_params()
- ->max_acceleration();
-
- if (unsafe_goal != nullptr) {
- switch (unsafe_goal->climber_goal()) {
- case ClimberGoal::FULL_EXTEND:
- climber_position =
- robot_constants_->common()->climber_set_points()->full_extend();
- // The climber can go reasonably fast when extending out.
- climber_velocity = 0.5;
-
- if (unsafe_goal->slow_climber()) {
- climber_velocity = 0.01;
- }
- break;
- case ClimberGoal::RETRACT:
- climber_position =
- robot_constants_->common()->climber_set_points()->retract();
- // Keep the climber slower while retracting.
- climber_velocity = 0.1;
- break;
- case ClimberGoal::STOWED:
- climber_position =
- robot_constants_->common()->climber_set_points()->stowed();
- // Keep the climber slower while retracting.
- climber_velocity = 0.1;
- }
- }
-
// If we started off preloaded, skip to the ready state.
if (unsafe_goal != nullptr && unsafe_goal->shooter_goal() &&
unsafe_goal->shooter_goal()->preloaded()) {
@@ -554,31 +511,6 @@
: 0.0),
});
- aos::FlatbufferFixedAllocatorArray<
- frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal, 512>
- climber_goal_buffer;
-
- {
- flatbuffers::FlatBufferBuilder *climber_fbb = climber_goal_buffer.fbb();
- flatbuffers::Offset<frc971::ProfileParameters> climber_profile =
- frc971::CreateProfileParameters(*climber_fbb, climber_velocity,
- climber_accel);
-
- climber_goal_buffer.Finish(
- frc971::control_loops::
- CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *climber_fbb, climber_position, climber_profile));
- }
-
- const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
- *climber_goal = &climber_goal_buffer.message();
-
- const flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
- climber_status_offset = climber_.Iterate(
- climber_goal, position->climber(),
- output != nullptr ? &output_struct.climber_voltage : nullptr,
- status->fbb());
-
double max_intake_pivot_position = 0;
double min_intake_pivot_position = 0;
double max_extend_position = 0;
@@ -663,10 +595,11 @@
output_struct.extend_voltage = 0.0;
}
- // Zero out climber voltage if "disable_climber" is true
- if (robot_constants_->robot()->disable_climber()) {
- output_struct.climber_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)));
@@ -674,17 +607,16 @@
Status::Builder status_builder = status->MakeBuilder<Status>();
- const bool zeroed = intake_pivot_.zeroed() && climber_.zeroed() &&
- shooter_.zeroed() && extend_.zeroed();
- const bool estopped = intake_pivot_.estopped() || climber_.estopped() ||
- shooter_.estopped() || extend_.estopped();
+ const bool zeroed =
+ intake_pivot_.zeroed() && shooter_.zeroed() && extend_.zeroed();
+ const bool estopped =
+ intake_pivot_.estopped() || shooter_.estopped() || extend_.estopped();
status_builder.add_zeroed(zeroed);
status_builder.add_estopped(estopped);
status_builder.add_intake_roller(intake_roller_state);
status_builder.add_intake_pivot(intake_pivot_status_offset);
status_builder.add_transfer_roller(transfer_roller_status);
- status_builder.add_climber(climber_status_offset);
status_builder.add_shooter(shooter_status_offset);
status_builder.add_collided(collided);
status_builder.add_extend_roller(extend_roller_status);
diff --git a/y2024/control_loops/superstructure/superstructure.h b/y2024/control_loops/superstructure/superstructure.h
index a1bf841..6286615 100644
--- a/y2024/control_loops/superstructure/superstructure.h
+++ b/y2024/control_loops/superstructure/superstructure.h
@@ -40,10 +40,6 @@
return intake_pivot_;
}
- inline const PotAndAbsoluteEncoderSubsystem &climber() const {
- return climber_;
- }
-
inline const Shooter &shooter() const { return shooter_; }
inline const PotAndAbsoluteEncoderSubsystem &extend() const {
return extend_;
@@ -86,7 +82,6 @@
aos::monotonic_clock::time_point::min();
AbsoluteEncoderSubsystem intake_pivot_;
- PotAndAbsoluteEncoderSubsystem climber_;
Shooter shooter_;
diff --git a/y2024/control_loops/superstructure/superstructure_goal.fbs b/y2024/control_loops/superstructure/superstructure_goal.fbs
index abfd405..c217bcc 100644
--- a/y2024/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2024/control_loops/superstructure/superstructure_goal.fbs
@@ -60,13 +60,23 @@
table Goal {
intake_goal:IntakeGoal = NONE (id: 0);
intake_pivot:IntakePivotGoal = UP (id: 5);
+
+ // Deprecated since climber no longer has an encoder
climber_goal:ClimberGoal (id: 1);
+
+ // Voltage we want to give to climber
+ // Positive voltage is for climber up
+ // Negative voltage is for climber down
+ climber_goal_voltage:double = 0.0 (id: 8);
+
shooter_goal:ShooterGoal (id: 2);
note_goal:NoteGoal (id: 3);
fire: bool (id: 4);
// Tells the climber to go absurdly slow on FULL_EXTEND
- slow_climber: bool = false (id: 6);
+ // Deprecated now because we take care of this through
+ // climber_goal_voltage
+ slow_climber: bool = false (id: 6, deprecated);
// Spit on the extend motors
spit_extend: bool = false (id: 7);
diff --git a/y2024/control_loops/superstructure/superstructure_lib_test.cc b/y2024/control_loops/superstructure/superstructure_lib_test.cc
index a4e47d7..5544fc0 100644
--- a/y2024/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2024/control_loops/superstructure/superstructure_lib_test.cc
@@ -15,7 +15,6 @@
#include "y2024/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "y2024/control_loops/superstructure/altitude/altitude_plant.h"
#include "y2024/control_loops/superstructure/catapult/catapult_plant.h"
-#include "y2024/control_loops/superstructure/climber/climber_plant.h"
#include "y2024/control_loops/superstructure/extend/extend_plant.h"
#include "y2024/control_loops/superstructure/intake_pivot/intake_pivot_plant.h"
#include "y2024/control_loops/superstructure/superstructure.h"
@@ -81,26 +80,6 @@
->intake_constants()
->measured_absolute_position(),
dt_),
- climber_(new CappedTestPlant(climber::MakeClimberPlant()),
- PositionSensorSimulator(simulated_robot_constants->robot()
- ->climber_constants()
- ->zeroing_constants()
- ->one_revolution_distance()),
- {.subsystem_params =
- {simulated_robot_constants->common()->climber(),
- simulated_robot_constants->robot()
- ->climber_constants()
- ->zeroing_constants()},
- .potentiometer_offset = simulated_robot_constants->robot()
- ->climber_constants()
- ->potentiometer_offset()},
- frc971::constants::Range::FromFlatbuffer(
- simulated_robot_constants->common()->climber()->range()),
- simulated_robot_constants->robot()
- ->climber_constants()
- ->zeroing_constants()
- ->measured_absolute_position(),
- dt_),
catapult_(new CappedTestPlant(catapult::MakeCatapultPlant()),
PositionSensorSimulator(simulated_robot_constants->robot()
->catapult_constants()
@@ -185,10 +164,6 @@
frc971::constants::Range::FromFlatbuffer(
simulated_robot_constants->common()->intake_pivot()->range())
.middle());
- climber_.InitializePosition(
- frc971::constants::Range::FromFlatbuffer(
- simulated_robot_constants->common()->climber()->range())
- .middle());
catapult_.InitializePosition(
frc971::constants::Range::FromFlatbuffer(
simulated_robot_constants->common()->catapult()->range())
@@ -213,8 +188,6 @@
superstructure_output_fetcher_->intake_pivot_voltage(),
superstructure_status_fetcher_->intake_pivot());
- climber_.Simulate(superstructure_output_fetcher_->climber_voltage(),
- superstructure_status_fetcher_->climber());
catapult_.Simulate(
superstructure_output_fetcher_->catapult_voltage(),
superstructure_status_fetcher_->shooter()->catapult());
@@ -246,14 +219,8 @@
flatbuffers::Offset<frc971::AbsolutePosition> intake_pivot_offset =
intake_pivot_.encoder()->GetSensorValues(&intake_pivot_builder);
- frc971::PotAndAbsolutePosition::Builder climber_builder =
- builder.MakeBuilder<frc971::PotAndAbsolutePosition>();
-
- flatbuffers::Offset<frc971::PotAndAbsolutePosition> climber_offset =
- climber_.encoder()->GetSensorValues(&climber_builder);
frc971::PotAndAbsolutePosition::Builder catapult_builder =
builder.MakeBuilder<frc971::PotAndAbsolutePosition>();
-
flatbuffers::Offset<frc971::PotAndAbsolutePosition> catapult_offset =
catapult_.encoder()->GetSensorValues(&catapult_builder);
@@ -281,7 +248,6 @@
position_builder.add_catapult(catapult_offset);
position_builder.add_altitude(altitude_offset);
position_builder.add_turret(turret_offset);
- position_builder.add_climber(climber_offset);
position_builder.add_extend(extend_offset);
CHECK_EQ(builder.Send(position_builder.Finish()),
@@ -302,7 +268,6 @@
PotAndAbsoluteEncoderSimulator *catapult() { return &catapult_; }
PotAndAbsoluteEncoderSimulator *altitude() { return &altitude_; }
PotAndAbsoluteEncoderSimulator *turret() { return &turret_; }
- PotAndAbsoluteEncoderSimulator *climber() { return &climber_; }
PotAndAbsoluteEncoderSimulator *extend() { return &extend_; }
@@ -321,7 +286,6 @@
bool transfer_beambreak_;
AbsoluteEncoderSimulator intake_pivot_;
- PotAndAbsoluteEncoderSimulator climber_;
PotAndAbsoluteEncoderSimulator catapult_;
PotAndAbsoluteEncoderSimulator altitude_;
PotAndAbsoluteEncoderSimulator turret_;
@@ -459,26 +423,6 @@
EXPECT_EQ(superstructure_output_fetcher_->intake_roller_voltage(), 0.0);
}
- if (superstructure_goal_fetcher_->has_climber_goal()) {
- double set_point =
- simulated_robot_constants_->common()->climber_set_points()->retract();
-
- if (superstructure_goal_fetcher_->climber_goal() ==
- ClimberGoal::FULL_EXTEND) {
- set_point = simulated_robot_constants_->common()
- ->climber_set_points()
- ->full_extend();
- }
-
- if (superstructure_goal_fetcher_->climber_goal() == ClimberGoal::STOWED) {
- set_point = simulated_robot_constants_->common()
- ->climber_set_points()
- ->stowed();
- }
- EXPECT_NEAR(set_point,
- superstructure_status_fetcher_->climber()->position(), 0.001);
- }
-
if (superstructure_status_fetcher_->has_uncompleted_note_goal()) {
double set_point = simulated_robot_constants_->common()
->extend_set_points()
@@ -623,7 +567,6 @@
shooter_goal_builder.Finish();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_climber_goal(ClimberGoal::RETRACT);
goal_builder.add_shooter_goal(shooter_goal_offset);
goal_builder.add_intake_goal(IntakeGoal::NONE);
goal_builder.add_intake_pivot(IntakePivotGoal::UP);
@@ -675,7 +618,6 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake_pivot(IntakePivotGoal::DOWN);
- goal_builder.add_climber_goal(ClimberGoal::FULL_EXTEND);
goal_builder.add_shooter_goal(shooter_goal_offset);
goal_builder.add_note_goal(NoteGoal::NONE);
@@ -726,7 +668,6 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake_goal(IntakeGoal::INTAKE);
- goal_builder.add_climber_goal(ClimberGoal::FULL_EXTEND);
goal_builder.add_shooter_goal(shooter_goal_offset);
goal_builder.add_note_goal(NoteGoal::AMP);
@@ -772,7 +713,6 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake_goal(IntakeGoal::NONE);
- goal_builder.add_climber_goal(ClimberGoal::RETRACT);
goal_builder.add_shooter_goal(shooter_goal_offset);
goal_builder.add_note_goal(NoteGoal::NONE);
@@ -798,9 +738,6 @@
superstructure_.intake_pivot().state());
EXPECT_EQ(PotAndAbsoluteEncoderSubsystem::State::RUNNING,
- superstructure_.climber().state());
-
- EXPECT_EQ(PotAndAbsoluteEncoderSubsystem::State::RUNNING,
superstructure_.shooter().turret().state());
EXPECT_EQ(PotAndAbsoluteEncoderSubsystem::State::RUNNING,
@@ -1661,8 +1598,6 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_climber_goal(ClimberGoal::STOWED);
-
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
@@ -1675,8 +1610,6 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_climber_goal(ClimberGoal::FULL_EXTEND);
-
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
@@ -1689,8 +1622,6 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_climber_goal(ClimberGoal::RETRACT);
-
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
diff --git a/y2024/control_loops/superstructure/superstructure_position.fbs b/y2024/control_loops/superstructure/superstructure_position.fbs
index b1facba..6905fcf 100644
--- a/y2024/control_loops/superstructure/superstructure_position.fbs
+++ b/y2024/control_loops/superstructure/superstructure_position.fbs
@@ -28,11 +28,14 @@
// True means there is a game piece in the transfer.
transfer_beambreak:bool (id: 4);
- // Values of the encoder and potentiometer at the climber.
+ // Deprecated since climber no longer has an encoder
+ deprecated_climber:frc971.PotAndAbsolutePosition (id: 5, deprecated);
+
+ // Value of the potentiometer at the climber.
// Zero is fully extended, with top of the highest slider aligned with the
// caps on the tubes for the climber.
// Positive is more extended.
- climber:frc971.PotAndAbsolutePosition (id: 5);
+ climber:frc971.RelativePosition (id: 9);
// True if there is a game piece in the catapult
catapult_beambreak:bool (id: 6);
diff --git a/y2024/control_loops/superstructure/superstructure_status.fbs b/y2024/control_loops/superstructure/superstructure_status.fbs
index 0b30edd..7a508dd 100644
--- a/y2024/control_loops/superstructure/superstructure_status.fbs
+++ b/y2024/control_loops/superstructure/superstructure_status.fbs
@@ -140,7 +140,9 @@
transfer_roller:TransferRollerStatus (id: 5);
// Estimated angle and angular velocitiy of the climber.
- climber:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 6);
+ // Deprecated now because climber no longer has an encoder
+ // and climber status is not used
+ climber:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 6, deprecated);
// Status of the subsytems involved in the shooter
shooter:ShooterStatus (id: 7);