Decouple profiled subsystem axis from inputs and outputs
If you want to put an indexer in with a turret, you need to
decouple these...
Change-Id: Ie8a999cc10972e67cd5ce822710d305dbf4ce06a
diff --git a/frc971/control_loops/profiled_subsystem.h b/frc971/control_loops/profiled_subsystem.h
index 4d09bbc..6ebba72 100644
--- a/frc971/control_loops/profiled_subsystem.h
+++ b/frc971/control_loops/profiled_subsystem.h
@@ -24,12 +24,15 @@
// styles.
template <int number_of_states, int number_of_axes,
class ZeroingEstimator =
- ::frc971::zeroing::PotAndIndexPulseZeroingEstimator>
+ ::frc971::zeroing::PotAndIndexPulseZeroingEstimator,
+ int number_of_inputs = number_of_axes,
+ int number_of_outputs = number_of_axes>
class ProfiledSubsystem {
public:
ProfiledSubsystem(
::std::unique_ptr<::frc971::control_loops::SimpleCappedStateFeedbackLoop<
- number_of_states, number_of_axes, number_of_axes>> loop,
+ number_of_states, number_of_inputs, number_of_outputs>>
+ loop,
::std::array<ZeroingEstimator, number_of_axes> &&estimators)
: loop_(::std::move(loop)), estimators_(::std::move(estimators)) {
zeroed_.fill(false);
@@ -55,7 +58,7 @@
}
// Returns the controller.
- const StateFeedbackLoop<number_of_states, number_of_axes, number_of_axes> &
+ const StateFeedbackLoop<number_of_states, number_of_inputs, number_of_outputs> &
controller() const {
return *loop_;
}
@@ -102,8 +105,8 @@
}
// Sets the maximum voltage that will be commanded by the loop.
- void set_max_voltage(::std::array<double, number_of_axes> voltages) {
- for (int i = 0; i < number_of_axes; ++i) {
+ void set_max_voltage(::std::array<double, number_of_inputs> voltages) {
+ for (int i = 0; i < number_of_inputs; ++i) {
loop_->set_max_voltage(i, voltages[i]);
}
}
@@ -114,7 +117,8 @@
// TODO(austin): It's a bold assumption to assume that we will have the same
// number of sensors as axes. So far, that's been fine.
::std::unique_ptr<::frc971::control_loops::SimpleCappedStateFeedbackLoop<
- number_of_states, number_of_axes, number_of_axes>> loop_;
+ number_of_states, number_of_inputs, number_of_outputs>>
+ loop_;
// The goal that the profile tries to reach.
Eigen::Matrix<double, number_of_states, 1> unprofiled_goal_;