commit 093535c2c3b82fefdc45e143e981a99442d35b19
author Austin Schuh <austin.linux@gmail.com> Sat Mar 05 23:21:00 2016 -0800
committer Austin Schuh <austin.linux@gmail.com> Sat Mar 12 17:13:21 2016 -0800
tree 5f04496c73452dbbb8d520f863ed04d50c511d42
parent 5b1a4cd5817927e9f7c17e0f39c406427755669f

Added position control and profiling to drivetrain.

Change-Id: I09ad01c46b23f1c7b6e8530bc10b03a733245b95

frc971/control_loops/state_feedback_loop.h

diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index a51564b..e78a786 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -416,6 +416,12 @@
     }
 
     UpdateObserver(U_);
+
+    UpdateFFReference();
+  }
+
+  // Updates R() after any CapU operations happen on U().
+  void UpdateFFReference() {
     ff_U_ -= U_uncapped() - U();
     if (!Kff().isZero(0)) {
       R_ = A() * R() + B() * ff_U_;