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

diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index 05bf711..1aa3e13 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -9,6 +9,7 @@
 #include "frc971/control_loops/drivetrain/drivetrain.q.h"
 #include "frc971/control_loops/drivetrain/drivetrain_config.h"
 #include "frc971/control_loops/drivetrain/gear.h"
+#include "frc971/control_loops/drivetrain/localizer.h"
 #include "frc971/control_loops/drivetrain/polydrivetrain.h"
 #include "frc971/control_loops/drivetrain/ssdrivetrain.h"
 #include "frc971/control_loops/drivetrain/splinedrivetrain.h"
@@ -24,6 +25,7 @@
   // drivetrain at frc971::control_loops::drivetrain
   explicit DrivetrainLoop(
       const DrivetrainConfig<double> &dt_config, ::aos::EventLoop *event_loop,
+      LocalizerInterface *localizer,
       const ::std::string &name = ".frc971.control_loops.drivetrain_queue");
 
   int ControllerIndexFromGears();
@@ -38,16 +40,12 @@
 
   void Zero(::frc971::control_loops::DrivetrainQueue::Output *output) override;
 
-  // Computes the xy state change given the change in the lr state.
-  ::Eigen::Matrix<double, 3, 1> PredictState(
-      const ::Eigen::Matrix<double, 3, 1> &xytheta_state,
-      const ::Eigen::Matrix<double, 7, 1> &state,
-      const ::Eigen::Matrix<double, 7, 1> &previous_state) const;
-
   double last_gyro_rate_ = 0.0;
 
   const DrivetrainConfig<double> dt_config_;
 
+  LocalizerInterface *localizer_;
+
   StateFeedbackLoop<7, 2, 4> kf_;
   PolyDrivetrain<double> dt_openloop_;
   DrivetrainMotorsSS dt_closedloop_;
@@ -65,8 +63,6 @@
   double last_left_voltage_ = 0;
   double last_right_voltage_ = 0;
 
-  double integrated_kf_heading_ = 0;
-
   bool left_high_requested_;
   bool right_high_requested_;
 
@@ -74,12 +70,6 @@
 
   double last_accel_ = 0.0;
 
-  // Current xytheta state of the robot.  This is essentially the kalman filter
-  // integrated up in a direction.
-  // [x, y, theta, vl, vr, left_error, right_error]
-  ::Eigen::Matrix<double, 7, 1> xytheta_state_ =
-      ::Eigen::Matrix<double, 7, 1>::Zero();
-
   // Last kalman filter state.
   ::Eigen::Matrix<double, 7, 1> last_state_ =
       ::Eigen::Matrix<double, 7, 1>::Zero();