Add Climber Superstructure
Signed-off-by: Filip Kujawa <filip.j.kujawa@gmail.com>
Change-Id: Ic2ef4e2fd7c7445402617ded6d7bab19329b2fea
diff --git a/y2024/constants/common.json b/y2024/constants/common.json
index c632afb..ec46723 100644
--- a/y2024/constants/common.json
+++ b/y2024/constants/common.json
@@ -54,9 +54,9 @@
"transfer_out": -12.0
},
"climber_set_points": {
- "full_extend": 0.0,
- "half_extend": 0.0,
- "retract": 0.0
+ "full_extend": 0.8,
+ "half_extend": 0.6,
+ "retract": 0.2
},
"climber": {
"zeroing_voltage": 3.0,
@@ -70,10 +70,10 @@
"max_acceleration": 30.0
},
"range": {
- "lower_hard": -0.01,
- "upper_hard": 0.5,
- "lower": 0.003,
- "upper": 0.4
+ "lower_hard": 0.1,
+ "upper_hard": 2.01,
+ "lower": 0.2,
+ "upper": 2.0
},
"loop": {% include 'y2024/control_loops/superstructure/climber/integral_climber_plant.json' %}
}
diff --git a/y2024/control_loops/superstructure/superstructure.cc b/y2024/control_loops/superstructure/superstructure.cc
index 566c402..18db6d6 100644
--- a/y2024/control_loops/superstructure/superstructure.cc
+++ b/y2024/control_loops/superstructure/superstructure.cc
@@ -33,7 +33,10 @@
transfer_goal_(TransferRollerGoal::NONE),
intake_pivot_(
robot_constants_->common()->intake_pivot(),
- robot_constants_->robot()->intake_constants()->zeroing_constants()) {
+ robot_constants_->robot()->intake_constants()->zeroing_constants()),
+ climber_(
+ robot_constants_->common()->climber(),
+ robot_constants_->robot()->climber_constants()->zeroing_constants()) {
event_loop->SetRuntimeRealtimePriority(30);
}
@@ -50,6 +53,7 @@
if (WasReset()) {
AOS_LOG(ERROR, "WPILib reset, restarting\n");
intake_pivot_.Reset();
+ climber_.Reset();
}
OutputT output_struct;
@@ -109,6 +113,28 @@
break;
}
+ double climber_position =
+ robot_constants_->common()->climber_set_points()->retract();
+
+ if (unsafe_goal != nullptr) {
+ switch (unsafe_goal->climber_goal()) {
+ case ClimberGoal::FULL_EXTEND:
+ climber_position =
+ robot_constants_->common()->climber_set_points()->full_extend();
+ break;
+ case ClimberGoal::HALF_EXTEND:
+ climber_position =
+ robot_constants_->common()->climber_set_points()->half_extend();
+ break;
+ case ClimberGoal::RETRACT:
+ climber_position =
+ robot_constants_->common()->climber_set_points()->retract();
+ break;
+ default:
+ break;
+ }
+ }
+
if (joystick_state_fetcher_.Fetch() &&
joystick_state_fetcher_->has_alliance()) {
alliance_ = joystick_state_fetcher_->alliance();
@@ -133,20 +159,38 @@
output != nullptr ? &output_struct.intake_pivot_voltage : nullptr,
status->fbb());
+ aos::FlatbufferFixedAllocatorArray<
+ frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal, 512>
+ climber_goal_buffer;
+
+ climber_goal_buffer.Finish(
+ frc971::control_loops::CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *climber_goal_buffer.fbb(), climber_position));
+
+ 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());
+
if (output) {
output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
}
Status::Builder status_builder = status->MakeBuilder<Status>();
- const bool zeroed = intake_pivot_.zeroed();
- const bool estopped = intake_pivot_.estopped();
+ const bool zeroed = intake_pivot_.zeroed() && climber_.zeroed();
+ const bool estopped = intake_pivot_.estopped() || climber_.estopped();
status_builder.add_zeroed(zeroed);
status_builder.add_estopped(estopped);
status_builder.add_intake_roller_state(intake_roller_state);
status_builder.add_intake_pivot_state(intake_pivot_status_offset);
status_builder.add_transfer_roller_state(transfer_roller_state);
+ status_builder.add_climber_state(climber_status_offset);
(void)status->Send(status_builder.Finish());
}
diff --git a/y2024/control_loops/superstructure/superstructure.h b/y2024/control_loops/superstructure/superstructure.h
index 67a8328..88db2e2 100644
--- a/y2024/control_loops/superstructure/superstructure.h
+++ b/y2024/control_loops/superstructure/superstructure.h
@@ -34,6 +34,10 @@
return intake_pivot_;
}
+ inline const PotAndAbsoluteEncoderSubsystem &climber() const {
+ return climber_;
+ }
+
double robot_velocity() const;
protected:
@@ -53,7 +57,7 @@
TransferRollerGoal transfer_goal_;
PotAndAbsoluteEncoderSubsystem intake_pivot_;
-
+ PotAndAbsoluteEncoderSubsystem climber_;
DISALLOW_COPY_AND_ASSIGN(Superstructure);
};
diff --git a/y2024/control_loops/superstructure/superstructure_lib_test.cc b/y2024/control_loops/superstructure/superstructure_lib_test.cc
index 710de4b..8020a4d 100644
--- a/y2024/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2024/control_loops/superstructure/superstructure_lib_test.cc
@@ -12,6 +12,7 @@
#include "frc971/zeroing/absolute_encoder.h"
#include "y2024/constants/simulated_constants_sender.h"
#include "y2024/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2024/control_loops/superstructure/climber/climber_plant.h"
#include "y2024/control_loops/superstructure/intake_pivot/intake_pivot_plant.h"
#include "y2024/control_loops/superstructure/superstructure.h"
@@ -72,11 +73,35 @@
->intake_constants()
->zeroing_constants()
->measured_absolute_position(),
- dt_) {
+ 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_) {
intake_pivot_.InitializePosition(
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());
phased_loop_handle_ = event_loop_->AddPhasedLoop(
[this](int) {
// Skip this the first time.
@@ -87,6 +112,9 @@
intake_pivot_.Simulate(
superstructure_output_fetcher_->intake_pivot_voltage(),
superstructure_status_fetcher_->intake_pivot_state());
+
+ climber_.Simulate(superstructure_output_fetcher_->climber_voltage(),
+ superstructure_status_fetcher_->climber_state());
}
first_ = false;
SendPositionMessage();
@@ -104,11 +132,17 @@
flatbuffers::Offset<frc971::PotAndAbsolutePosition> 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);
+
Position::Builder position_builder = builder.MakeBuilder<Position>();
position_builder.add_transfer_beambreak(transfer_beambreak_);
position_builder.add_intake_pivot(intake_pivot_offset);
+ position_builder.add_climber(climber_offset);
CHECK_EQ(builder.Send(position_builder.Finish()),
aos::RawSender::Error::kOk);
}
@@ -119,6 +153,8 @@
PotAndAbsoluteEncoderSimulator *intake_pivot() { return &intake_pivot_; }
+ PotAndAbsoluteEncoderSimulator *climber() { return &climber_; }
+
private:
::aos::EventLoop *event_loop_;
const chrono::nanoseconds dt_;
@@ -131,6 +167,8 @@
bool transfer_beambreak_;
PotAndAbsoluteEncoderSimulator intake_pivot_;
+ PotAndAbsoluteEncoderSimulator climber_;
+
bool first_ = true;
};
@@ -211,6 +249,27 @@
IntakeRollerState::NONE) {
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();
+ } else if (superstructure_goal_fetcher_->climber_goal() ==
+ ClimberGoal::HALF_EXTEND) {
+ set_point = simulated_robot_constants_->common()
+ ->climber_set_points()
+ ->half_extend();
+ }
+
+ EXPECT_NEAR(set_point,
+ superstructure_status_fetcher_->climber_state()->position(),
+ 0.001);
+ }
}
void CheckIfZeroed() {
@@ -294,12 +353,11 @@
auto builder = superstructure_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
-
goal_builder.add_intake_pivot_goal(IntakePivotGoal::RETRACTED);
+ goal_builder.add_climber_goal(ClimberGoal::RETRACT);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
-
RunFor(chrono::seconds(10));
EXPECT_TRUE(superstructure_output_fetcher_.Fetch());
@@ -314,13 +372,17 @@
frc971::constants::Range::FromFlatbuffer(
simulated_robot_constants_->common()->intake_pivot()->range())
.middle());
+ superstructure_plant_.climber()->InitializePosition(
+ frc971::constants::Range::FromFlatbuffer(
+ simulated_robot_constants_->common()->climber()->range())
+ .lower);
WaitUntilZeroed();
{
auto builder = superstructure_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
-
goal_builder.add_intake_pivot_goal(IntakePivotGoal::EXTENDED);
+ goal_builder.add_climber_goal(ClimberGoal::FULL_EXTEND);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
@@ -343,8 +405,8 @@
auto builder = superstructure_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
-
goal_builder.add_intake_pivot_goal(IntakePivotGoal::EXTENDED);
+ goal_builder.add_climber_goal(ClimberGoal::FULL_EXTEND);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
@@ -357,8 +419,8 @@
auto builder = superstructure_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
-
goal_builder.add_intake_pivot_goal(IntakePivotGoal::RETRACTED);
+ goal_builder.add_climber_goal(ClimberGoal::RETRACT);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
@@ -375,6 +437,9 @@
EXPECT_EQ(PotAndAbsoluteEncoderSubsystem::State::RUNNING,
superstructure_.intake_pivot().state());
+
+ EXPECT_EQ(PotAndAbsoluteEncoderSubsystem::State::RUNNING,
+ superstructure_.climber().state());
}
// Tests that running disabled works
@@ -383,6 +448,65 @@
CheckIfZeroed();
}
+// Tests Climber in multiple scenarios
+TEST_F(SuperstructureTest, ClimberTest) {
+ SetEnabled(true);
+ WaitUntilZeroed();
+
+ superstructure_plant_.climber()->InitializePosition(
+ frc971::constants::Range::FromFlatbuffer(
+ simulated_robot_constants_->common()->climber()->range())
+ .middle());
+
+ WaitUntilZeroed();
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ 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);
+ }
+
+ RunFor(chrono::seconds(5));
+
+ VerifyNearGoal();
+
+ WaitUntilZeroed();
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_climber_goal(ClimberGoal::HALF_EXTEND);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ RunFor(chrono::seconds(5));
+
+ VerifyNearGoal();
+
+ WaitUntilZeroed();
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ 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);
+ }
+
+ RunFor(chrono::seconds(5));
+
+ VerifyNearGoal();
+}
+
// Tests intake and transfer in multiple scenarios
TEST_F(SuperstructureTest, IntakeGoal) {
SetEnabled(true);