Made error available outside the SS loop.
Change-Id: I5f5ea68fa9ec0117e388accc08e62da4adb49646
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 8af82b3..64f8cd4 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -389,10 +389,14 @@
X_hat_ += A_inv() * L() * (Y - C() * X_hat_ - D() * U());
}
+ const Eigen::Matrix<double, number_of_states, 1> error() const {
+ return R() - X_hat();
+ }
+
// Returns the calculated controller power.
virtual const Eigen::Matrix<double, number_of_inputs, 1> ControllerOutput() {
ff_U_ = FeedForward();
- return K() * (R() - X_hat()) + ff_U_;
+ return K() * error() + ff_U_;
}
// Calculates the feed forwards power.