Add shooter superstructure code
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
Change-Id: I559d79e1308c8ec6d434235e16f0273b4ce09f0d
diff --git a/y2024/control_loops/superstructure/aiming.cc b/y2024/control_loops/superstructure/aiming.cc
new file mode 100644
index 0000000..980efba
--- /dev/null
+++ b/y2024/control_loops/superstructure/aiming.cc
@@ -0,0 +1,84 @@
+#include "y2024/control_loops/superstructure/aiming.h"
+
+#include "frc971/control_loops/aiming/aiming.h"
+#include "frc971/control_loops/pose.h"
+
+using frc971::control_loops::aiming::RobotState;
+using frc971::control_loops::aiming::ShotConfig;
+using frc971::control_loops::aiming::ShotMode;
+using y2024::control_loops::superstructure::Aimer;
+
+Aimer::Aimer(aos::EventLoop *event_loop,
+ const y2024::Constants *robot_constants)
+ : event_loop_(event_loop),
+ robot_constants_(CHECK_NOTNULL(robot_constants)),
+ drivetrain_config_(
+ frc971::control_loops::drivetrain::DrivetrainConfig<double>::
+ FromFlatbuffer(*robot_constants_->common()->drivetrain())),
+ interpolation_table_(
+ y2024::constants::Values::InterpolationTableFromFlatbuffer(
+ robot_constants_->common()->shooter_interpolation_table())),
+ joystick_state_fetcher_(
+ event_loop_->MakeFetcher<aos::JoystickState>("/aos")) {}
+
+void Aimer::Update(
+ const frc971::control_loops::drivetrain::Status *status,
+ frc971::control_loops::aiming::ShotMode shot_mode,
+ frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoalStatic
+ *turret_goal) {
+ const frc971::control_loops::Pose robot_pose({status->x(), status->y(), 0},
+ status->theta());
+ joystick_state_fetcher_.Fetch();
+ CHECK_NOTNULL(joystick_state_fetcher_.get());
+
+ aos::Alliance 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());
+
+ const frc971::control_loops::Pose goal =
+ alliance == aos::Alliance::kRed ? red_alliance_goal : blue_alliance_goal;
+
+ const Eigen::Vector2d linear_angular =
+ drivetrain_config_.Tlr_to_la() *
+ Eigen::Vector2d(status->estimated_left_velocity(),
+ status->estimated_right_velocity());
+ const double xdot = linear_angular(0) * std::cos(status->theta());
+ const double ydot = linear_angular(0) * std::sin(status->theta());
+
+ // Use the previous shot distance to estimate the speed-over-ground of the
+ // note.
+ current_goal_ = frc971::control_loops::aiming::AimerGoal(
+ ShotConfig{goal, shot_mode,
+ frc971::constants::Range::FromFlatbuffer(
+ robot_constants_->common()->turret()->range()),
+ interpolation_table_.Get(current_goal_.target_distance)
+ .shot_speed_over_ground,
+ /*wrap_mode=*/0.0, /*turret_zero_offset*/ 0.0},
+ RobotState{
+ robot_pose, {xdot, ydot}, linear_angular(1), current_goal_.position});
+
+ turret_goal->set_unsafe_goal(current_goal_.position);
+ turret_goal->set_goal_velocity(current_goal_.velocity);
+}
+
+flatbuffers::Offset<AimerStatus> Aimer::PopulateStatus(
+ flatbuffers::FlatBufferBuilder *fbb) const {
+ AimerStatus::Builder builder(*fbb);
+ builder.add_turret_position(current_goal_.position);
+ builder.add_turret_velocity(current_goal_.velocity);
+ builder.add_target_distance(current_goal_.target_distance);
+ builder.add_shot_distance(DistanceToGoal());
+ return builder.Finish();
+}