commit 2a59cf0b39ad725a82f71b1225f911c1c13fc989
author James Kuszmaul <jabukuszmaul+collab@gmail.com> Thu Mar 17 11:02:02 2022 -0700
committer James Kuszmaul <jabukuszmaul+collab@gmail.com> Thu Mar 17 13:42:49 2022 -0700
tree 92d69188f6be2581a8acec60118dec37a00db49e
parent 61a876e623ef65e25e8ff98b3d67026ab953cdb1

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>

frc971/constants.h

diff --git a/frc971/constants.h b/frc971/constants.h
index 6188f01..4f7cb36 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -127,6 +127,7 @@
   double upper;
 
   constexpr double middle() const { return (lower_hard + upper_hard) / 2.0; }
+  constexpr double middle_soft() const { return (lower + upper) / 2.0; }
 
   constexpr double range() const { return upper_hard - lower_hard; }
 };

frc971/control_loops/aiming/BUILD

diff --git a/frc971/control_loops/aiming/BUILD b/frc971/control_loops/aiming/BUILD
index f779b8e..cb525b1 100644
--- a/frc971/control_loops/aiming/BUILD
+++ b/frc971/control_loops/aiming/BUILD
@@ -8,6 +8,7 @@
         "//aos/logging",
         "//frc971:constants",
         "//frc971/control_loops:pose",
+        "//frc971/zeroing:wrap",
     ],
 )
 

frc971/control_loops/aiming/aiming.cc

diff --git a/frc971/control_loops/aiming/aiming.cc b/frc971/control_loops/aiming/aiming.cc
index 8229e13..cea2476 100644
--- a/frc971/control_loops/aiming/aiming.cc
+++ b/frc971/control_loops/aiming/aiming.cc
@@ -1,6 +1,7 @@
 #include "frc971/control_loops/aiming/aiming.h"
 
 #include "glog/logging.h"
+#include "frc971/zeroing/wrap.h"
 
 namespace frc971::control_loops::aiming {
 
@@ -110,8 +111,6 @@
   //     (rel_x * rel_y' - rel_y * rel_x') / (rel_x^2 + rel_y^2) - dtheta / dt
   const double dheading_dt = atan_diff - state.yaw_rate;
 
-  const double range =
-      config.turret_range.range() - config.anti_wrap_buffer * 2.0;
   // Calculate a goal turret heading such that it is within +/- pi of the
   // current position (i.e., a goal that would minimize the amount the turret
   // would have to travel).
@@ -121,8 +120,10 @@
       state.last_turret_goal +
       aos::math::NormalizeAngle(heading_to_goal - config.turret_zero_offset -
                                 state.last_turret_goal);
-  if (std::abs(turret_heading - config.turret_range.middle()) > range / 2.0) {
-    turret_heading = aos::math::NormalizeAngle(turret_heading);
+  if (turret_heading > config.turret_range.upper - config.anti_wrap_buffer ||
+      turret_heading < config.turret_range.lower + config.anti_wrap_buffer) {
+    turret_heading = frc971::zeroing::Wrap(config.turret_range.middle_soft(),
+                                           turret_heading, 2.0 * M_PI);
   }
   result.position = turret_heading;
   result.velocity = dheading_dt;

y2022/control_loops/superstructure/superstructure.cc

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);
   }