Austin update the closed dt loop to keep valid estimates of velocity. we then use these in shoot action to set an offset in claw angle for shooting at velocity.
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index ed6bc22..f289e81 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -323,13 +323,17 @@
//::std::cout << "Predict xhat before " << X_hat;
//::std::cout << "Measurement error is " << Y_ - C() * X_hat;
//X_hat = A() * X_hat + B() * U;
+ //::std::cout << "Predict xhat after " << X_hat;
+ UpdateObserver();
+ }
+
+ void UpdateObserver() {
if (new_y_) {
X_hat = (A() - L() * C()) * X_hat + L() * Y_ + B() * U;
new_y_ = false;
} else {
X_hat = A() * X_hat + B() * U;
}
- //::std::cout << "Predict xhat after " << X_hat;
}
// Sets the current controller to be index and verifies that it isn't out of