Further robot bringup
* Add some basic buttons for controlling the superstructure.
* Put climber into brake mode.
* Tune intake a bit.
* Add more information to superstructure.
* Improve sequencing to match robot's behavior better.
* Add slightly better collision avoidance for turret/extend, until
a more proper solution is implemented (see
https://software.frc971.org/gerrit/c/971-Robot-Code/+/8184).
Change-Id: I5d5c77de5f3ef209be64950301ddf610f56d8064
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2024/control_loops/superstructure/superstructure.cc b/y2024/control_loops/superstructure/superstructure.cc
index 56980d5..577ac15 100644
--- a/y2024/control_loops/superstructure/superstructure.cc
+++ b/y2024/control_loops/superstructure/superstructure.cc
@@ -168,6 +168,7 @@
// 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:
if (unsafe_goal != nullptr &&
@@ -208,13 +209,14 @@
// avoid collision when the extend moves.
if (unsafe_goal->note_goal() == NoteGoal::AMP ||
unsafe_goal->note_goal() == NoteGoal::TRAP) {
- bool turret_ready_for_extend_move =
+ turret_ready_for_extend_move =
PositionNear(shooter_.turret().estimated_position(),
robot_constants_->common()
->turret_avoid_extend_collision_position(),
kTurretLoadingThreshold);
+ transfer_roller_status = TransferRollerStatus::TRANSFERING_IN;
- if (turret_ready_for_extend_move) {
+ if (turret_ready_for_extend_move.value()) {
state_ = SuperstructureState::MOVING;
} else {
move_turret_to_standby = true;
@@ -231,6 +233,7 @@
}
break;
case SuperstructureState::MOVING:
+ transfer_roller_status = TransferRollerStatus::TRANSFERING_IN;
if (catapult_requested_) {
extend_goal = ExtendStatus::CATAPULT;
@@ -345,6 +348,7 @@
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;
@@ -429,6 +433,17 @@
double extend_position = 0.0;
+ if (unsafe_goal != nullptr && unsafe_goal->note_goal() == NoteGoal::TRAP) {
+ extend_goal = ExtendStatus::TRAP;
+ move_turret_to_standby = true;
+ }
+
+ // In lieu of having full collision avoidance ready, move the turret out of
+ // the way whenever the extend is raised too much.
+ if (extend_.position() > 0.05) {
+ move_turret_to_standby = true;
+ }
+
// Set the extend position based on the state machine output
switch (extend_goal) {
case ExtendStatus::RETRACTED:
@@ -576,6 +591,11 @@
status_builder.add_extend_status(extend_status);
status_builder.add_extend(extend_status_offset);
status_builder.add_state(state_);
+ 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());
}