Lock-in decision on trap/amp when moving extend
Signed-off-by: Niko Sohmers <nikolai@sohmers.com>
Change-Id: I2db1875b8816b742e8fe03f88ba33d0f8baa7512
diff --git a/y2024/control_loops/superstructure/superstructure.cc b/y2024/control_loops/superstructure/superstructure.cc
index 495f8ad..1c37017 100644
--- a/y2024/control_loops/superstructure/superstructure.cc
+++ b/y2024/control_loops/superstructure/superstructure.cc
@@ -100,7 +100,7 @@
if (state_ != SuperstructureState::READY &&
state_ != SuperstructureState::FIRING) {
state_ = SuperstructureState::READY;
- catapult_requested_ = true;
+ requested_note_goal_ = NoteGoal::CATAPULT;
}
}
@@ -143,10 +143,40 @@
const bool extend_at_retracted = 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 =
+ PositionNear(shooter_.turret().estimated_position(),
+ robot_constants_->common()->turret_loading_position(),
+ kTurretLoadingThreshold);
+
+ // Check if turret is at the right position for the extend to extend.
+ const bool turret_ready_for_extend_move = PositionNear(
+ shooter_.turret().estimated_position(),
+ robot_constants_->common()->turret_avoid_extend_collision_position(),
+ kTurretLoadingThreshold);
+
+ // Check if the altitude is at the position to accept the note from
+ // extend
+ const bool altitude_ready_for_load =
+ PositionNear(shooter_.altitude().estimated_position(),
+ robot_constants_->common()->altitude_loading_position(),
+ kAltitudeLoadingThreshold);
+
+ // Check if the extend is at the position to load the catapult
+ const bool extend_ready_for_catapult_transfer = PositionNear(
+ extend_.position(), extend_set_points->catapult(), kExtendThreshold);
+
// If true, the turret should be moved to the position to avoid collision with
// the extend.
bool move_turret_to_standby = false;
+ // Only update the reuested note goal to the first goal that is requested by
+ // the manipulator
+ if (unsafe_goal != nullptr && unsafe_goal->note_goal() != NoteGoal::NONE &&
+ requested_note_goal_ == NoteGoal::NONE) {
+ requested_note_goal_ = unsafe_goal->note_goal();
+ }
+
// Superstructure state machine:
// 1. IDLE. The intake is retracted and there is no note in the robot.
// Wait for a intake goal to switch state to INTAKING if the extend is ready
@@ -170,9 +200,10 @@
// 7. FIRING. The note is being fired, either from the extend or the catapult.
// Switch state back to IDLE when the note is fired.
- std::optional<bool> turret_ready_for_extend_move;
switch (state_) {
case SuperstructureState::IDLE:
+ requested_note_goal_ = NoteGoal::NONE;
+
if (unsafe_goal != nullptr &&
unsafe_goal->intake_goal() == IntakeGoal::INTAKE &&
extend_at_retracted) {
@@ -212,104 +243,63 @@
state_ = SuperstructureState::IDLE;
}
- if (catapult_requested_ == true) {
- state_ = SuperstructureState::MOVING;
- break;
- }
-
- if (unsafe_goal != nullptr &&
- unsafe_goal->note_goal() != NoteGoal::NONE) {
- // If the goal is AMP or TRAP, check if the turret is in a position to
- // avoid collision when the extend moves.
- if (unsafe_goal->note_goal() == NoteGoal::AMP ||
- unsafe_goal->note_goal() == NoteGoal::TRAP) {
- turret_ready_for_extend_move =
- PositionNear(shooter_.turret().estimated_position(),
- robot_constants_->common()
- ->turret_avoid_extend_collision_position(),
- kTurretLoadingThreshold);
+ switch (requested_note_goal_) {
+ case NoteGoal::NONE:
+ break;
+ case NoteGoal::CATAPULT:
+ state_ = SuperstructureState::MOVING;
+ transfer_roller_status = TransferRollerStatus::TRANSFERING_IN;
+ break;
+ case NoteGoal::TRAP:
+ [[fallthrough]];
+ case NoteGoal::AMP:
transfer_roller_status = TransferRollerStatus::EXTEND_MOVING;
-
- if (turret_ready_for_extend_move.value()) {
+ if (turret_ready_for_extend_move) {
state_ = SuperstructureState::MOVING;
} else {
move_turret_to_standby = true;
}
- } else if (unsafe_goal->note_goal() == NoteGoal::CATAPULT) {
- // If catapult is requested, switch to MOVING state
- state_ = SuperstructureState::MOVING;
- }
+ break;
}
extend_goal = ExtendStatus::RETRACTED;
- if (!catapult_requested_ && unsafe_goal != nullptr &&
- unsafe_goal->note_goal() == NoteGoal::CATAPULT) {
- catapult_requested_ = true;
- }
break;
case SuperstructureState::MOVING:
transfer_roller_status = TransferRollerStatus::EXTEND_MOVING;
-
- if (catapult_requested_) {
- extend_goal = ExtendStatus::CATAPULT;
-
- // Check if the extend is at the position to load the catapult
- bool extend_ready_for_catapult_transfer =
- PositionNear(extend_.position(), extend_set_points->catapult(),
- kExtendThreshold);
-
- // Check if the turret is at the position to accept the note from extend
- bool turret_ready_for_load =
- PositionNear(shooter_.turret().estimated_position(),
- robot_constants_->common()->turret_loading_position(),
- kTurretLoadingThreshold);
-
- // Check if the altitude is at the position to accept the note from
- // extend
- bool altitude_ready_for_load = PositionNear(
- shooter_.altitude().estimated_position(),
- robot_constants_->common()->altitude_loading_position(),
- kAltitudeLoadingThreshold);
-
- if (extend_ready_for_catapult_transfer && turret_ready_for_load &&
- altitude_ready_for_load) {
- state_ = SuperstructureState::LOADING_CATAPULT;
- }
- } else {
- if (unsafe_goal != nullptr) {
- switch (unsafe_goal->note_goal()) {
- case NoteGoal::AMP:
- extend_goal = ExtendStatus::AMP;
- move_turret_to_standby = true;
- // Check if the extend is at the AMP position and if it is
- // switch to READY state
- if (PositionNear(extend_.position(), extend_set_points->amp(),
- kExtendThreshold)) {
- state_ = SuperstructureState::READY;
- }
- break;
- case NoteGoal::TRAP:
- extend_goal = ExtendStatus::TRAP;
- move_turret_to_standby = true;
- // Check if the extend is at the TRAP position and if it is
- // switch to READY state
- if (PositionNear(extend_.position(), extend_set_points->trap(),
- kExtendThreshold)) {
- state_ = SuperstructureState::READY;
- }
- break;
- case NoteGoal::NONE:
- extend_goal = ExtendStatus::RETRACTED;
- move_turret_to_standby = true;
- if (extend_at_retracted) {
- state_ = SuperstructureState::LOADED;
- }
- break;
- case NoteGoal::CATAPULT:
- catapult_requested_ = true;
- extend_goal = ExtendStatus::CATAPULT;
- break;
+ switch (requested_note_goal_) {
+ case NoteGoal::NONE:
+ extend_goal = ExtendStatus::RETRACTED;
+ move_turret_to_standby = true;
+ if (extend_at_retracted) {
+ state_ = SuperstructureState::LOADED;
}
- }
+ break;
+ case NoteGoal::CATAPULT:
+ extend_goal = ExtendStatus::CATAPULT;
+ if (extend_ready_for_catapult_transfer && turret_ready_for_load &&
+ altitude_ready_for_load) {
+ state_ = SuperstructureState::LOADING_CATAPULT;
+ }
+ break;
+ case NoteGoal::TRAP:
+ extend_goal = ExtendStatus::TRAP;
+ move_turret_to_standby = true;
+ // Check if the extend is at the TRAP position and if it is
+ // switch to READY state
+ if (PositionNear(extend_.position(), extend_set_points->trap(),
+ kExtendThreshold)) {
+ state_ = SuperstructureState::READY;
+ }
+ break;
+ case NoteGoal::AMP:
+ extend_goal = ExtendStatus::AMP;
+ move_turret_to_standby = true;
+ // Check if the extend is at the AMP position and if it is
+ // switch to READY state
+ if (PositionNear(extend_.position(), extend_set_points->amp(),
+ kExtendThreshold)) {
+ state_ = SuperstructureState::READY;
+ }
+ break;
}
extend_moving = true;
@@ -332,55 +322,55 @@
state_ = SuperstructureState::FIRING;
}
- if (catapult_requested_) {
- extend_goal = ExtendStatus::CATAPULT;
- } else {
- if (unsafe_goal != nullptr) {
- if (unsafe_goal->note_goal() == NoteGoal::AMP) {
- extend_goal = ExtendStatus::AMP;
- move_turret_to_standby = true;
- } else if (unsafe_goal->note_goal() == NoteGoal::TRAP) {
- extend_goal = ExtendStatus::TRAP;
- move_turret_to_standby = true;
- } else {
- extend_goal = ExtendStatus::RETRACTED;
- extend_moving = true;
- state_ = SuperstructureState::MOVING;
- }
- }
+ switch (requested_note_goal_) {
+ case NoteGoal::NONE:
+ extend_goal = ExtendStatus::RETRACTED;
+ extend_moving = true;
+ state_ = SuperstructureState::MOVING;
+ break;
+ case NoteGoal::CATAPULT:
+ extend_goal = ExtendStatus::CATAPULT;
+ break;
+ case NoteGoal::TRAP:
+ extend_goal = ExtendStatus::TRAP;
+ move_turret_to_standby = true;
+ break;
+ case NoteGoal::AMP:
+ extend_goal = ExtendStatus::AMP;
+ move_turret_to_standby = true;
+ break;
}
-
break;
case SuperstructureState::FIRING:
- if (catapult_requested_) {
- extend_goal = ExtendStatus::CATAPULT;
+ switch (requested_note_goal_) {
+ case NoteGoal::NONE:
- // Reset the state to IDLE when the game piece is fired from the
- // catapult. We consider the game piece to be fired from the catapult
- // when the catapultbeambreak is no longer triggered.
- if (!position->catapult_beambreak()) {
- state_ = SuperstructureState::IDLE;
- }
- } else {
- move_turret_to_standby = true;
- if (unsafe_goal != nullptr &&
- unsafe_goal->note_goal() == NoteGoal::AMP) {
- extend_roller_status = ExtendRollerStatus::SCORING_IN_AMP;
- extend_goal = ExtendStatus::AMP;
- } else if (unsafe_goal != nullptr &&
- unsafe_goal->note_goal() == NoteGoal::TRAP) {
+ break;
+ case NoteGoal::CATAPULT:
+ extend_goal = ExtendStatus::CATAPULT;
+ // Reset the state to IDLE when the game piece is fired from the
+ // catapult. We consider the game piece to be fired from the catapult
+ // when the catapultbeambreak is no longer triggered.
+ if (!position->catapult_beambreak()) {
+ state_ = SuperstructureState::IDLE;
+ }
+ break;
+ case NoteGoal::TRAP:
extend_roller_status = ExtendRollerStatus::SCORING_IN_TRAP;
extend_goal = ExtendStatus::TRAP;
- }
-
- // Reset the state to IDLE when the game piece is fired from the extend.
- // We consider the game piece to be fired from the extend when
- // the extend beambreak is no longer triggered and the fire button is
- // released.
- if (!position->extend_beambreak() && unsafe_goal != nullptr &&
- !unsafe_goal->fire()) {
- state_ = SuperstructureState::IDLE;
- }
+ if (!position->extend_beambreak() && unsafe_goal != nullptr &&
+ !unsafe_goal->fire()) {
+ state_ = SuperstructureState::IDLE;
+ }
+ break;
+ case NoteGoal::AMP:
+ extend_roller_status = ExtendRollerStatus::SCORING_IN_AMP;
+ extend_goal = ExtendStatus::AMP;
+ if (!position->extend_beambreak() && unsafe_goal != nullptr &&
+ !unsafe_goal->fire()) {
+ state_ = SuperstructureState::IDLE;
+ }
+ break;
}
break;
}
@@ -482,6 +472,23 @@
break;
}
+ NoteStatus uncompleted_note_goal_status = NoteStatus::NONE;
+
+ switch (requested_note_goal_) {
+ case NoteGoal::NONE:
+ uncompleted_note_goal_status = NoteStatus::NONE;
+ break;
+ case NoteGoal::CATAPULT:
+ uncompleted_note_goal_status = NoteStatus::CATAPULT;
+ break;
+ case NoteGoal::AMP:
+ uncompleted_note_goal_status = NoteStatus::AMP;
+ break;
+ case NoteGoal::TRAP:
+ uncompleted_note_goal_status = NoteStatus::TRAP;
+ break;
+ }
+
// Set the extend status based on the state machine output
// If the extend is moving, the status is MOVING, otherwise it is the same
// as extend_status
@@ -611,11 +618,8 @@
status_builder.add_extend_status(extend_status);
status_builder.add_extend(extend_status_offset);
status_builder.add_state(state_);
+ status_builder.add_uncompleted_note_goal(uncompleted_note_goal_status);
status_builder.add_extend_ready_for_transfer(extend_at_retracted);
- if (turret_ready_for_extend_move) {
- status_builder.add_turret_ready_for_extend_move(
- turret_ready_for_extend_move.value());
- }
(void)status->Send(status_builder.Finish());
}
diff --git a/y2024/control_loops/superstructure/superstructure.h b/y2024/control_loops/superstructure/superstructure.h
index 868f11f..e1856a3 100644
--- a/y2024/control_loops/superstructure/superstructure.h
+++ b/y2024/control_loops/superstructure/superstructure.h
@@ -71,6 +71,8 @@
SuperstructureState state_ = SuperstructureState::IDLE;
+ NoteGoal requested_note_goal_ = NoteGoal::NONE;
+
aos::monotonic_clock::time_point transfer_start_time_ =
aos::monotonic_clock::time_point::min();
diff --git a/y2024/control_loops/superstructure/superstructure_lib_test.cc b/y2024/control_loops/superstructure/superstructure_lib_test.cc
index 5c8676d..487a073 100644
--- a/y2024/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2024/control_loops/superstructure/superstructure_lib_test.cc
@@ -403,8 +403,10 @@
0.03);
if (superstructure_goal_fetcher_->has_shooter_goal() &&
- superstructure_goal_fetcher_->note_goal() != NoteGoal::AMP &&
- superstructure_goal_fetcher_->note_goal() != NoteGoal::TRAP) {
+ superstructure_status_fetcher_->uncompleted_note_goal() !=
+ NoteStatus::AMP &&
+ superstructure_status_fetcher_->uncompleted_note_goal() !=
+ NoteStatus::TRAP) {
if (superstructure_goal_fetcher_->shooter_goal()->has_turret_position() &&
!superstructure_goal_fetcher_->shooter_goal()->auto_aim()) {
EXPECT_NEAR(
@@ -414,8 +416,10 @@
superstructure_status_fetcher_->shooter()->turret()->position(),
0.001);
}
- } else if (superstructure_goal_fetcher_->note_goal() == NoteGoal::AMP ||
- superstructure_goal_fetcher_->note_goal() == NoteGoal::TRAP) {
+ } else if (superstructure_status_fetcher_->uncompleted_note_goal() ==
+ NoteStatus::AMP ||
+ superstructure_status_fetcher_->uncompleted_note_goal() ==
+ NoteStatus::TRAP) {
EXPECT_NEAR(
simulated_robot_constants_->common()
->turret_avoid_extend_collision_position(),
@@ -465,18 +469,20 @@
superstructure_status_fetcher_->climber()->position(), 0.001);
}
- if (superstructure_goal_fetcher_->has_note_goal()) {
+ if (superstructure_status_fetcher_->has_uncompleted_note_goal()) {
double set_point = simulated_robot_constants_->common()
->extend_set_points()
->retracted();
- if (superstructure_goal_fetcher_->note_goal() == NoteGoal::TRAP) {
+ if (superstructure_status_fetcher_->uncompleted_note_goal() ==
+ NoteStatus::TRAP) {
set_point =
simulated_robot_constants_->common()->extend_set_points()->trap();
- } else if (superstructure_goal_fetcher_->note_goal() == NoteGoal::AMP) {
+ } else if (superstructure_status_fetcher_->uncompleted_note_goal() ==
+ NoteStatus::AMP) {
set_point =
simulated_robot_constants_->common()->extend_set_points()->amp();
- } else if (superstructure_goal_fetcher_->note_goal() ==
- NoteGoal::CATAPULT) {
+ } else if (superstructure_status_fetcher_->uncompleted_note_goal() ==
+ NoteStatus::CATAPULT) {
set_point = simulated_robot_constants_->common()
->extend_set_points()
->catapult();
@@ -809,7 +815,7 @@
VerifyNearGoal();
EXPECT_EQ(superstructure_status_fetcher_->state(),
- SuperstructureState::LOADED);
+ SuperstructureState::READY);
}
// Tests that the loop zeroes when run for a while without a goal.
@@ -1322,6 +1328,9 @@
superstructure_status_fetcher_.Fetch();
ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr);
+ LOG(INFO) << EnumNameNoteStatus(
+ superstructure_status_fetcher_->uncompleted_note_goal());
+
EXPECT_EQ(superstructure_status_fetcher_->state(),
SuperstructureState::READY);
diff --git a/y2024/control_loops/superstructure/superstructure_status.fbs b/y2024/control_loops/superstructure/superstructure_status.fbs
index cd3e65b..0420c26 100644
--- a/y2024/control_loops/superstructure/superstructure_status.fbs
+++ b/y2024/control_loops/superstructure/superstructure_status.fbs
@@ -54,6 +54,15 @@
shot_distance:double (id: 3);
}
+// Enum representing where the superstructure
+// is currently trying to send the note.
+enum NoteStatus : ubyte {
+ NONE = 0,
+ CATAPULT = 1,
+ AMP = 2,
+ TRAP = 3,
+}
+
table ShooterStatus {
// Estimated angle and angular velocitiy of the turret.
turret:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 0);
@@ -145,7 +154,9 @@
extend_ready_for_transfer:bool (id: 12);
// Indicates that the turret is in position to avoid the extend.
- turret_ready_for_extend_move:bool (id: 13);
+ turret_ready_for_extend_move:bool (id: 13, deprecated);
+
+ uncompleted_note_goal:NoteStatus = NONE (id: 14);
}
root_type Status;
diff --git a/y2024/y2024_roborio.json b/y2024/y2024_roborio.json
index 5ba882a..3743ce6 100644
--- a/y2024/y2024_roborio.json
+++ b/y2024/y2024_roborio.json
@@ -128,7 +128,8 @@
"source_node": "roborio",
"frequency": 400,
"max_size": 2048,
- "num_senders": 2
+ "num_senders": 2,
+ "max_size": 1504
},
{
"name": "/superstructure",