cleaned up the control loop code
There were a lot of mutable public members, which are against the style
guide (and they were in the wrong place declaration order-wise).
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index d62bc63..aa3f9a1 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -1,5 +1,5 @@
-#ifndef FRC971_CONTROL_LOOPS_STATEFEEDBACKLOOP_H_
-#define FRC971_CONTROL_LOOPS_STATEFEEDBACKLOOP_H_
+#ifndef FRC971_CONTROL_LOOPS_STATE_FEEDBACK_LOOP_H_
+#define FRC971_CONTROL_LOOPS_STATE_FEEDBACK_LOOP_H_
#include <assert.h>
@@ -11,29 +11,18 @@
#include "aos/common/logging/logging.h"
#include "aos/common/macros.h"
-// TODO(brians): There are a lot of public member variables in here that should
-// be made private and have (const) accessors instead (and have _s at the end of
-// their names).
-
template <int number_of_states, int number_of_inputs, int number_of_outputs>
-class StateFeedbackPlantCoefficients {
+class StateFeedbackPlantCoefficients final {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
- const Eigen::Matrix<double, number_of_states, number_of_states> A;
- const Eigen::Matrix<double, number_of_states, number_of_inputs> B;
- const Eigen::Matrix<double, number_of_outputs, number_of_states> C;
- const Eigen::Matrix<double, number_of_outputs, number_of_inputs> D;
- const Eigen::Matrix<double, number_of_inputs, 1> U_min;
- const Eigen::Matrix<double, number_of_inputs, 1> U_max;
-
StateFeedbackPlantCoefficients(const StateFeedbackPlantCoefficients &other)
- : A(other.A),
- B(other.B),
- C(other.C),
- D(other.D),
- U_min(other.U_min),
- U_max(other.U_max) {
+ : A_(other.A()),
+ B_(other.B()),
+ C_(other.C()),
+ D_(other.D()),
+ U_min_(other.U_min()),
+ U_max_(other.U_max()) {
}
StateFeedbackPlantCoefficients(
@@ -43,19 +32,49 @@
const Eigen::Matrix<double, number_of_outputs, number_of_inputs> &D,
const Eigen::Matrix<double, number_of_outputs, 1> &U_max,
const Eigen::Matrix<double, number_of_outputs, 1> &U_min)
- : A(A),
- B(B),
- C(C),
- D(D),
- U_min(U_min),
- U_max(U_max) {
+ : A_(A),
+ B_(B),
+ C_(C),
+ D_(D),
+ U_min_(U_min),
+ U_max_(U_max) {
}
- protected:
- // these are accessible from non-templated subclasses
- static const int kNumStates = number_of_states;
- static const int kNumOutputs = number_of_outputs;
- static const int kNumInputs = number_of_inputs;
+ const Eigen::Matrix<double, number_of_states, number_of_states> &A() const {
+ return A_;
+ }
+ double A(int i, int j) const { return A()(i, j); }
+ const Eigen::Matrix<double, number_of_states, number_of_inputs> &B() const {
+ return B_;
+ }
+ double B(int i, int j) const { return B()(i, j); }
+ const Eigen::Matrix<double, number_of_outputs, number_of_states> &C() const {
+ return C_;
+ }
+ double C(int i, int j) const { return C()(i, j); }
+ const Eigen::Matrix<double, number_of_outputs, number_of_inputs> &D() const {
+ return D_;
+ }
+ double D(int i, int j) const { return D()(i, j); }
+ const Eigen::Matrix<double, number_of_inputs, 1> &U_min() const {
+ return U_min_;
+ }
+ double U_min(int i) const { return U_min()(i, 0); }
+ const Eigen::Matrix<double, number_of_inputs, 1> &U_max() const {
+ return U_max_;
+ }
+ double U_max(int i) const { return U_max()(i, 0); }
+
+ private:
+ const Eigen::Matrix<double, number_of_states, number_of_states> A_;
+ const Eigen::Matrix<double, number_of_states, number_of_inputs> B_;
+ const Eigen::Matrix<double, number_of_outputs, number_of_states> C_;
+ const Eigen::Matrix<double, number_of_outputs, number_of_inputs> D_;
+ const Eigen::Matrix<double, number_of_inputs, 1> U_min_;
+ const Eigen::Matrix<double, number_of_inputs, 1> U_max_;
+
+ StateFeedbackPlantCoefficients &operator=(
+ StateFeedbackPlantCoefficients other) = delete;
};
template <int number_of_states, int number_of_inputs, int number_of_outputs>
@@ -75,9 +94,9 @@
StateFeedbackPlant(StateFeedbackPlant &&other)
: plant_index_(other.plant_index_) {
::std::swap(coefficients_, other.coefficients_);
- X.swap(other.X);
- Y.swap(other.Y);
- U.swap(other.U);
+ X_.swap(other.X_);
+ Y_.swap(other.Y_);
+ U_.swap(other.U_);
}
virtual ~StateFeedbackPlant() {
@@ -86,33 +105,44 @@
}
}
- ::std::vector<StateFeedbackPlantCoefficients<
- number_of_states, number_of_inputs, number_of_outputs> *> coefficients_;
-
const Eigen::Matrix<double, number_of_states, number_of_states> &A() const {
- return coefficients().A;
+ return coefficients().A();
}
double A(int i, int j) const { return A()(i, j); }
const Eigen::Matrix<double, number_of_states, number_of_inputs> &B() const {
- return coefficients().B;
+ return coefficients().B();
}
double B(int i, int j) const { return B()(i, j); }
const Eigen::Matrix<double, number_of_outputs, number_of_states> &C() const {
- return coefficients().C;
+ return coefficients().C();
}
double C(int i, int j) const { return C()(i, j); }
const Eigen::Matrix<double, number_of_outputs, number_of_inputs> &D() const {
- return coefficients().D;
+ return coefficients().D();
}
double D(int i, int j) const { return D()(i, j); }
const Eigen::Matrix<double, number_of_inputs, 1> &U_min() const {
- return coefficients().U_min;
+ return coefficients().U_min();
}
- double U_min(int i, int j) const { return U_min()(i, j); }
+ double U_min(int i) const { return U_min()(i, 0); }
const Eigen::Matrix<double, number_of_inputs, 1> &U_max() const {
- return coefficients().U_max;
+ return coefficients().U_max();
}
- double U_max(int i, int j) const { return U_max()(i, j); }
+ double U_max(int i) const { return U_max()(i, 0); }
+
+ const Eigen::Matrix<double, number_of_states, 1> &X() const { return X_; }
+ double X(int i) const { return X()(i, 0); }
+ const Eigen::Matrix<double, number_of_outputs, 1> &Y() const { return Y_; }
+ double Y(int i) const { return Y()(i, 0); }
+ const Eigen::Matrix<double, number_of_inputs, 1> &U() const { return U_; }
+ double U(int i) const { return X()(i, 0); }
+
+ Eigen::Matrix<double, number_of_states, 1> &change_X() { return X_; }
+ double &change_X(int i) { return change_X()(i, 0); }
+ Eigen::Matrix<double, number_of_outputs, 1> &change_Y() { return Y_; }
+ double &change_Y(int i) { return change_Y()(i, 0); }
+ Eigen::Matrix<double, number_of_inputs, 1> &change_U() { return U_; }
+ double &change_U(int i) { return change_U()(i, 0); }
const StateFeedbackPlantCoefficients<
number_of_states, number_of_inputs, number_of_outputs>
@@ -132,20 +162,16 @@
}
void Reset() {
- X.setZero();
- Y.setZero();
- U.setZero();
+ X_.setZero();
+ Y_.setZero();
+ U_.setZero();
}
- Eigen::Matrix<double, number_of_states, 1> X;
- Eigen::Matrix<double, number_of_outputs, 1> Y;
- Eigen::Matrix<double, number_of_inputs, 1> U;
-
// Assert that U is within the hardware range.
virtual void CheckU() {
for (int i = 0; i < kNumOutputs; ++i) {
- assert(U(i, 0) <= U_max(i, 0) + 0.00001);
- assert(U(i, 0) >= U_min(i, 0) - 0.00001);
+ assert(U(i) <= U_max(i) + 0.00001);
+ assert(U(i) >= U_min(i) - 0.00001);
}
}
@@ -154,8 +180,8 @@
// Powers outside of the range are more likely controller bugs than things
// that the plant should deal with.
CheckU();
- X = A() * X + B() * U;
- Y = C() * X + D() * U;
+ X_ = A() * X() + B() * U();
+ Y_ = C() * X() + D() * U();
}
protected:
@@ -165,6 +191,13 @@
static const int kNumInputs = number_of_inputs;
private:
+ Eigen::Matrix<double, number_of_states, 1> X_;
+ Eigen::Matrix<double, number_of_outputs, 1> Y_;
+ Eigen::Matrix<double, number_of_inputs, 1> U_;
+
+ ::std::vector<StateFeedbackPlantCoefficients<
+ number_of_states, number_of_inputs, number_of_outputs> *> coefficients_;
+
int plant_index_;
DISALLOW_COPY_AND_ASSIGN(StateFeedbackPlant);
@@ -174,8 +207,9 @@
// This is designed such that multiple controllers can share one set of state to
// support gain scheduling easily.
template <int number_of_states, int number_of_inputs, int number_of_outputs>
-struct StateFeedbackController {
+struct StateFeedbackController final {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
const Eigen::Matrix<double, number_of_states, number_of_outputs> L;
const Eigen::Matrix<double, number_of_outputs, number_of_states> K;
StateFeedbackPlantCoefficients<number_of_states, number_of_inputs,
@@ -205,24 +239,6 @@
Reset();
}
- StateFeedbackLoop(const ::std::vector<StateFeedbackController<
- number_of_states, number_of_inputs, number_of_outputs> *> &controllers)
- : controllers_(controllers), controller_index_(0) {
- Reset();
- }
-
- StateFeedbackLoop(StateFeedbackLoop &&other) {
- X_hat.swap(other.X_hat);
- R.swap(other.R);
- U.swap(other.U);
- U_uncapped.swap(other.U_uncapped);
- U_ff.swap(other.U_ff);
- ::std::swap(controllers_, other.controllers_);
- Y_.swap(other.Y_);
- new_y_ = other.new_y_;
- controller_index_ = other.controller_index_;
- }
-
StateFeedbackLoop(
const Eigen::Matrix<double, number_of_states, number_of_outputs> &L,
const Eigen::Matrix<double, number_of_outputs, number_of_states> &K,
@@ -236,6 +252,23 @@
Reset();
}
+ StateFeedbackLoop(const ::std::vector<StateFeedbackController<
+ number_of_states, number_of_inputs, number_of_outputs> *> &controllers)
+ : controllers_(controllers), controller_index_(0) {
+ Reset();
+ }
+
+ StateFeedbackLoop(StateFeedbackLoop &&other) {
+ X_hat_.swap(other.X_hat_);
+ R_.swap(other.R_);
+ U_.swap(other.U_);
+ U_uncapped_.swap(other.U_uncapped_);
+ ::std::swap(controllers_, other.controllers_);
+ Y_.swap(other.Y_);
+ new_y_ = other.new_y_;
+ controller_index_ = other.controller_index_;
+ }
+
virtual ~StateFeedbackLoop() {
for (auto c : controllers_) {
delete c;
@@ -243,21 +276,30 @@
}
const Eigen::Matrix<double, number_of_states, number_of_states> &A() const {
- return controller().plant.A;
+ return controller().plant.A();
}
double A(int i, int j) const { return A()(i, j); }
const Eigen::Matrix<double, number_of_states, number_of_inputs> &B() const {
- return controller().plant.B;
+ return controller().plant.B();
}
double B(int i, int j) const { return B()(i, j); }
const Eigen::Matrix<double, number_of_outputs, number_of_states> &C() const {
- return controller().plant.C;
+ return controller().plant.C();
}
double C(int i, int j) const { return C()(i, j); }
const Eigen::Matrix<double, number_of_outputs, number_of_inputs> &D() const {
- return controller().plant.D;
+ return controller().plant.D();
}
double D(int i, int j) const { return D()(i, j); }
+ const Eigen::Matrix<double, number_of_inputs, 1> &U_min() const {
+ return controller().plant.U_min();
+ }
+ double U_min(int i) const { return U_min()(i, 0); }
+ const Eigen::Matrix<double, number_of_inputs, 1> &U_max() const {
+ return controller().plant.U_max();
+ }
+ double U_max(int i) const { return U_max()(i, 0); }
+
const Eigen::Matrix<double, number_of_outputs, number_of_states> &K() const {
return controller().K;
}
@@ -266,14 +308,34 @@
return controller().L;
}
double L(int i, int j) const { return L()(i, j); }
- const Eigen::Matrix<double, number_of_inputs, 1> &U_min() const {
- return controller().plant.U_min;
+
+ const Eigen::Matrix<double, number_of_states, 1> &X_hat() const {
+ return X_hat_;
}
- double U_min(int i, int j) const { return U_min()(i, j); }
- const Eigen::Matrix<double, number_of_inputs, 1> &U_max() const {
- return controller().plant.U_max;
+ double X_hat(int i) const { return X_hat()(i, 0); }
+ const Eigen::Matrix<double, number_of_states, 1> &R() const { return R_; }
+ double R(int i) const { return R()(i, 0); }
+ const Eigen::Matrix<double, number_of_inputs, 1> &U() const { return U_; }
+ double U(int i) const { return U()(i, 0); }
+ const Eigen::Matrix<double, number_of_inputs, 1> &U_uncapped() const {
+ return U_uncapped_;
}
- double U_max(int i, int j) const { return U_max()(i, j); }
+ double U_uncapped(int i) const { return U_uncapped()(i, 0); }
+ const Eigen::Matrix<double, number_of_outputs, 1> &Y() const { return Y_; }
+ double Y(int i) const { return Y()(i, 0); }
+
+ Eigen::Matrix<double, number_of_states, 1> &change_X_hat() { return X_hat_; }
+ double &change_X_hat(int i) { return change_X_hat()(i, 0); }
+ Eigen::Matrix<double, number_of_states, 1> &change_R() { return R_; }
+ double &change_R(int i) { return change_R()(i, 0); }
+ Eigen::Matrix<double, number_of_inputs, 1> &change_U() { return U_; }
+ double &change_U(int i) { return change_U()(i, 0); }
+ Eigen::Matrix<double, number_of_inputs, 1> &change_U_uncapped() {
+ return U_uncapped_;
+ }
+ double &change_U_uncapped(int i) { return change_U_uncapped()(i, 0); }
+ Eigen::Matrix<double, number_of_outputs, 1> &change_Y() { return Y_; }
+ double &change_Y(int i) { return change_Y()(i, 0); }
const StateFeedbackController<number_of_states, number_of_inputs,
number_of_outputs> &controller() const {
@@ -287,27 +349,20 @@
}
void Reset() {
- X_hat.setZero();
- R.setZero();
- U.setZero();
- U_uncapped.setZero();
- U_ff.setZero();
- }
-
- virtual void FeedForward() {
- for (int i = 0; i < number_of_outputs; ++i) {
- U_ff[i] = 0.0;
- }
+ X_hat_.setZero();
+ R_.setZero();
+ U_.setZero();
+ U_uncapped_.setZero();
}
// If U is outside the hardware range, limit it before the plant tries to use
// it.
virtual void CapU() {
for (int i = 0; i < kNumOutputs; ++i) {
- if (U(i, 0) > U_max(i, 0)) {
- U(i, 0) = U_max(i, 0);
- } else if (U(i, 0) < U_min(i, 0)) {
- U(i, 0) = U_min(i, 0);
+ if (U(i) > U_max(i)) {
+ U_(i, 0) = U_max(i);
+ } else if (U(i) < U_min(i)) {
+ U_(i, 0) = U_min(i);
}
}
}
@@ -338,31 +393,26 @@
// stop_motors is whether or not to output all 0s.
void Update(bool stop_motors) {
if (stop_motors) {
- U.setZero();
- U_uncapped.setZero();
+ U_.setZero();
+ U_uncapped_.setZero();
} else {
- U = U_uncapped = K() * (R - X_hat);
+ U_ = U_uncapped_ = K() * (R() - X_hat());
CapU();
}
- //::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;
+ X_hat_ = (A() - L() * C()) * X_hat() + L() * Y() + B() * U();
new_y_ = false;
} else {
- X_hat = A() * X_hat + B() * U;
+ X_hat_ = A() * X_hat() + B() * U();
}
}
- // Sets the current controller to be index and verifies that it isn't out of
- // range.
+ // Sets the current controller to be index, clamped to be within range.
void set_controller_index(int index) {
if (index < 0) {
controller_index_ = 0;
@@ -375,21 +425,21 @@
int controller_index() const { return controller_index_; }
- Eigen::Matrix<double, number_of_states, 1> X_hat;
- Eigen::Matrix<double, number_of_states, 1> R;
- 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;
-
protected:
::std::vector<StateFeedbackController<number_of_states, number_of_inputs,
number_of_outputs> *> controllers_;
- // these are accessible from non-templated subclasses
+ // These are accessible from non-templated subclasses.
static const int kNumStates = number_of_states;
static const int kNumOutputs = number_of_outputs;
static const int kNumInputs = number_of_inputs;
+ private:
+ Eigen::Matrix<double, number_of_states, 1> X_hat_;
+ Eigen::Matrix<double, number_of_states, 1> R_;
+ Eigen::Matrix<double, number_of_inputs, 1> U_;
+ Eigen::Matrix<double, number_of_inputs, 1> U_uncapped_;
+
// 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_;
@@ -397,8 +447,7 @@
int controller_index_;
- private:
DISALLOW_COPY_AND_ASSIGN(StateFeedbackLoop);
};
-#endif // FRC971_CONTROL_LOOPS_STATEFEEDBACKLOOP_H_
+#endif // FRC971_CONTROL_LOOPS_STATE_FEEDBACK_LOOP_H_