Stop intaking when transfer beam break triggered
Signed-off-by: Filip Kujawa <filip.j.kujawa@gmail.com>
Change-Id: I8ce749bd5f44012fc2c7a1014e0555988600d979
diff --git a/y2024/control_loops/superstructure/superstructure.cc b/y2024/control_loops/superstructure/superstructure.cc
index 5727546..af3f9de 100644
--- a/y2024/control_loops/superstructure/superstructure.cc
+++ b/y2024/control_loops/superstructure/superstructure.cc
@@ -51,8 +51,10 @@
robot_constants_->common()->extend(),
robot_constants_->robot()->extend_constants()->zeroing_constants()),
extend_debouncer_(std::chrono::milliseconds(30),
- std::chrono::milliseconds(8)) {
- event_loop->SetRuntimeRealtimePriority(37);
+ std::chrono::milliseconds(8)),
+ transfer_debouncer_(std::chrono::milliseconds(30),
+ std::chrono::milliseconds(8)) {
+ event_loop->SetRuntimeRealtimePriority(30);
}
bool PositionNear(double position, double goal, double threshold) {
@@ -79,6 +81,9 @@
extend_debouncer_.Update(position->extend_beambreak(), timestamp);
const bool extend_beambreak = extend_debouncer_.state();
+ 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();
@@ -221,6 +226,7 @@
unsafe_goal->intake_goal() == IntakeGoal::INTAKE &&
extend_at_retracted) {
state_ = SuperstructureState::INTAKING;
+ note_in_transfer_ = false;
}
extend_goal_location = ExtendStatus::RETRACTED;
@@ -231,7 +237,18 @@
if (extend_beambreak) {
state_ = SuperstructureState::LOADED;
}
- intake_roller_state = IntakeRollerStatus::INTAKING;
+
+ if (transfer_beambreak) {
+ note_in_transfer_ = true;
+ }
+
+ // Once the note is in the transfer, stop the intake rollers
+ if (note_in_transfer_) {
+ intake_roller_state = IntakeRollerStatus::NONE;
+ } else {
+ intake_roller_state = IntakeRollerStatus::INTAKING;
+ }
+
transfer_roller_status = TransferRollerStatus::TRANSFERING_IN;
extend_roller_status = ExtendRollerStatus::TRANSFERING_TO_EXTEND;
extend_goal_location = ExtendStatus::RETRACTED;
diff --git a/y2024/control_loops/superstructure/superstructure.h b/y2024/control_loops/superstructure/superstructure.h
index ce279a6..a1bf841 100644
--- a/y2024/control_loops/superstructure/superstructure.h
+++ b/y2024/control_loops/superstructure/superstructure.h
@@ -73,6 +73,9 @@
NoteGoal requested_note_goal_ = NoteGoal::NONE;
+ // True if the transfer beambreak has been triggered since last intake request
+ bool note_in_transfer_ = false;
+
aos::monotonic_clock::time_point transfer_start_time_ =
aos::monotonic_clock::time_point::min();
@@ -91,6 +94,8 @@
Debouncer extend_debouncer_;
+ Debouncer transfer_debouncer_;
+
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 9dad4ec..e3368e9 100644
--- a/y2024/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2024/control_loops/superstructure/superstructure_lib_test.cc
@@ -66,6 +66,7 @@
event_loop_->MakeFetcher<Output>("/superstructure")),
extend_beambreak_(false),
catapult_beambreak_(false),
+ transfer_beambreak_(false),
intake_pivot_(
new CappedTestPlant(intake_pivot::MakeIntakePivotPlant()),
PositionSensorSimulator(simulated_robot_constants->robot()
@@ -275,6 +276,7 @@
position_builder.add_extend_beambreak(extend_beambreak_);
position_builder.add_catapult_beambreak(catapult_beambreak_);
+ position_builder.add_transfer_beambreak(transfer_beambreak_);
position_builder.add_intake_pivot(intake_pivot_offset);
position_builder.add_catapult(catapult_offset);
position_builder.add_altitude(altitude_offset);
@@ -292,6 +294,10 @@
catapult_beambreak_ = triggered;
}
+ void set_transfer_beambreak(bool triggered) {
+ transfer_beambreak_ = triggered;
+ }
+
AbsoluteEncoderSimulator *intake_pivot() { return &intake_pivot_; }
PotAndAbsoluteEncoderSimulator *catapult() { return &catapult_; }
PotAndAbsoluteEncoderSimulator *altitude() { return &altitude_; }
@@ -312,6 +318,7 @@
bool extend_beambreak_;
bool catapult_beambreak_;
+ bool transfer_beambreak_;
AbsoluteEncoderSimulator intake_pivot_;
PotAndAbsoluteEncoderSimulator climber_;
@@ -945,6 +952,58 @@
EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(), 0.0);
}
+// Make sure we stop intaking once transfer beambreak is triggered
+TEST_F(SuperstructureTest, TransferBeamBreakStopsIntake) {
+ SetEnabled(true);
+
+ WaitUntilZeroed();
+
+ superstructure_plant_.intake_pivot()->InitializePosition(
+ frc971::constants::Range::FromFlatbuffer(
+ simulated_robot_constants_->common()->intake_pivot()->range())
+ .middle());
+
+ WaitUntilZeroed();
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_intake_goal(IntakeGoal::INTAKE);
+ goal_builder.add_intake_pivot(IntakePivotGoal::DOWN);
+ goal_builder.add_note_goal(NoteGoal::NONE);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ RunFor(chrono::seconds(5));
+
+ VerifyNearGoal();
+
+ EXPECT_EQ(superstructure_status_fetcher_->state(),
+ SuperstructureState::INTAKING);
+
+ EXPECT_EQ(superstructure_output_fetcher_->intake_roller_voltage(),
+ simulated_robot_constants_->common()
+ ->intake_roller_voltages()
+ ->intaking());
+
+ superstructure_plant_.set_transfer_beambreak(true);
+
+ RunFor(chrono::seconds(2));
+
+ VerifyNearGoal();
+
+ EXPECT_EQ(superstructure_status_fetcher_->state(),
+ SuperstructureState::INTAKING);
+
+ EXPECT_EQ(superstructure_status_fetcher_->intake_roller(),
+ IntakeRollerStatus::NONE);
+
+ EXPECT_EQ(superstructure_output_fetcher_->intake_roller_voltage(), 0.0);
+}
+
// Tests the full range of activities we need to be doing from loading ->
// shooting
TEST_F(SuperstructureTest, LoadingToShooting) {