commit 81d5f2d6f3f854505252a1ddf2f787388403630b
author James Kuszmaul <jabukuszmaul+collab@gmail.com> Fri Apr 05 21:57:14 2024 -0700
committer James Kuszmaul <jabukuszmaul+collab@gmail.com> Sun Apr 07 13:26:40 2024 -0700
tree c3b1c9e791eab1fa215f958e4bbea9eea273286b
parent 2347ad70bb0c35b2de5ba22742021f7635e25b02

Normalize more angles in localization code

This makes comparing debug signals a bit easier.

Change-Id: I7e3c6992a0536879ab8ccc371162e52d1e9e0084
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>

frc971/control_loops/drivetrain/BUILD

diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 789de2b..9d439b6 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -770,10 +770,11 @@
         ":drivetrain_config",
         ":drivetrain_status_fbs",
         "//aos/events:event_loop",
+        "//aos/time",
+        "//aos/util:math",
         "//frc971/control_loops:control_loops_fbs",
         "//frc971/control_loops:quaternion_utils",
         "//frc971/control_loops:runge_kutta",
-        "@//aos/time",
         "@com_github_google_glog//:glog",
         "@org_tuxfamily_eigen//:eigen",
     ],

frc971/control_loops/drivetrain/improved_down_estimator.cc

diff --git a/frc971/control_loops/drivetrain/improved_down_estimator.cc b/frc971/control_loops/drivetrain/improved_down_estimator.cc
index b7b3878..4687d46 100644
--- a/frc971/control_loops/drivetrain/improved_down_estimator.cc
+++ b/frc971/control_loops/drivetrain/improved_down_estimator.cc
@@ -3,6 +3,7 @@
 #include "Eigen/Dense"
 #include "Eigen/Geometry"
 
+#include "aos/util/math.h"
 #include "frc971/control_loops/quaternion_utils.h"
 
 namespace frc971::control_loops::drivetrain {
@@ -270,7 +271,7 @@
   builder.add_quaternion_z(X_hat().z());
   builder.add_quaternion_w(X_hat().w());
 
-  builder.add_yaw(yaw());
+  builder.add_yaw(aos::math::NormalizeAngle(yaw()));
 
   // Calculate the current pitch numbers to provide a more human-readable
   // debugging output.

y2024/localizer/localizer.cc

diff --git a/y2024/localizer/localizer.cc b/y2024/localizer/localizer.cc
index dc7c568..86ae457 100644
--- a/y2024/localizer/localizer.cc
+++ b/y2024/localizer/localizer.cc
@@ -573,7 +573,8 @@
     debug_builder->set_accepted(true);
     debug_builder->set_expected_robot_x(ekf_.X_hat()(StateIdx::kX));
     debug_builder->set_expected_robot_y(ekf_.X_hat()(StateIdx::kY));
-    debug_builder->set_expected_robot_theta(ekf_.X_hat()(StateIdx::kTheta));
+    debug_builder->set_expected_robot_theta(
+        aos::math::NormalizeAngle(ekf_.X_hat()(StateIdx::kTheta)));
   }
 }