Claw gets further when zeroing.
- Claw now roughly finds out where it is.
- The state feedback loop now has a Correct method which takes the
measurement to use.
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 38ca89c..420a0e7 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -218,7 +218,6 @@
Eigen::Matrix<double, number_of_inputs, 1> U;
Eigen::Matrix<double, number_of_inputs, 1> U_uncapped;
Eigen::Matrix<double, number_of_outputs, 1> U_ff;
- Eigen::Matrix<double, number_of_outputs, 1> Y;
const StateFeedbackController<number_of_states, number_of_inputs,
number_of_outputs> &controller() const {
@@ -237,7 +236,6 @@
U.setZero();
U_uncapped.setZero();
U_ff.setZero();
- Y.setZero();
}
StateFeedbackLoop(const StateFeedbackController<
@@ -286,9 +284,14 @@
}
}
- // update_observer is whether or not to use the values in Y.
+ // Corrects X_hat given the observation in Y.
+ void Correct(const Eigen::Matrix<double, number_of_outputs, 1> &Y) {
+ Y_ = Y;
+ new_y_ = true;
+ }
+
// stop_motors is whether or not to output all 0s.
- void Update(bool update_observer, bool stop_motors) {
+ void Update(bool stop_motors) {
if (stop_motors) {
U.setZero();
} else {
@@ -296,8 +299,9 @@
CapU();
}
- if (update_observer) {
- X_hat = (A() - L() * C()) * X_hat + L() * Y + B() * U;
+ 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;
}
@@ -326,6 +330,11 @@
static const int kNumOutputs = number_of_outputs;
static const int kNumInputs = number_of_inputs;
+ // Temporary storage for a measurement until I can figure out why I can't
+ // correct when the measurement is made.
+ Eigen::Matrix<double, number_of_outputs, 1> Y_;
+ bool new_y_ = false;
+
int controller_index_;
};