Create a nice general SimpleCappedStateFeedbackLoop
Change-Id: I79e686b1c6d689735b73634ccf9877e00bac4348
diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index 26d00a5..5eeed1d 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -111,3 +111,14 @@
'//aos/common:macros',
],
)
+
+cc_library(
+ name = 'simple_capped_state_feedback_loop',
+ hdrs = [
+ 'simple_capped_state_feedback_loop.h',
+ ],
+ deps = [
+ '//third_party/eigen',
+ ':state_feedback_loop',
+ ],
+)
diff --git a/frc971/control_loops/simple_capped_state_feedback_loop.h b/frc971/control_loops/simple_capped_state_feedback_loop.h
new file mode 100644
index 0000000..38994e0
--- /dev/null
+++ b/frc971/control_loops/simple_capped_state_feedback_loop.h
@@ -0,0 +1,71 @@
+#ifndef FRC971_CONTROL_LOOPS_SIMPLE_CAPPED_STATE_FEEDBACK_LOOP_H_
+#define FRC971_CONTROL_LOOPS_SIMPLE_CAPPED_STATE_FEEDBACK_LOOP_H_
+
+#include "Eigen/Dense"
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+// A StateFeedbackLoop which implements CapU based on a simple set of maximum
+// absolute values for each element of U.
+template <int number_of_states, int number_of_inputs, int number_of_outputs>
+class SimpleCappedStateFeedbackLoop
+ : public StateFeedbackLoop<number_of_states, number_of_inputs,
+ number_of_outputs> {
+ public:
+ SimpleCappedStateFeedbackLoop(StateFeedbackLoop<
+ number_of_states, number_of_inputs, number_of_outputs> &&loop)
+ : StateFeedbackLoop<number_of_states, number_of_inputs,
+ number_of_outputs>(::std::move(loop)) {}
+
+ void set_max_voltages(
+ const ::Eigen::Array<double, number_of_inputs, 1> &max_voltages) {
+ max_voltages_ = max_voltages;
+ }
+ void set_max_voltage(int i, double max_voltage) {
+ mutable_max_voltage(i) = max_voltage;
+ }
+
+ // Easier to use overloads for number_of_inputs == 1 or 2. Using the wrong one
+ // will result in a compile-time Eigen error about mixing matrices of
+ // different sizes.
+ void set_max_voltages(double v1) {
+ set_max_voltages((::Eigen::Array<double, 1, 1>() << v1).finished());
+ }
+ void set_max_voltages(double v1, double v2) {
+ set_max_voltages((::Eigen::Array<double, 2, 1>() << v1, v2).finished());
+ }
+
+ const ::Eigen::Array<double, number_of_inputs, 1> &max_voltages() const {
+ return max_voltages_;
+ }
+ ::Eigen::Array<double, number_of_inputs, 1> &mutable_max_voltages() {
+ return max_voltages_;
+ }
+
+ double max_voltage(int i) const { return max_voltages()(i, 0); }
+ double max_voltage(double) const = delete;
+ double &mutable_max_voltage(int i) { return mutable_max_voltages()(i, 0); }
+ double &mutable_max_voltage(double) = delete;
+
+ // Don't accidentally call these when you mean to call set_max_voltages
+ // with a low number_of_inputs.
+ void set_max_voltage(double) = delete;
+ void set_max_voltage(double, double) = delete;
+
+ private:
+ void CapU() override {
+ this->mutable_U() =
+ this->U().array().min(max_voltages_).max(-max_voltages_);
+ }
+
+ ::Eigen::Array<double, number_of_inputs, 1> max_voltages_ =
+ ::Eigen::Array<double, number_of_inputs, 1>::Constant(12);
+};
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_SIMPLE_CAPPED_STATE_FEEDBACK_LOOP_H_
diff --git a/y2016/control_loops/superstructure/BUILD b/y2016/control_loops/superstructure/BUILD
index c584135..17653ad 100644
--- a/y2016/control_loops/superstructure/BUILD
+++ b/y2016/control_loops/superstructure/BUILD
@@ -78,6 +78,7 @@
'//aos/common/controls:control_loop',
'//aos/common/util:trapezoid_profile',
'//frc971/control_loops:state_feedback_loop',
+ '//frc971/control_loops:simple_capped_state_feedback_loop',
'//frc971/zeroing',
'//y2016:constants',
],
diff --git a/y2016/control_loops/superstructure/superstructure_controls.cc b/y2016/control_loops/superstructure/superstructure_controls.cc
index 7cd67e3..3c3e690 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.cc
+++ b/y2016/control_loops/superstructure/superstructure_controls.cc
@@ -25,22 +25,10 @@
}
} // namespace
-void SimpleCappedStateFeedbackLoop::CapU() {
- mutable_U(0, 0) = ::std::min(U(0, 0), max_voltage_);
- mutable_U(0, 0) = ::std::max(U(0, 0), -max_voltage_);
-}
-
-void DoubleCappedStateFeedbackLoop::CapU() {
- mutable_U(0, 0) = ::std::min(U(0, 0), shoulder_max_voltage_);
- mutable_U(0, 0) = ::std::max(U(0, 0), -shoulder_max_voltage_);
- mutable_U(1, 0) = ::std::min(U(1, 0), wrist_max_voltage_);
- mutable_U(1, 0) = ::std::max(U(1, 0), -wrist_max_voltage_);
-}
-
// Intake
Intake::Intake()
- : loop_(new SimpleCappedStateFeedbackLoop(StateFeedbackLoop<3, 1, 1>(
- ::y2016::control_loops::superstructure::MakeIntegralIntakeLoop()))),
+ : loop_(new ::frc971::control_loops::SimpleCappedStateFeedbackLoop<3, 1, 1>(
+ ::y2016::control_loops::superstructure::MakeIntegralIntakeLoop())),
estimator_(constants::GetValues().intake.zeroing),
profile_(::aos::controls::kLoopFrequency) {
Y_.setZero();
@@ -147,7 +135,7 @@
}
void Intake::set_max_voltage(double voltage) {
- loop_->set_max_voltage(voltage);
+ loop_->set_max_voltages(voltage);
}
void Intake::AdjustProfile(double max_angular_velocity,
@@ -171,7 +159,7 @@
}
Arm::Arm()
- : loop_(new DoubleCappedStateFeedbackLoop(
+ : loop_(new ::frc971::control_loops::SimpleCappedStateFeedbackLoop<6, 2, 2>(
::y2016::control_loops::superstructure::MakeIntegralArmLoop())),
shoulder_profile_(::aos::controls::kLoopFrequency),
wrist_profile_(::aos::controls::kLoopFrequency),
@@ -368,7 +356,7 @@
void Arm::set_max_voltage(double shoulder_max_voltage,
double wrist_max_voltage) {
- loop_->set_max_voltage(shoulder_max_voltage, wrist_max_voltage);
+ loop_->set_max_voltages(shoulder_max_voltage, wrist_max_voltage);
}
void Arm::Reset() {
diff --git a/y2016/control_loops/superstructure/superstructure_controls.h b/y2016/control_loops/superstructure/superstructure_controls.h
index d96e24d..f3d8752 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.h
+++ b/y2016/control_loops/superstructure/superstructure_controls.h
@@ -5,6 +5,7 @@
#include "aos/common/controls/control_loop.h"
#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/control_loops/simple_capped_state_feedback_loop.h"
#include "aos/common/util/trapezoid_profile.h"
#include "frc971/zeroing/zeroing.h"
@@ -17,41 +18,6 @@
class SuperstructureTest_DisabledGoalTest_Test;
} // namespace testing
-class SimpleCappedStateFeedbackLoop : public StateFeedbackLoop<3, 1, 1> {
- public:
- SimpleCappedStateFeedbackLoop(StateFeedbackLoop<3, 1, 1> &&loop)
- : StateFeedbackLoop<3, 1, 1>(::std::move(loop)), max_voltage_(12.0) {}
-
- void set_max_voltage(double max_voltage) {
- max_voltage_ = ::std::max(0.0, ::std::min(12.0, max_voltage));
- }
-
- void CapU() override;
-
- private:
- double max_voltage_;
-};
-
-class DoubleCappedStateFeedbackLoop : public StateFeedbackLoop<6, 2, 2> {
- public:
- DoubleCappedStateFeedbackLoop(StateFeedbackLoop<6, 2, 2> &&loop)
- : StateFeedbackLoop<6, 2, 2>(::std::move(loop)),
- shoulder_max_voltage_(12.0),
- wrist_max_voltage_(12.0) {}
-
- void set_max_voltage(double shoulder_max_voltage, double wrist_max_voltage) {
- shoulder_max_voltage_ =
- ::std::max(0.0, ::std::min(12.0, shoulder_max_voltage));
- wrist_max_voltage_ = ::std::max(0.0, ::std::min(12.0, wrist_max_voltage));
- }
-
- void CapU() override;
-
- private:
- double shoulder_max_voltage_;
- double wrist_max_voltage_;
-};
-
class Intake {
public:
Intake();
@@ -114,7 +80,8 @@
void UpdateIntakeOffset(double offset);
- ::std::unique_ptr<SimpleCappedStateFeedbackLoop> loop_;
+ ::std::unique_ptr<
+ ::frc971::control_loops::SimpleCappedStateFeedbackLoop<3, 1, 1>> loop_;
::frc971::zeroing::ZeroingEstimator estimator_;
aos::util::TrapezoidProfile profile_;
@@ -204,7 +171,8 @@
void UpdateShoulderOffset(double offset);
friend class testing::SuperstructureTest_DisabledGoalTest_Test;
- ::std::unique_ptr<DoubleCappedStateFeedbackLoop> loop_;
+ ::std::unique_ptr<
+ ::frc971::control_loops::SimpleCappedStateFeedbackLoop<6, 2, 2>> loop_;
aos::util::TrapezoidProfile shoulder_profile_, wrist_profile_;
::frc971::zeroing::ZeroingEstimator shoulder_estimator_, wrist_estimator_;