Tune speaker & shuttle shots
Includes ability to have more "stable" auto-aim shuttle shot that does
not rely on x/y of localizer.
Change-Id: I6b76cd8f8436b61bb2f40966dfa8e28a44e85297
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2024/control_loops/superstructure/aiming.cc b/y2024/control_loops/superstructure/aiming.cc
index a2e92fd..d8d336f 100644
--- a/y2024/control_loops/superstructure/aiming.cc
+++ b/y2024/control_loops/superstructure/aiming.cc
@@ -52,8 +52,6 @@
if (status == nullptr) {
return;
}
- const frc971::control_loops::Pose robot_pose({status->x(), status->y(), 0},
- status->theta());
aos::Alliance alliance = aos::Alliance::kRed;
if (!joystick_state_fetcher_.Fetch() && !received_joystick_state_) {
received_joystick_state_ = false;
@@ -64,6 +62,15 @@
alliance = joystick_state_fetcher_->alliance();
}
+ const bool ignore_localizer_pos =
+ auto_aim_mode == AutoAimMode::TURRET_SHUTTLE;
+ const Eigen::Vector3d ignore_localizer_position{
+ 0.0 * (alliance == aos::Alliance::kRed ? 1.0 : -1.0), -1.0, 0.0};
+ const frc971::control_loops::Pose robot_pose(
+ ignore_localizer_pos ? ignore_localizer_position
+ : Eigen::Vector3d{status->x(), status->y(), 0},
+ status->theta());
+
frc971::shooter_interpolation::InterpolationTable<
y2024::constants::Values::ShotParams> *current_interpolation_table =
interpolation_tables_.at(auto_aim_mode);
diff --git a/y2024/control_loops/superstructure/aiming.h b/y2024/control_loops/superstructure/aiming.h
index 808a175..95e5840 100644
--- a/y2024/control_loops/superstructure/aiming.h
+++ b/y2024/control_loops/superstructure/aiming.h
@@ -67,7 +67,8 @@
y2024::constants::Values::ShotParams> *>
interpolation_tables_ = {
{AutoAimMode::SPEAKER, &interpolation_table_},
- {AutoAimMode::SHUTTLE, &interpolation_table_shuttle_}};
+ {AutoAimMode::SHUTTLE, &interpolation_table_shuttle_},
+ {AutoAimMode::TURRET_SHUTTLE, &interpolation_table_shuttle_}};
std::map<AutoAimMode, frc971::control_loops::Pose> red_alliance_goals_ = {
{AutoAimMode::SPEAKER,
@@ -91,6 +92,18 @@
->shooter_shuttle_targets()
->red_alliance()
->theta()),
+ },
+ {
+ AutoAimMode::TURRET_SHUTTLE,
+ frc971::control_loops::Pose(
+ frc971::ToEigenOrDie<3, 1>(*robot_constants_->common()
+ ->shooter_shuttle_targets()
+ ->red_alliance()
+ ->pos()),
+ robot_constants_->common()
+ ->shooter_shuttle_targets()
+ ->red_alliance()
+ ->theta()),
}};
std::map<AutoAimMode, frc971::control_loops::Pose> blue_alliance_goals_ = {
@@ -115,6 +128,18 @@
->shooter_shuttle_targets()
->blue_alliance()
->theta()),
+ },
+ {
+ AutoAimMode::TURRET_SHUTTLE,
+ frc971::control_loops::Pose(
+ frc971::ToEigenOrDie<3, 1>(*robot_constants_->common()
+ ->shooter_shuttle_targets()
+ ->blue_alliance()
+ ->pos()),
+ robot_constants_->common()
+ ->shooter_shuttle_targets()
+ ->blue_alliance()
+ ->theta()),
}};
aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
diff --git a/y2024/control_loops/superstructure/collision_avoidance.h b/y2024/control_loops/superstructure/collision_avoidance.h
index be58b35..dcc26d4 100644
--- a/y2024/control_loops/superstructure/collision_avoidance.h
+++ b/y2024/control_loops/superstructure/collision_avoidance.h
@@ -47,7 +47,7 @@
// Tolerances for the subsystems
static constexpr double kEpsTurret = 0.05;
static constexpr double kEpsIntake = 0.05;
- static constexpr double kEpsExtend = 0.01;
+ static constexpr double kEpsExtend = 0.005;
CollisionAvoidance();
diff --git a/y2024/control_loops/superstructure/shooter.cc b/y2024/control_loops/superstructure/shooter.cc
index 7fb8973..6e1fc71 100644
--- a/y2024/control_loops/superstructure/shooter.cc
+++ b/y2024/control_loops/superstructure/shooter.cc
@@ -152,7 +152,7 @@
frc971::shooter_interpolation::InterpolationTable<
y2024::constants::Values::ShotParams> *interpolation_table =
(shooter_goal != nullptr &&
- shooter_goal->auto_aim() == AutoAimMode::SHUTTLE)
+ shooter_goal->auto_aim() != AutoAimMode::SPEAKER)
? &interpolation_table_shuttle_
: &interpolation_table_;
if ((piece_loaded || state_ == CatapultState::FIRING) &&
diff --git a/y2024/control_loops/superstructure/superstructure.cc b/y2024/control_loops/superstructure/superstructure.cc
index e22dc80..a485943 100644
--- a/y2024/control_loops/superstructure/superstructure.cc
+++ b/y2024/control_loops/superstructure/superstructure.cc
@@ -17,7 +17,8 @@
constexpr double kExtendThreshold = 0.01;
constexpr double kTurretLoadingThreshold = 0.05;
-constexpr double kAltitudeLoadingThreshold = 0.02;
+// Extra large so that we can survive loss of zeroes.
+constexpr double kAltitudeLoadingThreshold = 0.04;
constexpr std::chrono::milliseconds kExtraIntakingTime =
std::chrono::milliseconds(500);
diff --git a/y2024/control_loops/superstructure/superstructure_goal.fbs b/y2024/control_loops/superstructure/superstructure_goal.fbs
index 2a5fb00..c93f6f3 100644
--- a/y2024/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2024/control_loops/superstructure/superstructure_goal.fbs
@@ -30,6 +30,7 @@
NONE = 0, // No auto aim.
SPEAKER = 1, // Auto aim for the speaker shot.
SHUTTLE = 2, // Auto aim for the shuttle shot.
+ TURRET_SHUTTLE = 3, // Auto aim for the turret-only shuttle shot.
}
table ShooterGoal {
@@ -79,7 +80,7 @@
fire: bool (id: 4);
// Tells the climber to go absurdly slow on FULL_EXTEND
- // Deprecated now because we take care of this through
+ // Deprecated now because we take care of this through
// climber_goal_voltage
slow_climber: bool = false (id: 6, deprecated);