Add 2022 aimer to superstructure
Change-Id: I1ae24de0ed43d73578dabc63a9c9efc79c904552
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2022/control_loops/superstructure/turret/BUILD b/y2022/control_loops/superstructure/turret/BUILD
index c2948d7..2f7ad14 100644
--- a/y2022/control_loops/superstructure/turret/BUILD
+++ b/y2022/control_loops/superstructure/turret/BUILD
@@ -32,3 +32,21 @@
"//frc971/control_loops:state_feedback_loop",
],
)
+
+cc_library(
+ name = "aiming",
+ srcs = ["aiming.cc"],
+ hdrs = ["aiming.h"],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ "//aos:flatbuffers",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:pose",
+ "//frc971/control_loops:profiled_subsystem_fbs",
+ "//frc971/control_loops/aiming",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+ "//y2022:constants",
+ "//y2022/control_loops/drivetrain:drivetrain_base",
+ "//y2022/control_loops/superstructure:superstructure_status_fbs",
+ ],
+)
diff --git a/y2022/control_loops/superstructure/turret/aiming.cc b/y2022/control_loops/superstructure/turret/aiming.cc
new file mode 100644
index 0000000..4c0309c
--- /dev/null
+++ b/y2022/control_loops/superstructure/turret/aiming.cc
@@ -0,0 +1,78 @@
+#include "y2022/control_loops/superstructure/turret/aiming.h"
+
+#include "y2022/constants.h"
+#include "y2022/control_loops/drivetrain/drivetrain_base.h"
+
+namespace y2022 {
+namespace control_loops {
+namespace superstructure {
+namespace turret {
+
+using frc971::control_loops::Pose;
+using frc971::control_loops::aiming::ShotConfig;
+using frc971::control_loops::aiming::RobotState;
+
+namespace {
+// Average speed-over-ground of the ball on its way to the target. Our current
+// model assumes constant ball velocity regardless of shot distance.
+constexpr double kBallSpeedOverGround = 12.0; // m/s
+
+// If the turret is at zero, then it will be at this angle at which the shot
+// will leave the robot. I.e., if the turret is at zero, then the shot will go
+// straight out the back of the robot.
+constexpr double kTurretZeroOffset = M_PI;
+
+flatbuffers::DetachedBuffer MakePrefilledGoal() {
+ flatbuffers::FlatBufferBuilder fbb;
+ fbb.ForceDefaults(true);
+ Aimer::Goal::Builder builder(fbb);
+ builder.add_unsafe_goal(0);
+ builder.add_goal_velocity(0);
+ builder.add_ignore_profile(true);
+ fbb.Finish(builder.Finish());
+ return fbb.Release();
+}
+} // namespace
+
+Aimer::Aimer() : goal_(MakePrefilledGoal()) {}
+
+void Aimer::Update(const Status *status, ShotMode shot_mode) {
+ const Pose robot_pose({status->x(), status->y(), 0}, status->theta());
+ const Pose goal({0.0, 0.0, 0.0}, 0.0);
+
+ const Eigen::Vector2d linear_angular =
+ drivetrain::GetDrivetrainConfig().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());
+
+ current_goal_ =
+ frc971::control_loops::aiming::AimerGoal(
+ ShotConfig{goal, shot_mode, constants::Values::kTurretRange(),
+ kBallSpeedOverGround,
+ /*wrap_mode=*/0.0, kTurretZeroOffset},
+ RobotState{robot_pose,
+ {xdot, ydot},
+ linear_angular(1),
+ goal_.message().unsafe_goal()});
+
+ goal_.mutable_message()->mutate_unsafe_goal(current_goal_.position);
+ goal_.mutable_message()->mutate_goal_velocity(
+ std::clamp(current_goal_.velocity, -2.0, 2.0));
+}
+
+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();
+}
+
+} // namespace turret
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2022
diff --git a/y2022/control_loops/superstructure/turret/aiming.h b/y2022/control_loops/superstructure/turret/aiming.h
new file mode 100644
index 0000000..9494103
--- /dev/null
+++ b/y2022/control_loops/superstructure/turret/aiming.h
@@ -0,0 +1,40 @@
+#ifndef Y2022_CONTROL_LOOPS_SUPERSTRUCTURE_TURRET_AIMING_H_
+#define Y2022_CONTROL_LOOPS_SUPERSTRUCTURE_TURRET_AIMING_H_
+
+#include "aos/flatbuffers.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
+#include "frc971/control_loops/pose.h"
+#include "frc971/control_loops/profiled_subsystem_generated.h"
+#include "frc971/control_loops/aiming/aiming.h"
+#include "y2022/control_loops/superstructure/superstructure_status_generated.h"
+
+namespace y2022::control_loops::superstructure::turret {
+
+// This class manages taking in drivetrain status messages and generating turret
+// goals so that it gets aimed at the goal.
+class Aimer {
+ public:
+ typedef frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
+ Goal;
+ typedef frc971::control_loops::drivetrain::Status Status;
+ typedef frc971::control_loops::aiming::ShotMode ShotMode;
+
+ Aimer();
+
+ void Update(const Status *status, ShotMode shot_mode);
+
+ const Goal *TurretGoal() const { return &goal_.message(); }
+
+ // Returns the distance to the goal, in meters.
+ double DistanceToGoal() const { return current_goal_.virtual_shot_distance; }
+
+ flatbuffers::Offset<AimerStatus> PopulateStatus(
+ flatbuffers::FlatBufferBuilder *fbb) const;
+
+ private:
+ aos::FlatbufferDetachedBuffer<Goal> goal_;
+ frc971::control_loops::aiming::TurretGoal current_goal_;
+};
+
+} // namespace y2022::control_loops::superstructure::turret
+#endif // Y2020_CONTROL_LOOPS_SUPERSTRUCTURE_TURRET_AIMING_H_