commit e32eabc9a3c6e198b9bff7d0176efbe7c53acce9
author Alex Perry <alex.perry96@gmail.com> Fri Feb 08 19:51:19 2019 -0800
committer Alex Perry <alex.perry96@gmail.com> Sun Feb 17 15:46:28 2019 -0800
tree e29418afe11d48580c13fd5ef718917933c753bd
parent f11ef5c5cf88a2a94a77e2b48481aa32a0e7803e

Add feedback to the spline drivetrain.

Change-Id: I11bc55bd6d8ca866dbd85d2b1efcd8e278f8021f

frc971/control_loops/drivetrain/drivetrain.cc

diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 168f81d..d097ec8 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -337,7 +337,8 @@
 
   dt_closedloop_.Update(output != NULL && controller_type == 1);
 
-  dt_spline_.Update(output != NULL && controller_type == 2);
+  dt_spline_.Update(output != NULL && controller_type == 2,
+                    xytheta_state_.block<5, 1>(0, 0));
 
   switch (controller_type) {
     case 0:
@@ -389,6 +390,7 @@
 
     dt_openloop_.PopulateStatus(status);
     dt_closedloop_.PopulateStatus(status);
+    dt_spline_.PopulateStatus(status);
   }
 
   double left_voltage = 0.0;