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/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_