Tune flywheels
Switch to hybrid kalman filter, plot voltage error
Change-Id: I9ade9a741aae58bdb3818c23bfe91b18786235f3
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index d63bca7..168df43 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -108,6 +108,8 @@
}
Scalar U_max(int i, int j = 0) const { return U_max()(i, j); }
+ const std::chrono::nanoseconds dt() const { return coefficients().dt; }
+
const Eigen::Matrix<Scalar, number_of_states, 1> &X() const { return X_; }
Scalar X(int i, int j = 0) const { return X()(i, j); }
const Eigen::Matrix<Scalar, number_of_outputs, 1> &Y() const { return Y_; }
@@ -503,9 +505,7 @@
return controller().Kff() * (next_R() - plant().A() * R());
}
- // stop_motors is whether or not to output all 0s.
- void Update(bool stop_motors,
- ::std::chrono::nanoseconds dt = ::std::chrono::milliseconds(5)) {
+ void UpdateController(bool stop_motors) {
if (stop_motors) {
U_.setZero();
U_uncapped_.setZero();
@@ -514,10 +514,15 @@
U_ = U_uncapped_ = ControllerOutput();
CapU();
}
+ UpdateFFReference();
+ }
+
+ // stop_motors is whether or not to output all 0s.
+ void Update(bool stop_motors,
+ ::std::chrono::nanoseconds dt = ::std::chrono::milliseconds(0)) {
+ UpdateController(stop_motors);
UpdateObserver(U_, dt);
-
- UpdateFFReference();
}
// Updates R() after any CapU operations happen on U().