commit 4156560165d1036976f73edb63bd00cdf88c4e92
author Austin Schuh <austin.linux@gmail.com> Sun Feb 28 20:10:49 2016 -0800
committer Austin Schuh <austin.linux@gmail.com> Sun Feb 28 22:03:30 2016 -0800
tree 720308a9e7777ec7d943f011553a1d42562c0eaa
parent 4cd5b3540fed835a21000443e906121e6d77fe0d

Fixed bug where kalman filter was being coppied rather than passed by pointer.

Change-Id: Ib2d0fa0ec2e5a0b19592ca419fec339252a3f698

frc971/control_loops/drivetrain/polydrivetrain.h

diff --git a/frc971/control_loops/drivetrain/polydrivetrain.h b/frc971/control_loops/drivetrain/polydrivetrain.h
index c482f77..689e76f 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.h
+++ b/frc971/control_loops/drivetrain/polydrivetrain.h
@@ -15,7 +15,8 @@
  public:
   enum Gear { HIGH, LOW, SHIFTING_UP, SHIFTING_DOWN };
 
-  PolyDrivetrain(const DrivetrainConfig &dt_config);
+  PolyDrivetrain(const DrivetrainConfig &dt_config,
+                 StateFeedbackLoop<7, 2, 3> *kf);
 
   int controller_index() const { return loop_->controller_index(); }
 
@@ -39,7 +40,7 @@
   void SetPosition(
       const ::frc971::control_loops::DrivetrainQueue::Position *position);
 
-  double FilterVelocity(double throttle);
+  double FilterVelocity(double throttle) const;
 
   double MaxVelocity();
 
@@ -47,8 +48,10 @@
 
   void SendMotors(::frc971::control_loops::DrivetrainQueue::Output *output);
 
+  void PopulateStatus(::frc971::control_loops::DrivetrainQueue::Status *status);
+
  private:
-  StateFeedbackLoop<7, 2, 3> kf_;
+  StateFeedbackLoop<7, 2, 3> *kf_;
 
   const ::aos::controls::HPolytope<2> U_Poly_;
 
@@ -66,6 +69,9 @@
   ::frc971::control_loops::DrivetrainQueue::Position position_;
   int counter_;
   DrivetrainConfig dt_config_;
+
+  double goal_left_velocity_ = 0.0;
+  double goal_right_velocity_ = 0.0;
 };
 
 }  // namespace drivetrain