Add shuttle auto-aim
This change prepares the code for shuttle support by adding a new button,
shot table, and shot target location for the shuttle shot.
Signed-off-by: Filip Kujawa <filip.j.kujawa@gmail.com>
Change-Id: Ic70f1b0344fcab8316553cbda4a337cf8353dfd6
diff --git a/y2024/control_loops/superstructure/BUILD b/y2024/control_loops/superstructure/BUILD
index 6c67b26..4e33f20 100644
--- a/y2024/control_loops/superstructure/BUILD
+++ b/y2024/control_loops/superstructure/BUILD
@@ -220,6 +220,7 @@
"aiming.h",
],
deps = [
+ ":superstructure_goal_fbs",
":superstructure_status_fbs",
"//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
"//frc971/control_loops/aiming",
diff --git a/y2024/control_loops/superstructure/aiming.cc b/y2024/control_loops/superstructure/aiming.cc
index bf68527..200029e 100644
--- a/y2024/control_loops/superstructure/aiming.cc
+++ b/y2024/control_loops/superstructure/aiming.cc
@@ -18,6 +18,10 @@
interpolation_table_(
y2024::constants::Values::InterpolationTableFromFlatbuffer(
robot_constants_->common()->shooter_interpolation_table())),
+ interpolation_table_shuttle_(
+ y2024::constants::Values::InterpolationTableFromFlatbuffer(
+ robot_constants_->common()
+ ->shooter_shuttle_interpolation_table())),
joystick_state_fetcher_(
event_loop_->MakeFetcher<aos::JoystickState>("/aos")) {}
@@ -25,7 +29,12 @@
const frc971::control_loops::drivetrain::Status *status,
frc971::control_loops::aiming::ShotMode shot_mode,
frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoalStatic
- *turret_goal) {
+ *turret_goal,
+ AutoAimMode auto_aim_mode) {
+ // Default to aiming at the speaker in the absence of any other targets.
+ if (auto_aim_mode == AutoAimMode::NONE) {
+ auto_aim_mode = AutoAimMode::SPEAKER;
+ }
if (status == nullptr) {
return;
}
@@ -41,22 +50,13 @@
alliance = joystick_state_fetcher_->alliance();
}
- const frc971::control_loops::Pose red_alliance_goal(
- frc971::ToEigenOrDie<3, 1>(*robot_constants_->common()
- ->shooter_targets()
- ->red_alliance()
- ->pos()),
- robot_constants_->common()->shooter_targets()->red_alliance()->theta());
-
- const frc971::control_loops::Pose blue_alliance_goal(
- frc971::ToEigenOrDie<3, 1>(*robot_constants_->common()
- ->shooter_targets()
- ->blue_alliance()
- ->pos()),
- robot_constants_->common()->shooter_targets()->blue_alliance()->theta());
+ frc971::shooter_interpolation::InterpolationTable<
+ y2024::constants::Values::ShotParams> *current_interpolation_table =
+ interpolation_tables_.at(auto_aim_mode);
const frc971::control_loops::Pose goal =
- alliance == aos::Alliance::kRed ? red_alliance_goal : blue_alliance_goal;
+ alliance == aos::Alliance::kRed ? red_alliance_goals_.at(auto_aim_mode)
+ : blue_alliance_goals_.at(auto_aim_mode);
const Eigen::Vector2d linear_angular =
drivetrain_config_.Tlr_to_la() *
@@ -71,7 +71,7 @@
ShotConfig{goal, shot_mode,
frc971::constants::Range::FromFlatbuffer(
robot_constants_->common()->turret()->range()),
- interpolation_table_.Get(current_goal_.target_distance)
+ current_interpolation_table->Get(current_goal_.target_distance)
.shot_speed_over_ground,
/*wrap_mode=*/0.15, M_PI - kTurretZeroOffset},
RobotState{
diff --git a/y2024/control_loops/superstructure/aiming.h b/y2024/control_loops/superstructure/aiming.h
index 97e319d..de4c117 100644
--- a/y2024/control_loops/superstructure/aiming.h
+++ b/y2024/control_loops/superstructure/aiming.h
@@ -1,6 +1,8 @@
#ifndef Y2024_CONTROL_LOOPS_SUPERSTRUCTURE_AIMING_H_
#define Y2024_CONTROL_LOOPS_SUPERSTRUCTURE_AIMING_H_
+#include <map>
+
#include "frc971/control_loops/aiming/aiming.h"
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/pose.h"
@@ -9,8 +11,8 @@
#include "y2024/constants.h"
#include "y2024/constants/constants_generated.h"
#include "y2024/control_loops/drivetrain/drivetrain_base.h"
+#include "y2024/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2024/control_loops/superstructure/superstructure_status_generated.h"
-
using y2024::control_loops::superstructure::AimerStatus;
namespace y2024::control_loops::superstructure {
@@ -26,7 +28,8 @@
const frc971::control_loops::drivetrain::Status *status,
frc971::control_loops::aiming::ShotMode shot_mode,
frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoalStatic
- *turret_goal);
+ *turret_goal,
+ AutoAimMode auto_aim_mode);
double DistanceToGoal() const { return current_goal_.virtual_shot_distance; }
@@ -45,6 +48,64 @@
y2024::constants::Values::ShotParams>
interpolation_table_;
+ frc971::shooter_interpolation::InterpolationTable<
+ y2024::constants::Values::ShotParams>
+ interpolation_table_shuttle_;
+
+ std::map<AutoAimMode, frc971::shooter_interpolation::InterpolationTable<
+ y2024::constants::Values::ShotParams> *>
+ interpolation_tables_ = {
+ {AutoAimMode::SPEAKER, &interpolation_table_},
+ {AutoAimMode::SHUTTLE, &interpolation_table_shuttle_}};
+
+ std::map<AutoAimMode, frc971::control_loops::Pose> red_alliance_goals_ = {
+ {AutoAimMode::SPEAKER,
+ frc971::control_loops::Pose(
+ frc971::ToEigenOrDie<3, 1>(*robot_constants_->common()
+ ->shooter_targets()
+ ->red_alliance()
+ ->pos()),
+ robot_constants_->common()
+ ->shooter_targets()
+ ->red_alliance()
+ ->theta())},
+ {
+ AutoAimMode::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_ = {
+ {AutoAimMode::SPEAKER,
+ frc971::control_loops::Pose(
+ frc971::ToEigenOrDie<3, 1>(*robot_constants_->common()
+ ->shooter_targets()
+ ->blue_alliance()
+ ->pos()),
+ robot_constants_->common()
+ ->shooter_targets()
+ ->blue_alliance()
+ ->theta())},
+ {
+ AutoAimMode::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_;
frc971::control_loops::aiming::TurretGoal current_goal_;
diff --git a/y2024/control_loops/superstructure/shooter.cc b/y2024/control_loops/superstructure/shooter.cc
index 1cd0191..0c52360 100644
--- a/y2024/control_loops/superstructure/shooter.cc
+++ b/y2024/control_loops/superstructure/shooter.cc
@@ -31,6 +31,10 @@
interpolation_table_(
y2024::constants::Values::InterpolationTableFromFlatbuffer(
robot_constants_->common()->shooter_interpolation_table())),
+ interpolation_table_shuttle_(
+ y2024::constants::Values::InterpolationTableFromFlatbuffer(
+ robot_constants_->common()
+ ->shooter_shuttle_interpolation_table())),
debouncer_(std::chrono::milliseconds(100), std::chrono::milliseconds(8)) {
}
@@ -116,7 +120,8 @@
PopulateStaticZeroingSingleDOFProfiledSubsystemGoal(
altitude_goal_builder.get(),
robot_constants_->common()->altitude_avoid_extend_collision_position());
- } else if (shooter_goal == nullptr || !shooter_goal->auto_aim() ||
+ } else if (shooter_goal == nullptr ||
+ (shooter_goal->auto_aim() == AutoAimMode::NONE) ||
(!piece_loaded && state_ == CatapultState::READY)) {
// We don't have the note so we should be ready to intake it.
PopulateStaticZeroingSingleDOFProfiledSubsystemGoal(
@@ -137,13 +142,21 @@
aimer_.Update(
drivetrain_status_fetcher_.get(),
frc971::control_loops::aiming::ShotMode::kShootOnTheFly,
- aiming ? turret_goal_builder.get() : auto_aim_goal_builder.get());
+ aiming ? turret_goal_builder.get() : auto_aim_goal_builder.get(),
+ shooter_goal != nullptr ? shooter_goal->auto_aim() : AutoAimMode::NONE);
// We have a game piece and are being asked to aim.
constants::Values::ShotParams shot_params;
+ frc971::shooter_interpolation::InterpolationTable<
+ y2024::constants::Values::ShotParams> *interpolation_table =
+ (shooter_goal != nullptr &&
+ shooter_goal->auto_aim() == AutoAimMode::SHUTTLE)
+ ? &interpolation_table_shuttle_
+ : &interpolation_table_;
if ((piece_loaded || state_ == CatapultState::FIRING) &&
- shooter_goal != nullptr && shooter_goal->auto_aim() &&
- interpolation_table_.GetInRange(distance_to_goal, &shot_params)) {
+ shooter_goal != nullptr &&
+ (shooter_goal->auto_aim() != AutoAimMode::NONE) &&
+ interpolation_table->GetInRange(distance_to_goal, &shot_params)) {
PopulateStaticZeroingSingleDOFProfiledSubsystemGoal(
altitude_goal_builder.get(), shot_params.shot_altitude_angle);
}
@@ -153,14 +166,16 @@
const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
*turret_goal =
- (shooter_goal != nullptr && !shooter_goal->auto_aim() &&
+ (shooter_goal != nullptr &&
+ (shooter_goal->auto_aim() == AutoAimMode::NONE) &&
(piece_loaded || state_ == CatapultState::FIRING || climbing) &&
shooter_goal->has_turret_position())
? shooter_goal->turret_position()
: &turret_goal_builder->AsFlatbuffer();
const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
- *altitude_goal = (shooter_goal != nullptr && !shooter_goal->auto_aim() &&
+ *altitude_goal = (shooter_goal != nullptr &&
+ (shooter_goal->auto_aim() == AutoAimMode::NONE) &&
(piece_loaded || state_ == CatapultState::FIRING) &&
shooter_goal->has_altitude_position())
? shooter_goal->altitude_position()
diff --git a/y2024/control_loops/superstructure/shooter.h b/y2024/control_loops/superstructure/shooter.h
index dc57507..f0561a1 100644
--- a/y2024/control_loops/superstructure/shooter.h
+++ b/y2024/control_loops/superstructure/shooter.h
@@ -151,6 +151,10 @@
y2024::constants::Values::ShotParams>
interpolation_table_;
+ frc971::shooter_interpolation::InterpolationTable<
+ y2024::constants::Values::ShotParams>
+ interpolation_table_shuttle_;
+
Debouncer debouncer_;
uint32_t shot_count_ = 0;
diff --git a/y2024/control_loops/superstructure/superstructure_goal.fbs b/y2024/control_loops/superstructure/superstructure_goal.fbs
index c217bcc..2a5fb00 100644
--- a/y2024/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2024/control_loops/superstructure/superstructure_goal.fbs
@@ -26,11 +26,16 @@
STOWED = 2,
}
+enum AutoAimMode: ubyte {
+ NONE = 0, // No auto aim.
+ SPEAKER = 1, // Auto aim for the speaker shot.
+ SHUTTLE = 2, // Auto aim for the shuttle shot.
+}
+
table ShooterGoal {
catapult_goal:frc971.control_loops.catapult.CatapultGoal (id: 0);
- // If true we ignore the other provided positions
- auto_aim: bool (id: 1);
+ auto_aim: AutoAimMode (id: 1);
// Position for the turret when we aren't auto aiming
turret_position: frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 2);
diff --git a/y2024/control_loops/superstructure/superstructure_lib_test.cc b/y2024/control_loops/superstructure/superstructure_lib_test.cc
index 5544fc0..512ccab 100644
--- a/y2024/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2024/control_loops/superstructure/superstructure_lib_test.cc
@@ -375,7 +375,8 @@
superstructure_status_fetcher_->uncompleted_note_goal() !=
NoteStatus::TRAP) {
if (superstructure_goal_fetcher_->shooter_goal()->has_turret_position() &&
- !superstructure_goal_fetcher_->shooter_goal()->auto_aim()) {
+ (superstructure_goal_fetcher_->shooter_goal()->auto_aim() ==
+ AutoAimMode::NONE)) {
EXPECT_NEAR(
superstructure_goal_fetcher_->shooter_goal()
->turret_position()
@@ -388,7 +389,8 @@
if (superstructure_goal_fetcher_->has_shooter_goal()) {
if (superstructure_goal_fetcher_->shooter_goal()
->has_altitude_position() &&
- !superstructure_goal_fetcher_->shooter_goal()->auto_aim() &&
+ (superstructure_goal_fetcher_->shooter_goal()->auto_aim() ==
+ AutoAimMode::NONE) &&
(superstructure_status_fetcher_->uncompleted_note_goal() !=
NoteStatus::AMP &&
superstructure_status_fetcher_->uncompleted_note_goal() !=
@@ -561,7 +563,7 @@
shooter_goal_builder.add_turret_position(turret_offset);
shooter_goal_builder.add_altitude_position(altitude_offset);
- shooter_goal_builder.add_auto_aim(false);
+ shooter_goal_builder.add_auto_aim(AutoAimMode::NONE);
flatbuffers::Offset<ShooterGoal> shooter_goal_offset =
shooter_goal_builder.Finish();
@@ -610,7 +612,7 @@
shooter_goal_builder.add_turret_position(turret_offset);
shooter_goal_builder.add_altitude_position(altitude_offset);
- shooter_goal_builder.add_auto_aim(false);
+ shooter_goal_builder.add_auto_aim(AutoAimMode::NONE);
shooter_goal_builder.add_preloaded(true);
flatbuffers::Offset<ShooterGoal> shooter_goal_offset =
@@ -661,7 +663,7 @@
shooter_goal_builder.add_turret_position(turret_offset);
shooter_goal_builder.add_altitude_position(altitude_offset);
- shooter_goal_builder.add_auto_aim(false);
+ shooter_goal_builder.add_auto_aim(AutoAimMode::NONE);
flatbuffers::Offset<ShooterGoal> shooter_goal_offset =
shooter_goal_builder.Finish();
@@ -706,7 +708,7 @@
shooter_goal_builder.add_turret_position(turret_offset);
shooter_goal_builder.add_altitude_position(altitude_offset);
- shooter_goal_builder.add_auto_aim(false);
+ shooter_goal_builder.add_auto_aim(AutoAimMode::NONE);
flatbuffers::Offset<ShooterGoal> shooter_goal_offset =
shooter_goal_builder.Finish();
@@ -961,7 +963,7 @@
ShooterGoal::Builder shooter_goal_builder =
builder.MakeBuilder<ShooterGoal>();
- shooter_goal_builder.add_auto_aim(true);
+ shooter_goal_builder.add_auto_aim(AutoAimMode::SPEAKER);
flatbuffers::Offset<ShooterGoal> shooter_goal_offset =
shooter_goal_builder.Finish();
@@ -1002,7 +1004,7 @@
ShooterGoal::Builder shooter_goal_builder =
builder.MakeBuilder<ShooterGoal>();
- shooter_goal_builder.add_auto_aim(true);
+ shooter_goal_builder.add_auto_aim(AutoAimMode::SPEAKER);
flatbuffers::Offset<ShooterGoal> shooter_goal_offset =
shooter_goal_builder.Finish();
@@ -1041,7 +1043,7 @@
ShooterGoal::Builder shooter_goal_builder =
builder.MakeBuilder<ShooterGoal>();
- shooter_goal_builder.add_auto_aim(true);
+ shooter_goal_builder.add_auto_aim(AutoAimMode::SPEAKER);
flatbuffers::Offset<ShooterGoal> shooter_goal_offset =
shooter_goal_builder.Finish();
@@ -1109,7 +1111,7 @@
ShooterGoal::Builder shooter_goal_builder =
builder.MakeBuilder<ShooterGoal>();
- shooter_goal_builder.add_auto_aim(true);
+ shooter_goal_builder.add_auto_aim(AutoAimMode::SPEAKER);
flatbuffers::Offset<ShooterGoal> shooter_goal_offset =
shooter_goal_builder.Finish();
@@ -1188,7 +1190,7 @@
ShooterGoal::Builder shooter_goal_builder =
builder.MakeBuilder<ShooterGoal>();
- shooter_goal_builder.add_auto_aim(false);
+ shooter_goal_builder.add_auto_aim(AutoAimMode::NONE);
shooter_goal_builder.add_catapult_goal(catapult_offset);
shooter_goal_builder.add_altitude_position(altitude_offset);
shooter_goal_builder.add_turret_position(turret_offset);
@@ -1232,7 +1234,7 @@
ShooterGoal::Builder shooter_goal_builder =
builder.MakeBuilder<ShooterGoal>();
- shooter_goal_builder.add_auto_aim(false);
+ shooter_goal_builder.add_auto_aim(AutoAimMode::NONE);
flatbuffers::Offset<ShooterGoal> shooter_goal_offset =
shooter_goal_builder.Finish();
@@ -1357,7 +1359,7 @@
ShooterGoal::Builder shooter_goal_builder =
builder.MakeBuilder<ShooterGoal>();
- shooter_goal_builder.add_auto_aim(true);
+ shooter_goal_builder.add_auto_aim(AutoAimMode::SPEAKER);
shooter_goal_builder.add_preloaded(true);
flatbuffers::Offset<ShooterGoal> shooter_goal_offset =
@@ -1409,7 +1411,7 @@
ShooterGoal::Builder shooter_goal_builder =
builder.MakeBuilder<ShooterGoal>();
- shooter_goal_builder.add_auto_aim(true);
+ shooter_goal_builder.add_auto_aim(AutoAimMode::SPEAKER);
flatbuffers::Offset<ShooterGoal> shooter_goal_offset =
shooter_goal_builder.Finish();