commit 209f17067fdd2e3c1225b314aaa8f252b87adbc6
author Austin Schuh <austin.linux@gmail.com> Sun Nov 29 17:03:00 2015 -0800
committer Austin Schuh <austin.linux@gmail.com> Fri Dec 18 20:59:50 2015 -0800
tree 189537c2c85944f2c63ec027f48a243b553cdfdb
parent 250eb4c31a282717bafe63f37d64b88f48d21108

Tuned drivetrain kalman filter.

Change-Id: I630770a1331cf896bc6d2de6a58fee880c0185c3

y2014/control_loops/drivetrain/drivetrain.h

diff --git a/y2014/control_loops/drivetrain/drivetrain.h b/y2014/control_loops/drivetrain/drivetrain.h
index a77ab15..5b8da12 100644
--- a/y2014/control_loops/drivetrain/drivetrain.h
+++ b/y2014/control_loops/drivetrain/drivetrain.h
@@ -22,11 +22,7 @@
   // drivetrain at frc971::control_loops::drivetrain
   explicit DrivetrainLoop(
       ::frc971::control_loops::DrivetrainQueue *my_drivetrain =
-          &::frc971::control_loops::drivetrain_queue)
-      : aos::controls::ControlLoop<control_loops::DrivetrainQueue>(
-            my_drivetrain) {
-    ::aos::controls::HPolytope<0>::Init();
-  }
+          &::frc971::control_loops::drivetrain_queue);
 
  protected:
   // Executes one cycle of the control loop.
@@ -39,9 +35,17 @@
   typedef ::aos::util::SimpleLogInterval SimpleLogInterval;
   SimpleLogInterval no_position_ = SimpleLogInterval(
       ::aos::time::Time::InSeconds(0.25), WARNING, "no position");
+  double last_gyro_heading_ = 0.0;
+  double last_gyro_rate_ = 0.0;
 
   PolyDrivetrain dt_openloop_;
   DrivetrainMotorsSS dt_closedloop_;
+  StateFeedbackLoop<7, 2, 3> kf_;
+
+  double last_left_voltage_ = 0;
+  double last_right_voltage_ = 0;
+
+  double integrated_kf_heading_ = 0;
 };
 
 }  // namespace drivetrain