Wrap turret intake loctions
Signed-off-by: Ravago Jones <ravagojones@gmail.com>
Change-Id: I0e5bd91808613264b7469490cfb2f4ecac4339ae
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index b25d074..c2decea 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -184,6 +184,20 @@
? constants::Values::kTurretFrontIntakePos()
: constants::Values::kTurretBackIntakePos());
+ // Turn to the loading position as close to the current position as
+ // possible
+ // Strategy is copied from frc971/control_loops/aiming/aiming.cc
+ turret_loading_position =
+ turret_.estimated_position() +
+ aos::math::NormalizeAngle(turret_loading_position -
+ turret_.estimated_position());
+ // if out of range, reset back to within +/- pi of zero.
+ if (turret_loading_position > constants::Values::kTurretRange().upper ||
+ turret_loading_position < constants::Values::kTurretRange().lower) {
+ turret_loading_position =
+ aos::math::NormalizeAngle(turret_loading_position);
+ }
+
turret_goal_buffer.Finish(
frc971::control_loops::
CreateStaticZeroingSingleDOFProfiledSubsystemGoal(