Added the feed forwards constant to StateFeedbackLoop.
Change-Id: Ie222de1f302b0edcf5c8b1fdd7f2958e0de608c3
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index d1b24a5..7a04cb9 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -215,6 +215,7 @@
const Eigen::Matrix<double, number_of_states, number_of_outputs> L;
const Eigen::Matrix<double, number_of_inputs, number_of_states> K;
+ const Eigen::Matrix<double, number_of_inputs, number_of_states> Kff;
const Eigen::Matrix<double, number_of_states, number_of_states> A_inv;
StateFeedbackPlantCoefficients<number_of_states, number_of_inputs,
number_of_outputs> plant;
@@ -222,11 +223,13 @@
StateFeedbackController(
const Eigen::Matrix<double, number_of_states, number_of_outputs> &L,
const Eigen::Matrix<double, number_of_inputs, number_of_states> &K,
+ const Eigen::Matrix<double, number_of_inputs, number_of_states> &Kff,
const Eigen::Matrix<double, number_of_states, number_of_states> &A_inv,
const StateFeedbackPlantCoefficients<number_of_states, number_of_inputs,
number_of_outputs> &plant)
: L(L),
K(K),
+ Kff(Kff),
A_inv(A_inv),
plant(plant) {
}
@@ -309,6 +312,10 @@
return controller().K;
}
double K(int i, int j) const { return K()(i, j); }
+ const Eigen::Matrix<double, number_of_inputs, number_of_states> &Kff() const {
+ return controller().Kff;
+ }
+ double Kff(int i, int j) const { return Kff()(i, j); }
const Eigen::Matrix<double, number_of_states, number_of_outputs> &L() const {
return controller().L;
}