Go back to IDLE after a bit if we stop intaking
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: Ib7965980b3bffabeb8be9edc21466961e2261fd3
diff --git a/y2024/control_loops/superstructure/superstructure.cc b/y2024/control_loops/superstructure/superstructure.cc
index 56980d5..6274da5 100644
--- a/y2024/control_loops/superstructure/superstructure.cc
+++ b/y2024/control_loops/superstructure/superstructure.cc
@@ -19,6 +19,9 @@
constexpr double kTurretLoadingThreshold = 0.01;
constexpr double kAltitudeLoadingThreshold = 0.01;
+constexpr std::chrono::milliseconds kExtraIntakingTime =
+ std::chrono::milliseconds(500);
+
namespace y2024::control_loops::superstructure {
using ::aos::monotonic_clock;
@@ -61,8 +64,6 @@
const monotonic_clock::time_point timestamp =
event_loop()->context().monotonic_event_time;
- (void)timestamp;
-
if (WasReset()) {
AOS_LOG(ERROR, "WPILib reset, restarting\n");
intake_pivot_.Reset();
@@ -113,6 +114,7 @@
case IntakeGoal::INTAKE:
intake_pivot_position =
robot_constants_->common()->intake_pivot_set_points()->extended();
+ intake_end_time_ = timestamp;
break;
case IntakeGoal::SPIT:
intake_pivot_position =
@@ -179,7 +181,6 @@
catapult_requested_ = false;
break;
case SuperstructureState::INTAKING:
-
// Switch to LOADED state when the extend beambreak is triggered
// meaning the note is loaded in the extend
if (position->extend_beambreak()) {
@@ -195,6 +196,14 @@
catapult_requested_ = true;
}
+ // If we are no longer requesting INTAKE or we are no longer requesting
+ // an INTAKE goal, wait 0.5 seconds then go back to IDLE.
+ if (!(unsafe_goal != nullptr &&
+ unsafe_goal->intake_goal() == IntakeGoal::INTAKE) &&
+ timestamp > intake_end_time_ + kExtraIntakingTime) {
+ state_ = SuperstructureState::IDLE;
+ }
+
break;
case SuperstructureState::LOADED:
if (catapult_requested_ == true) {
diff --git a/y2024/control_loops/superstructure/superstructure.h b/y2024/control_loops/superstructure/superstructure.h
index 963d8c4..868f11f 100644
--- a/y2024/control_loops/superstructure/superstructure.h
+++ b/y2024/control_loops/superstructure/superstructure.h
@@ -74,6 +74,9 @@
aos::monotonic_clock::time_point transfer_start_time_ =
aos::monotonic_clock::time_point::min();
+ aos::monotonic_clock::time_point intake_end_time_ =
+ aos::monotonic_clock::time_point::min();
+
AbsoluteEncoderSubsystem intake_pivot_;
PotAndAbsoluteEncoderSubsystem climber_;
diff --git a/y2024/control_loops/superstructure/superstructure_lib_test.cc b/y2024/control_loops/superstructure/superstructure_lib_test.cc
index e0b9846..8041504 100644
--- a/y2024/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2024/control_loops/superstructure/superstructure_lib_test.cc
@@ -924,6 +924,41 @@
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
+ RunFor(chrono::milliseconds(500));
+
+ // Make sure we're still intaking for 500 ms after we stop giving it an
+ // intaking goal.
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_note_goal(NoteGoal::NONE);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ RunFor(chrono::milliseconds(200));
+
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+
+ EXPECT_EQ(superstructure_status_fetcher_->state(),
+ SuperstructureState::INTAKING);
+ EXPECT_EQ(superstructure_status_fetcher_->intake_roller(),
+ IntakeRollerStatus::INTAKING);
+
+ // Make sure we stop when loaded
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_intake_goal(IntakeGoal::INTAKE);
+ goal_builder.add_note_goal(NoteGoal::NONE);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
superstructure_plant_.set_extend_beambreak(true);
RunFor(chrono::seconds(2));