commit 518640d26c4702c5f660b59b5afe5c9b3d0a1ce9
author James Kuszmaul <jabukuszmaul@gmail.com> Sat Apr 13 15:50:50 2019 -0700
committer James Kuszmaul <jabukuszmaul@gmail.com> Sun Apr 14 12:19:49 2019 -0700
tree 6a00f24484cf1b9d6cfeabb46a097fdb40995d69
parent a1dc3b28df7b9e7f90c82c931ded044405d42eb6

Add constant-heading localizer reset button

Change-Id: I25afc8985dbce2199f6fc4e7eb91c58b1e9bbc5d

frc971/control_loops/drivetrain/localizer.h

diff --git a/frc971/control_loops/drivetrain/localizer.h b/frc971/control_loops/drivetrain/localizer.h
index bd48315..23a978c 100644
--- a/frc971/control_loops/drivetrain/localizer.h
+++ b/frc971/control_loops/drivetrain/localizer.h
@@ -50,8 +50,8 @@
                       double gyro_rate, double longitudinal_accelerometer) = 0;
   // Reset the absolute position of the estimator.
   virtual void ResetPosition(::aos::monotonic_clock::time_point t, double x,
-                             double y, double theta,
-                             double theta_uncertainty) = 0;
+                             double y, double theta, double theta_uncertainty,
+                             bool reset_theta) = 0;
   // There are several subtly different norms floating around for state
   // matrices. In order to avoid that mess, we jus tprovide direct accessors for
   // the values that most people care about.
@@ -110,7 +110,8 @@
   }
 
   void ResetPosition(::aos::monotonic_clock::time_point t, double x, double y,
-                     double theta, double /*theta_override*/) override {
+                     double theta, double /*theta_override*/,
+                     bool /*reset_theta*/) override {
     const double left_encoder = ekf_.X_hat(StateIdx::kLeftEncoder);
     const double right_encoder = ekf_.X_hat(StateIdx::kRightEncoder);
     ekf_.ResetInitialState(t, (Ekf::State() << x, y, theta, left_encoder, 0,