Factor out a generic "standard subsystem"
This is a work in progress. More refactoring is needed.
Change-Id: I02440af5111e2810e172c8a23c2134ab0571c4d5
diff --git a/y2016/control_loops/superstructure/superstructure_controls.cc b/y2016/control_loops/superstructure/superstructure_controls.cc
index eb6432d..7d4dea5 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.cc
+++ b/y2016/control_loops/superstructure/superstructure_controls.cc
@@ -23,16 +23,22 @@
return default_value;
}
}
+
+enum ArmIndices { kShoulderIndex = 0, kWristIndex = 1 };
+
} // namespace
// Intake
Intake::Intake()
- : loop_(new ::frc971::control_loops::SimpleCappedStateFeedbackLoop<3, 1, 1>(
- ::y2016::control_loops::superstructure::MakeIntegralIntakeLoop())),
+ : ProfiledSubsystem(
+ ::std::unique_ptr<
+ ::frc971::control_loops::SimpleCappedStateFeedbackLoop<3, 1, 1>>(
+ new ::frc971::control_loops::SimpleCappedStateFeedbackLoop<
+ 3, 1, 1>(::y2016::control_loops::superstructure::
+ MakeIntegralIntakeLoop()))),
estimator_(constants::GetValues().intake.zeroing),
profile_(::aos::controls::kLoopFrequency) {
Y_.setZero();
- unprofiled_goal_.setZero();
offset_.setZero();
AdjustProfile(0.0, 0.0);
}
@@ -66,9 +72,9 @@
}
}
- if (!zeroed_ && estimator_.zeroed()) {
+ if (!zeroed(0) && estimator_.zeroed()) {
UpdateIntakeOffset(estimator_.offset());
- zeroed_ = true;
+ set_zeroed(0, true);
}
Y_ << position.encoder;
@@ -148,10 +154,8 @@
UseUnlessZero(max_angular_acceleration, 10.0));
}
-void Intake::Reset() {
+void Intake::DoReset() {
estimator_.Reset();
- initialized_ = false;
- zeroed_ = false;
}
EstimatorState Intake::IntakeEstimatorState() {
@@ -162,15 +166,14 @@
}
Arm::Arm()
- : loop_(new ArmControlLoop(
- ::y2016::control_loops::superstructure::MakeIntegralArmLoop())),
+ : ProfiledSubsystem(::std::unique_ptr<ArmControlLoop>(new ArmControlLoop(
+ ::y2016::control_loops::superstructure::MakeIntegralArmLoop()))),
shoulder_profile_(::aos::controls::kLoopFrequency),
wrist_profile_(::aos::controls::kLoopFrequency),
shoulder_estimator_(constants::GetValues().shoulder.zeroing),
wrist_estimator_(constants::GetValues().wrist.zeroing) {
Y_.setZero();
offset_.setZero();
- unprofiled_goal_.setZero();
AdjustProfile(0.0, 0.0, 0.0, 0.0);
}
@@ -238,13 +241,13 @@
}
}
- if (!shoulder_zeroed_ && shoulder_estimator_.zeroed()) {
+ if (!zeroed(kShoulderIndex) && shoulder_estimator_.zeroed()) {
UpdateShoulderOffset(shoulder_estimator_.offset());
- shoulder_zeroed_ = true;
+ set_zeroed(kShoulderIndex, true);
}
- if (!wrist_zeroed_ && wrist_estimator_.zeroed()) {
+ if (!zeroed(kWristIndex) && wrist_estimator_.zeroed()) {
UpdateWristOffset(wrist_estimator_.offset());
- wrist_zeroed_ = true;
+ set_zeroed(kWristIndex, true);
}
{
@@ -373,12 +376,9 @@
loop_->set_max_voltage(1, wrist_max_voltage);
}
-void Arm::Reset() {
+void Arm::DoReset() {
shoulder_estimator_.Reset();
wrist_estimator_.Reset();
- initialized_ = false;
- shoulder_zeroed_ = false;
- wrist_zeroed_ = false;
}
EstimatorState Arm::ShoulderEstimatorState() {
diff --git a/y2016/control_loops/superstructure/superstructure_controls.h b/y2016/control_loops/superstructure/superstructure_controls.h
index 2934011..adb40e0 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.h
+++ b/y2016/control_loops/superstructure/superstructure_controls.h
@@ -19,93 +19,6 @@
class SuperstructureTest_DisabledGoalTest_Test;
} // namespace testing
-class Intake {
- public:
- Intake();
- // Returns whether the estimators have been initialized and zeroed.
- bool initialized() const { return initialized_; }
- bool zeroed() const { return zeroed_; }
- // Returns whether an error has occured
- bool error() const { return estimator_.error(); }
-
- // Updates our estimator with the latest position.
- void Correct(::frc971::PotAndIndexPosition position);
- // Runs the controller and profile generator for a cycle.
- void Update(bool disabled);
- // Sets the maximum voltage that will be commanded by the loop.
- void set_max_voltage(double voltage);
-
- // Forces the current goal to the provided goal, bypassing the profiler.
- void ForceGoal(double goal);
- // Sets the unprofiled goal. The profiler will generate a profile to go to
- // this goal.
- void set_unprofiled_goal(double unprofiled_goal);
- // Limits our profiles to a max velocity and acceleration for proper motion.
- void AdjustProfile(double max_angular_velocity,
- double max_angular_acceleration);
-
- // Returns true if we have exceeded any hard limits.
- bool CheckHardLimits();
- // Resets the internal state.
- void Reset();
-
- // Returns the current internal estimator state for logging.
- ::frc971::EstimatorState IntakeEstimatorState();
-
- // Returns the requested intake voltage.
- double intake_voltage() const { return loop_->U(0, 0); }
-
- // Returns the current position.
- double angle() const { return Y_(0, 0); }
-
- // Returns the controller error.
- const StateFeedbackLoop<3, 1, 1> &controller() const { return *loop_; }
-
- // Returns the filtered goal.
- const Eigen::Matrix<double, 3, 1> &goal() const { return loop_->R(); }
- double goal(int row, int col) const { return loop_->R(row, col); }
-
- // Returns the unprofiled goal.
- const Eigen::Matrix<double, 3, 1> &unprofiled_goal() const {
- return unprofiled_goal_;
- }
- double unprofiled_goal(int row, int col) const {
- return unprofiled_goal_(row, col);
- }
-
- // Returns the current state estimate.
- const Eigen::Matrix<double, 3, 1> &X_hat() const { return loop_->X_hat(); }
- double X_hat(int row, int col) const { return loop_->X_hat(row, col); }
-
- // For testing:
- // Triggers an estimator error.
- void TriggerEstimatorError() { estimator_.TriggerError(); }
-
- private:
- // Limits the provided goal to the soft limits. Prints "name" when it fails
- // to aid debugging.
- void CapGoal(const char *name, Eigen::Matrix<double, 3, 1> *goal);
-
- void UpdateIntakeOffset(double offset);
-
- ::std::unique_ptr<
- ::frc971::control_loops::SimpleCappedStateFeedbackLoop<3, 1, 1>> loop_;
-
- ::frc971::zeroing::ZeroingEstimator estimator_;
- aos::util::TrapezoidProfile profile_;
-
- // Current measurement.
- Eigen::Matrix<double, 1, 1> Y_;
- // Current offset. Y_ = offset_ + raw_sensor;
- Eigen::Matrix<double, 1, 1> offset_;
-
- // The goal that the profile tries to reach.
- Eigen::Matrix<double, 3, 1> unprofiled_goal_;
-
- bool initialized_ = false;
- bool zeroed_ = false;
-};
-
class ArmControlLoop
: public ::frc971::control_loops::SimpleCappedStateFeedbackLoop<6, 2, 2> {
public:
@@ -183,14 +96,145 @@
}
};
-class Arm {
+template <int number_of_states, int number_of_axes>
+class ProfiledSubsystem {
public:
- Arm();
+ ProfiledSubsystem(
+ ::std::unique_ptr<::frc971::control_loops::SimpleCappedStateFeedbackLoop<
+ number_of_states, number_of_axes, number_of_axes>>
+ loop)
+ : loop_(::std::move(loop)) {
+ zeroed_.fill(false);
+ unprofiled_goal_.setZero();
+ }
+
+ void Reset() {
+ zeroed_.fill(false);
+ DoReset();
+ }
+
+ // Returns the controller.
+ const StateFeedbackLoop<number_of_states, number_of_axes, number_of_axes>
+ &controller() const {
+ return *loop_;
+ }
+
+ int controller_index() const { return loop_->controller_index(); }
+
// Returns whether the estimators have been initialized and zeroed.
bool initialized() const { return initialized_; }
- bool zeroed() const { return shoulder_zeroed_ && wrist_zeroed_; }
- bool shoulder_zeroed() const { return shoulder_zeroed_; }
- bool wrist_zeroed() const { return wrist_zeroed_; }
+
+ bool zeroed() const {
+ for (int i = 0; i < number_of_axes; ++i) {
+ if (!zeroed_[i]) {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ bool zeroed(int index) const { return zeroed_[index]; };
+
+ // Returns the filtered goal.
+ const Eigen::Matrix<double, number_of_states, 1> &goal() const {
+ return loop_->R();
+ }
+ double goal(int row, int col) const { return loop_->R(row, col); }
+
+ // Returns the unprofiled goal.
+ const Eigen::Matrix<double, number_of_states, 1> &unprofiled_goal() const {
+ return unprofiled_goal_;
+ }
+ double unprofiled_goal(int row, int col) const {
+ return unprofiled_goal_(row, col);
+ }
+
+ // Returns the current state estimate.
+ const Eigen::Matrix<double, number_of_states, 1> &X_hat() const {
+ return loop_->X_hat();
+ }
+ double X_hat(int row, int col) const { return loop_->X_hat(row, col); }
+
+ protected:
+ void set_zeroed(int index, bool val) { zeroed_[index] = val; }
+
+ // 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_;
+
+ // The goal that the profile tries to reach.
+ Eigen::Matrix<double, number_of_states, 1> unprofiled_goal_;
+
+ bool initialized_ = false;
+
+ private:
+ ::std::array<bool, number_of_axes> zeroed_;
+
+ virtual void DoReset() = 0;
+};
+
+class Intake : public ProfiledSubsystem<3, 1> {
+ public:
+ Intake();
+ // Returns whether an error has occured
+ bool error() const { return estimator_.error(); }
+
+ // Updates our estimator with the latest position.
+ void Correct(::frc971::PotAndIndexPosition position);
+ // Runs the controller and profile generator for a cycle.
+ void Update(bool disabled);
+ // Sets the maximum voltage that will be commanded by the loop.
+ void set_max_voltage(double voltage);
+
+ // Forces the current goal to the provided goal, bypassing the profiler.
+ void ForceGoal(double goal);
+ // Sets the unprofiled goal. The profiler will generate a profile to go to
+ // this goal.
+ void set_unprofiled_goal(double unprofiled_goal);
+ // Limits our profiles to a max velocity and acceleration for proper motion.
+ void AdjustProfile(double max_angular_velocity,
+ double max_angular_acceleration);
+
+ // Returns true if we have exceeded any hard limits.
+ bool CheckHardLimits();
+
+ // Returns the current internal estimator state for logging.
+ ::frc971::EstimatorState IntakeEstimatorState();
+
+ // Returns the requested intake voltage.
+ double intake_voltage() const { return loop_->U(0, 0); }
+
+ // Returns the current position.
+ double angle() const { return Y_(0, 0); }
+
+ // For testing:
+ // Triggers an estimator error.
+ void TriggerEstimatorError() { estimator_.TriggerError(); }
+
+ private:
+ // Limits the provided goal to the soft limits. Prints "name" when it fails
+ // to aid debugging.
+ void CapGoal(const char *name, Eigen::Matrix<double, 3, 1> *goal);
+
+ // Resets the internal state.
+ void DoReset() override;
+
+ void UpdateIntakeOffset(double offset);
+
+ ::frc971::zeroing::ZeroingEstimator estimator_;
+ aos::util::TrapezoidProfile profile_;
+
+ // Current measurement.
+ Eigen::Matrix<double, 1, 1> Y_;
+ // Current offset. Y_ = offset_ + raw_sensor;
+ Eigen::Matrix<double, 1, 1> offset_;
+};
+
+class Arm : public ProfiledSubsystem<6, 2> {
+ public:
+ Arm();
// Returns whether an error has occured
bool error() const {
return shoulder_estimator_.error() || wrist_estimator_.error();
@@ -207,8 +251,6 @@
void set_unprofiled_goal(double unprofiled_goal_shoulder,
double unprofiled_goal_wrist);
- int controller_index() const { return loop_->controller_index(); }
-
// Runs the controller and profile generator for a cycle.
void Update(bool disabled);
@@ -226,8 +268,6 @@
// Returns true if we have exceeded any hard limits.
bool CheckHardLimits();
- // Resets the internal state.
- void Reset();
// Returns the current internal estimator state for logging.
::frc971::EstimatorState ShoulderEstimatorState();
@@ -241,30 +281,14 @@
double shoulder_angle() const { return Y_(0, 0); }
double wrist_angle() const { return Y_(1, 0) + Y_(0, 0); }
- // Returns the controller error.
- const StateFeedbackLoop<6, 2, 2> &controller() const { return *loop_; }
-
- // Returns the unprofiled goal.
- const Eigen::Matrix<double, 6, 1> &unprofiled_goal() const {
- return unprofiled_goal_;
- }
- double unprofiled_goal(int row, int col) const {
- return unprofiled_goal_(row, col);
- }
-
- // Returns the filtered goal.
- const Eigen::Matrix<double, 6, 1> &goal() const { return loop_->R(); }
- double goal(int row, int col) const { return loop_->R(row, col); }
-
- // Returns the current state estimate.
- const Eigen::Matrix<double, 6, 1> &X_hat() const { return loop_->X_hat(); }
- double X_hat(int row, int col) const { return loop_->X_hat(row, col); }
-
// For testing:
// Triggers an estimator error.
void TriggerEstimatorError() { shoulder_estimator_.TriggerError(); }
private:
+ // Resets the internal state.
+ void DoReset() override;
+
// Limits the provided goal to the soft limits. Prints "name" when it fails
// to aid debugging.
void CapGoal(const char *name, Eigen::Matrix<double, 6, 1> *goal);
@@ -274,8 +298,6 @@
void UpdateShoulderOffset(double offset);
friend class testing::SuperstructureTest_DisabledGoalTest_Test;
- ::std::unique_ptr<
- ArmControlLoop> loop_;
aos::util::TrapezoidProfile shoulder_profile_, wrist_profile_;
::frc971::zeroing::ZeroingEstimator shoulder_estimator_, wrist_estimator_;
@@ -284,13 +306,6 @@
Eigen::Matrix<double, 2, 1> Y_;
// Current offset. Y_ = offset_ + raw_sensor;
Eigen::Matrix<double, 2, 1> offset_;
-
- // The goal that the profile tries to reach.
- Eigen::Matrix<double, 6, 1> unprofiled_goal_;
-
- bool initialized_ = false;
- bool shoulder_zeroed_ = false;
- bool wrist_zeroed_ = false;
};
} // namespace superstructure