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/constants/common.json b/y2024/constants/common.json
index 866f8b6..16acec6 100644
--- a/y2024/constants/common.json
+++ b/y2024/constants/common.json
@@ -23,12 +23,12 @@
}
],
"intake_roller_voltages": {
- "spitting": -12.0,
- "intaking": 12.0
+ "spitting": -4.0,
+ "intaking": 6.0
},
"intake_pivot_set_points": {
- "extended": 0.0,
- "retracted": 1.0
+ "extended": -0.03,
+ "retracted": 1.73
},
"intake_pivot": {
"zeroing_voltage": 3.0,
@@ -55,18 +55,18 @@
// Values in amps
"intake_pivot_supply_current_limit": 5,
"intake_pivot_stator_current_limit": 40,
- "intake_roller_supply_current_limit": 24,
- "intake_roller_stator_current_limit": 55,
- "transfer_roller_supply_current_limit": 25,
- "transfer_roller_stator_current_limit": 66,
+ "intake_roller_supply_current_limit": 20,
+ "intake_roller_stator_current_limit": 50,
+ "transfer_roller_supply_current_limit": 20,
+ "transfer_roller_stator_current_limit": 50,
"drivetrain_supply_current_limit": 35,
"drivetrain_stator_current_limit": 60,
"climber_supply_current_limit": 30,
"climber_stator_current_limit": 100,
"extend_supply_current_limit": 20,
"extend_stator_current_limit": 100,
- "extend_roller_supply_current_limit": 23,
- "extend_roller_stator_current_limit": 118,
+ "extend_roller_supply_current_limit": 60,
+ "extend_roller_stator_current_limit": 200,
"turret_supply_current_limit": 20,
"turret_stator_current_limit": 40,
"altitude_supply_current_limit": 10,
@@ -78,17 +78,17 @@
"retention_roller_supply_current_limit": 10
},
"transfer_roller_voltages": {
- "transfer_in": 12.0,
- "transfer_out": -12.0
+ "transfer_in": 6.0,
+ "transfer_out": -4.0
},
"extend_roller_voltages": {
- "scoring": 12.0,
- "reversing": -12.0
+ "scoring": 6.0,
+ "reversing": -4.0
},
"climber_set_points": {
"full_extend": -0.005,
"stowed": -0.35,
- "retract": -0.44
+ "retract": -0.478
},
"climber": {
"zeroing_voltage": 3.0,
@@ -222,7 +222,7 @@
// TODO(Filip): Update the speaker and amp shooter setpoints
"shooter_speaker_set_point": {
"turret_position": 0.0,
- "altitude_position": 0.0,
+ "altitude_position": 0.75,
"shot_velocity": 0.0
},
"shooter_podium_set_point":{
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;
diff --git a/y2024/joystick_reader.cc b/y2024/joystick_reader.cc
index 6344127..caa6805 100644
--- a/y2024/joystick_reader.cc
+++ b/y2024/joystick_reader.cc
@@ -36,17 +36,21 @@
namespace superstructure = y2024::control_loops::superstructure;
-// TODO(Xander): add x,y location from physical wiring
-const ButtonLocation kIntake(0, 0);
+// TODO(Xander): add button location from physical wiring
+// Note: Due to use_redundant_joysticks, the AOS_LOG statements
+// for the internal joystick code will give offset joystick numbering.
+const ButtonLocation kIntake(2, 8);
const ButtonLocation kSpit(0, 0);
-const ButtonLocation kCatapultLoad(0, 0);
-const ButtonLocation kAmp(0, 0);
-const ButtonLocation kTrap(0, 0);
+const ButtonLocation kCatapultLoad(1, 7);
+const ButtonLocation kAmp(2, 7);
+const ButtonLocation kFire(2, 6);
+const ButtonLocation kTrap(2, 5);
const ButtonLocation kAutoAim(0, 0);
-const ButtonLocation kAimSpeaker(0, 0);
+const ButtonLocation kAimSpeaker(1, 6);
const ButtonLocation kAimPodium(0, 0);
const ButtonLocation kShoot(0, 0);
-const ButtonLocation kClimb(0, 0);
+const ButtonLocation kRaiseClimber(3, 2);
+const ButtonLocation kRetractClimber(2, 4);
const ButtonLocation kExtraButtonOne(0, 0);
const ButtonLocation kExtraButtonTwo(0, 0);
const ButtonLocation kExtraButtonThree(0, 0);
@@ -72,7 +76,6 @@
void HandleTeleop(
const ::frc971::input::driver_station::Data &data) override {
- (void)data;
superstructure_status_fetcher_.Fetch();
if (!superstructure_status_fetcher_.get()) {
AOS_LOG(ERROR, "Got no superstructure status message.\n");
@@ -87,37 +90,28 @@
// Intake is pressed
superstructure_goal_builder->set_intake_goal(
superstructure::IntakeGoal::INTAKE);
- } else if (data.IsPressed(kSpit)) {
- // If Intake not pressed and spit pressed, spit
+ } else {
superstructure_goal_builder->set_intake_goal(
- superstructure::IntakeGoal::SPIT);
+ superstructure::IntakeGoal::NONE);
}
-
- // Set note goal for the robot. Loading the catapult will always be
- // preferred over scoring in the Amp or Trap.
- if (data.IsPressed(kCatapultLoad)) {
- superstructure_goal_builder->set_note_goal(
- superstructure::NoteGoal::CATAPULT);
- } else if (data.IsPressed(kAmp)) {
+ if (data.IsPressed(kAmp)) {
superstructure_goal_builder->set_note_goal(superstructure::NoteGoal::AMP);
} else if (data.IsPressed(kTrap)) {
superstructure_goal_builder->set_note_goal(
superstructure::NoteGoal::TRAP);
+ } else if (data.IsPressed(kCatapultLoad)) {
+ superstructure_goal_builder->set_note_goal(
+ superstructure::NoteGoal::CATAPULT);
+ } else {
+ superstructure_goal_builder->set_note_goal(
+ superstructure::NoteGoal::NONE);
}
-
- // Firing note when requested
- superstructure_goal_builder->set_fire(data.IsPressed(kShoot));
-
- // Shooter goal contains all speaker-related goals
auto shooter_goal = superstructure_goal_builder->add_shooter_goal();
-
- shooter_goal->set_auto_aim(data.IsPressed(kAimSpeaker));
+ shooter_goal->set_auto_aim(false);
// Updating aiming for shooter goal, only one type of aim should be possible
// at a time, auto-aiming is preferred over the setpoints.
- if (data.IsPressed(kAutoAim)) {
- shooter_goal->set_auto_aim(true);
- } else if (data.IsPressed(kAimSpeaker)) {
+ if (data.IsPressed(kAimSpeaker)) {
auto catapult_goal = shooter_goal->add_catapult_goal();
catapult_goal->set_shot_velocity(robot_constants_->common()
->shooter_speaker_set_point()
@@ -131,29 +125,18 @@
shooter_goal->add_turret_position(), robot_constants_->common()
->shooter_speaker_set_point()
->turret_position());
- } else if (data.IsPressed(kAimPodium)) {
- auto catapult_goal = shooter_goal->add_catapult_goal();
- catapult_goal->set_shot_velocity(robot_constants_->common()
- ->shooter_podium_set_point()
- ->shot_velocity());
- PopulateStaticZeroingSingleDOFProfiledSubsystemGoal(
- shooter_goal->add_altitude_position(),
- robot_constants_->common()
- ->shooter_podium_set_point()
- ->altitude_position());
- PopulateStaticZeroingSingleDOFProfiledSubsystemGoal(
- shooter_goal->add_turret_position(), robot_constants_->common()
- ->shooter_podium_set_point()
- ->turret_position());
}
+ superstructure_goal_builder->set_fire(data.IsPressed(kFire));
- // Extend climbers if pressed, retract otherwise
- if (data.IsPressed(kClimb)) {
+ if (data.IsPressed(kRaiseClimber)) {
superstructure_goal_builder->set_climber_goal(
superstructure::ClimberGoal::FULL_EXTEND);
- } else {
+ } else if (data.IsPressed(kRetractClimber)) {
superstructure_goal_builder->set_climber_goal(
superstructure::ClimberGoal::RETRACT);
+ } else {
+ superstructure_goal_builder->set_climber_goal(
+ superstructure::ClimberGoal::STOWED);
}
superstructure_goal_builder.CheckOk(superstructure_goal_builder.Send());
diff --git a/y2024/wpilib_interface.cc b/y2024/wpilib_interface.cc
index 5605d4d..ece47d6 100644
--- a/y2024/wpilib_interface.cc
+++ b/y2024/wpilib_interface.cc
@@ -492,7 +492,6 @@
7, true, "rio", &rio_signal_registry,
current_limits->climber_stator_current_limit(),
current_limits->climber_supply_current_limit());
- climber->set_neutral_mode(ctre::phoenix6::signals::NeutralModeValue::Coast);
std::shared_ptr<TalonFX> extend = std::make_shared<TalonFX>(
12, false, "rio", &rio_signal_registry,
current_limits->extend_stator_current_limit(),
diff --git a/y2024/y2024_roborio.json b/y2024/y2024_roborio.json
index 30af4a2..8584484 100644
--- a/y2024/y2024_roborio.json
+++ b/y2024/y2024_roborio.json
@@ -127,6 +127,7 @@
"type": "y2024.control_loops.superstructure.Status",
"source_node": "roborio",
"frequency": 400,
+ "max_size": 2048,
"num_senders": 2
},
{