Tune flywheels
Switch to hybrid kalman filter, plot voltage error
Change-Id: I9ade9a741aae58bdb3818c23bfe91b18786235f3
diff --git a/frc971/control_loops/hybrid_state_feedback_loop.h b/frc971/control_loops/hybrid_state_feedback_loop.h
index d769541..f033a89 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop.h
+++ b/frc971/control_loops/hybrid_state_feedback_loop.h
@@ -142,7 +142,7 @@
A_.setZero();
B_.setZero();
DelayedU_.setZero();
- UpdateAB(::std::chrono::milliseconds(5));
+ UpdateAB(::std::chrono::microseconds(5050));
}
// Assert that U is within the hardware range.
@@ -158,6 +158,8 @@
// Computes the new X and Y given the control input.
void Update(const Eigen::Matrix<Scalar, number_of_inputs, 1> &U,
::std::chrono::nanoseconds dt, Scalar voltage_battery) {
+ CHECK_NE(dt, std::chrono::nanoseconds(0));
+
// Powers outside of the range are more likely controller bugs than things
// that the plant should deal with.
CheckU(U);
@@ -178,7 +180,11 @@
// or the unit delay... Might want to do that if we care about performance
// again.
UpdateAB(dt);
- return A() * X + B() * U;
+ const Eigen::Matrix<Scalar, number_of_states, 1> new_X =
+ A() * X + B() * DelayedU_;
+ DelayedU_ = U;
+
+ return new_X;
}
protected:
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 27a659a..9c36c90 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -284,9 +284,9 @@
def InitializeState(self):
"""Sets X, Y, and X_hat to zero defaults."""
- self.X = numpy.zeros((self.A.shape[0], 1))
+ self.X = numpy.matrix(numpy.zeros((self.A.shape[0], 1)))
self.Y = self.C * self.X
- self.X_hat = numpy.zeros((self.A.shape[0], 1))
+ self.X_hat = numpy.matrix(numpy.zeros((self.A.shape[0], 1)))
def PlaceControllerPoles(self, poles):
"""Places the controller poles.
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().