Ported the polydrivetrain code over to C++.
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index ff5e9d8..4c35419 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -223,15 +223,18 @@
Eigen::Matrix<double, number_of_outputs, 1> U_ff;
Eigen::Matrix<double, number_of_outputs, 1> Y;
- ::std::vector<StateFeedbackController<number_of_states, number_of_inputs,
- number_of_outputs> *> controllers_;
-
const StateFeedbackController<
number_of_states, number_of_inputs, number_of_outputs>
&controller() const {
return *controllers_[controller_index_];
}
+ const StateFeedbackController<
+ number_of_states, number_of_inputs, number_of_outputs>
+ &controller(int index) const {
+ return *controllers_[index];
+ }
+
void Reset() {
X_hat.setZero();
R.setZero();
@@ -324,6 +327,9 @@
void controller_index() const { return controller_index_; }
protected:
+ ::std::vector<StateFeedbackController<number_of_states, number_of_inputs,
+ number_of_outputs> *> controllers_;
+
// these are accessible from non-templated subclasses
static const int kNumStates = number_of_states;
static const int kNumOutputs = number_of_outputs;