Refactor szsdps to support the catapult controller taking over
We want to return the catapult back to the starting point using a motion
profile. We've got a nice class for this already, it just needs some
hooks to give up control. Add another interface which lets us separate
the kalman filter from the profile.
Change-Id: I3e57284394fa905e186392ed2adaec430113a172
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 0e4be89..53cd6a2 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -349,6 +349,10 @@
}
Eigen::Matrix<Scalar, number_of_states, 1> &mutable_X_hat() { return X_hat_; }
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &last_U() const {
+ return last_U_;
+ }
+
void Reset(StateFeedbackPlant<number_of_states, number_of_inputs,
number_of_outputs, Scalar> * /*loop*/) {
X_hat_.setZero();