commit 44e633c548ed0dba3b7b99e4d44461cc6d61175c
author Austin Schuh <austin.linux@gmail.com> Mon Mar 21 20:58:13 2022 -0700
committer Gerrit Code Review <gerrit@software.frc971.org> Mon Mar 21 20:58:13 2022 -0700
tree 1d1e2fb6ff243829b9b7e73137870b12cbef30a3
parent d6ad3027c73c1998ec2da8e39ab6da56e2b501c8
parent b2b8cd84d664961bc0c06293be724b2bbb6c4b89

Merge "Rotate image logger every minute"

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