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/python/intake_pivot.py b/y2024/control_loops/python/intake_pivot.py
index a699a2f..5e60311 100644
--- a/y2024/control_loops/python/intake_pivot.py
+++ b/y2024/control_loops/python/intake_pivot.py
@@ -22,8 +22,8 @@
motor=control_loop.KrakenFOC(),
G=(16. / 60.) * (18. / 62.) * (18. / 62.) * (15. / 24.),
J=0.25,
- q_pos=0.40,
- q_vel=60.0,
+ q_pos=0.80,
+ q_vel=30.0,
kalman_q_pos=0.12,
kalman_q_vel=2.0,
kalman_q_voltage=2.0,
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());
}
diff --git a/y2024/control_loops/superstructure/superstructure_lib_test.cc b/y2024/control_loops/superstructure/superstructure_lib_test.cc
index e0b9846..b223736 100644
--- a/y2024/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2024/control_loops/superstructure/superstructure_lib_test.cc
@@ -1067,8 +1067,8 @@
EXPECT_EQ(superstructure_status_fetcher_->extend_roller(),
ExtendRollerStatus::TRANSFERING_TO_EXTEND);
- EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(), 12.0);
- EXPECT_EQ(superstructure_output_fetcher_->extend_roller_voltage(), 12.0);
+ EXPECT_LT(4.0, superstructure_output_fetcher_->transfer_roller_voltage());
+ EXPECT_LT(4.0, superstructure_output_fetcher_->extend_roller_voltage());
superstructure_plant_.set_extend_beambreak(true);
@@ -1525,7 +1525,7 @@
EXPECT_EQ(superstructure_status_fetcher_->extend_roller(),
ExtendRollerStatus::SCORING_IN_AMP);
- EXPECT_EQ(superstructure_output_fetcher_->extend_roller_voltage(), 12.0);
+ EXPECT_LT(4.0, superstructure_output_fetcher_->extend_roller_voltage());
{
auto builder = superstructure_goal_sender_.MakeBuilder();
diff --git a/y2024/control_loops/superstructure/superstructure_status.fbs b/y2024/control_loops/superstructure/superstructure_status.fbs
index 7541b7e..e9fff2f 100644
--- a/y2024/control_loops/superstructure/superstructure_status.fbs
+++ b/y2024/control_loops/superstructure/superstructure_status.fbs
@@ -6,12 +6,12 @@
enum SuperstructureState : ubyte {
// Before a note has been intaked, the extend should be retracted.
IDLE = 0,
- // Intaking a note and transferring it to the extned through the
+ // Intaking a note and transferring it to the extned through the
// intake, transfer, and extend rollers.
INTAKING = 1,
// The note is in the extend and the extend is not moving.
LOADED = 2,
- // The note is in the extend and the extend is moving towards a goal,
+ // The note is in the extend and the extend is moving towards a goal,
// either the catapult, amp, or trap.
MOVING = 3,
// For Catapult Path, the note is being transferred between the extend and the catapult.
@@ -138,6 +138,13 @@
collided: bool (id: 10);
extend_status:ExtendStatus (id: 11);
+
+ // Indicates that the extend is in position to allow a game piece to
+ // be transfered into it.
+ 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);
}
root_type Status;