Add yaw corrections to plot, normalize angles in status message
Change-Id: I18814253233a193380a39a8a9979e3e158a5ebb8
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2024/localizer/localizer.cc b/y2024/localizer/localizer.cc
index daef22c..61e78e7 100644
--- a/y2024/localizer/localizer.cc
+++ b/y2024/localizer/localizer.cc
@@ -631,7 +631,7 @@
frc971::control_loops::drivetrain::LocalizerState::Builder builder(*fbb);
builder.add_x(X_hat(StateIdx::kX));
builder.add_y(X_hat(StateIdx::kY));
- builder.add_theta(X_hat(StateIdx::kTheta));
+ builder.add_theta(aos::math::NormalizeAngle(X_hat(StateIdx::kTheta)));
builder.add_left_velocity(X_hat(StateIdx::kLeftVelocity));
builder.add_right_velocity(X_hat(StateIdx::kRightVelocity));
builder.add_left_encoder(X_hat(StateIdx::kLeftEncoder));