Fix auto-aiming when turret range doesn't include [-pi, +pi]
Rather than assuming 0.0 is the center of the range, actually use the
middle of the range.
Also, fixed the fact that we were using the hard stop range instead of
the soft stop range in software limit calculations.
Change-Id: Ia81c6e0259696638c31f476d8b34e79f06d7279e
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index 63f3d60..6daea37 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -226,7 +226,7 @@
if (turret_loading_position > constants::Values::kTurretRange().upper ||
turret_loading_position < constants::Values::kTurretRange().lower) {
turret_loading_position =
- frc971::zeroing::Wrap(constants::Values::kTurretRange().middle(),
+ frc971::zeroing::Wrap(constants::Values::kTurretRange().middle_soft(),
turret_loading_position, 2.0 * M_PI);
}