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/drivetrain.cc

diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 2af6765..dd6b05c 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -243,10 +243,11 @@
     // simulation.
     if (localizer_control_fetcher_.Fetch()) {
       LOG_STRUCT(DEBUG, "localizer_control", *localizer_control_fetcher_);
-      localizer_->ResetPosition(monotonic_now, localizer_control_fetcher_->x,
-                                localizer_control_fetcher_->y,
-                                localizer_control_fetcher_->theta,
-                                localizer_control_fetcher_->theta_uncertainty);
+      localizer_->ResetPosition(
+          monotonic_now, localizer_control_fetcher_->x,
+          localizer_control_fetcher_->y, localizer_control_fetcher_->theta,
+          localizer_control_fetcher_->theta_uncertainty,
+          !localizer_control_fetcher_->keep_current_theta);
     }
     localizer_->Update({last_last_left_voltage_, last_last_right_voltage_},
                        monotonic_now, position->left_encoder,

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,

frc971/control_loops/drivetrain/localizer.q

diff --git a/frc971/control_loops/drivetrain/localizer.q b/frc971/control_loops/drivetrain/localizer.q
index 8fef686..b1169a9 100644
--- a/frc971/control_loops/drivetrain/localizer.q
+++ b/frc971/control_loops/drivetrain/localizer.q
@@ -7,6 +7,7 @@
   float y;      // Y position, meters
   float theta;  // heading, radians
   double theta_uncertainty; // Uncertainty in theta.
+  bool keep_current_theta; // Whether to keep the current theta value.
 };
 
 queue LocalizerControl localizer_control;