State feedback loop now supports gain scheduling.
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 894487b..f955796 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -80,13 +80,65 @@
static const int kNumInputs = number_of_inputs;
};
+// A Controller is a structure which holds a plant and the K and L matrices.
+// 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 {
+ 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;
+ StateFeedbackPlant<number_of_states, number_of_inputs,
+ number_of_outputs> plant;
+
+ StateFeedbackController(
+ const Eigen::Matrix<double, number_of_states, number_of_outputs> &L,
+ const Eigen::Matrix<double, number_of_outputs, number_of_states> &K,
+ const StateFeedbackPlant<number_of_states, number_of_inputs,
+ number_of_outputs> &plant)
+ : L(L),
+ K(K),
+ plant(plant) {
+ }
+};
+
template <int number_of_states, int number_of_inputs, int number_of_outputs>
class StateFeedbackLoop {
public:
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;
+ const Eigen::Matrix<double, number_of_states, number_of_states> &A() const {
+ 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;
+ }
+ 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;
+ }
+ 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;
+ }
+ double D(int i, int j) const { return D()(i, j); }
+ const Eigen::Matrix<double, number_of_outputs, number_of_states> &K() const {
+ return controller().K;
+ }
+ double K(int i, int j) const { return K()(i, j); }
+ const Eigen::Matrix<double, number_of_states, number_of_outputs> &L() const {
+ 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;
+ }
+ 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 U_max(int i, int j) const { return U_max()(i, j); }
Eigen::Matrix<double, number_of_states, 1> X_hat;
Eigen::Matrix<double, number_of_states, 1> R;
@@ -95,17 +147,16 @@
Eigen::Matrix<double, number_of_outputs, 1> U_ff;
Eigen::Matrix<double, number_of_outputs, 1> Y;
- StateFeedbackPlant<number_of_states, number_of_inputs,
- number_of_outputs> plant;
+ ::std::vector<StateFeedbackController<number_of_states, number_of_inputs,
+ number_of_outputs> *> controllers;
- StateFeedbackLoop(
- const Eigen::Matrix<double, number_of_states, number_of_outputs> &L,
- const Eigen::Matrix<double, number_of_outputs, number_of_states> &K,
- const StateFeedbackPlant<number_of_states, number_of_inputs,
- number_of_outputs> &plant)
- : L(L),
- K(K),
- plant(plant) {
+ const StateFeedbackController<
+ number_of_states, number_of_inputs, number_of_outputs>
+ &controller() const {
+ return *controllers[controller_index_];
+ }
+
+ void Reset() {
X_hat.setZero();
R.setZero();
U.setZero();
@@ -113,6 +164,36 @@
U_ff.setZero();
Y.setZero();
}
+
+ StateFeedbackLoop(const StateFeedbackPlant<number_of_states, number_of_inputs,
+ number_of_outputs> &controller)
+ : controller_index_(0) {
+ controllers.push_back(
+ new StateFeedbackController<number_of_states, number_of_inputs,
+ number_of_outputs>(controller));
+ Reset();
+ }
+
+ StateFeedbackLoop(
+ const ::std::vector<StateFeedbackPlant<number_of_states, number_of_inputs,
+ number_of_outputs> *> &controllers)
+ : controllers(controllers),
+ controller_index_(0) {
+ Reset();
+ }
+
+ StateFeedbackLoop(
+ const Eigen::Matrix<double, number_of_states, number_of_outputs> &L,
+ const Eigen::Matrix<double, number_of_outputs, number_of_states> &K,
+ const StateFeedbackPlant<number_of_states, number_of_inputs,
+ number_of_outputs> &plant)
+ : controller_index_(0) {
+ controllers.push_back(
+ new StateFeedbackController<number_of_states, number_of_inputs,
+ number_of_outputs>(L, K, plant));
+
+ Reset();
+ }
virtual ~StateFeedbackLoop() {}
virtual void FeedForward() {
@@ -125,10 +206,10 @@
// it.
virtual void CapU() {
for (int i = 0; i < kNumOutputs; ++i) {
- if (U[i] > plant.U_max[i]) {
- U[i] = plant.U_max[i];
- } else if (U[i] < plant.U_min[i]) {
- U[i] = plant.U_min[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);
}
}
}
@@ -137,26 +218,36 @@
// stop_motors is whether or not to output all 0s.
void Update(bool update_observer, bool stop_motors) {
if (stop_motors) {
- for (int i = 0; i < number_of_outputs; ++i) {
- U[i] = 0.0;
- }
+ U.setZero();
} else {
- U = U_uncapped = K * (R - X_hat);
+ U = U_uncapped = K() * (R - X_hat);
CapU();
}
if (update_observer) {
- X_hat = (plant.A - L * plant.C) * X_hat + L * Y + plant.B * U;
+ X_hat = (A() - L() * C()) * X_hat + L() * Y + B() * U;
} else {
- X_hat = plant.A * X_hat + plant.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.
+ void set_controller_index(int index) {
+ if (index >= 0 && index < controllers.size()) {
+ controller_index_ = index;
+ }
+ }
+
+ void controller_index() const { return controller_index_; }
+
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;
+
+ int controller_index_;
};
#endif // FRC971_CONTROL_LOOPS_STATEFEEDBACKLOOP_H_