commit 3431d62b52cec6e18a52acf36fb62c2ca815bdd9
author James Kuszmaul <jabukuszmaul@gmail.com> Sun Feb 17 17:07:44 2019 -0800
committer James Kuszmaul <jabukuszmaul@gmail.com> Mon Feb 18 20:52:36 2019 -0800
tree d7fffc3ae735f4f1755a3ace2dbb0ace5186b7ee
parent 1057ce8d2ebed01c81d58feb5dc03c5092a8257c

Make drivetrain use generic localizer

This will lead into switching y2019 over to using the Localizer with
camera images.

Change-Id: I2790c09ed9e1cec8d6141c25a4b5a173c2ddc24c

frc971/control_loops/drivetrain/ssdrivetrain.cc

diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.cc b/frc971/control_loops/drivetrain/ssdrivetrain.cc
index 292f291..5330414 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.cc
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.cc
@@ -15,7 +15,7 @@
 
 DrivetrainMotorsSS::DrivetrainMotorsSS(
     const DrivetrainConfig<double> &dt_config, StateFeedbackLoop<7, 2, 4> *kf,
-    double *integrated_kf_heading)
+    LocalizerInterface *localizer)
     : dt_config_(dt_config),
       kf_(kf),
       U_poly_(
@@ -34,7 +34,7 @@
               .finished()),
       linear_profile_(::aos::controls::kLoopFrequency),
       angular_profile_(::aos::controls::kLoopFrequency),
-      integrated_kf_heading_(integrated_kf_heading) {
+      localizer_(localizer) {
   ::aos::controls::HPolytope<0>::Init();
   T_ << 1, 1, 1, -1;
   T_inverse_ = T_.inverse();
@@ -168,8 +168,7 @@
   Eigen::Matrix<double, 2, 1> wheel_heading =
       dt_config_.LeftRightToAngular(kf_->X_hat());
 
-  const double gyro_to_wheel_offset =
-      wheel_heading(0, 0) - *integrated_kf_heading_;
+  const double gyro_to_wheel_offset = wheel_heading(0, 0) - localizer_->theta();
 
   if (enable_control_loop) {
     // Update profiles.
@@ -240,7 +239,7 @@
     Eigen::Matrix<double, 2, 1> wheel_linear =
         dt_config_.LeftRightToLinear(kf_->X_hat());
     Eigen::Matrix<double, 2, 1> next_angular = wheel_heading;
-    next_angular(0, 0) = *integrated_kf_heading_;
+    next_angular(0, 0) = localizer_->theta();
 
     unprofiled_goal_.block<4, 1>(0, 0) =
         dt_config_.AngularLinearToLeftRight(wheel_linear, next_angular);