Split controls out of superstructure.cc
Change-Id: Ia2303251cff44dcf7e319a8f94bf2d1142538cf9
diff --git a/y2016/control_loops/superstructure/BUILD b/y2016/control_loops/superstructure/BUILD
index b9628f6..c584135 100644
--- a/y2016/control_loops/superstructure/BUILD
+++ b/y2016/control_loops/superstructure/BUILD
@@ -66,9 +66,11 @@
name = 'superstructure_lib',
srcs = [
'superstructure.cc',
+ 'superstructure_controls.cc',
],
hdrs = [
'superstructure.h',
+ 'superstructure_controls.h',
],
deps = [
':superstructure_queue',
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index f0c85b6..214fc6c 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -1,4 +1,5 @@
#include "y2016/control_loops/superstructure/superstructure.h"
+#include "y2016/control_loops/superstructure/superstructure_controls.h"
#include "aos/common/controls/control_loops.q.h"
#include "aos/common/logging/logging.h"
@@ -14,387 +15,8 @@
namespace {
constexpr double kZeroingVoltage = 4.0;
-
-double UseUnlessZero(double target_value, double default_value) {
- if (target_value != 0.0) {
- return target_value;
- } else {
- return default_value;
- }
-}
-
} // 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()))),
- estimator_(constants::GetValues().intake.zeroing),
- profile_(::aos::controls::kLoopFrequency) {
- Y_.setZero();
- unprofiled_goal_.setZero();
- offset_.setZero();
- AdjustProfile(0.0, 0.0);
-}
-
-void Intake::UpdateIntakeOffset(double offset) {
- const double doffset = offset - offset_(0, 0);
- LOG(INFO, "Adjusting Intake offset from %f to %f\n", offset_(0, 0), offset);
-
- loop_->mutable_X_hat()(0, 0) += doffset;
- Y_(0, 0) += doffset;
- loop_->mutable_R(0, 0) += doffset;
-
- profile_.MoveGoal(doffset);
- offset_(0, 0) = offset;
-
- CapGoal("R", &loop_->mutable_R());
-}
-
-void Intake::Correct(PotAndIndexPosition position) {
- estimator_.UpdateEstimate(position);
-
- if (!initialized_) {
- if (estimator_.offset_ready()) {
- UpdateIntakeOffset(estimator_.offset());
- initialized_ = true;
- }
- }
-
- if (!zeroed_ && estimator_.zeroed()) {
- UpdateIntakeOffset(estimator_.offset());
- zeroed_ = true;
- }
-
- Y_ << position.encoder;
- Y_ += offset_;
- loop_->Correct(Y_);
-}
-
-void Intake::CapGoal(const char *name, Eigen::Matrix<double, 3, 1> *goal) {
- const auto &values = constants::GetValues();
-
- // Limit the goal to min/max allowable angles.
- if ((*goal)(0, 0) >= values.intake.limits.upper) {
- LOG(WARNING, "Intake goal %s above limit, %f > %f\n", name, (*goal)(0, 0),
- values.intake.limits.upper);
- (*goal)(0, 0) = values.intake.limits.upper;
- }
- if ((*goal)(0, 0) <= values.intake.limits.lower) {
- LOG(WARNING, "Intake goal %s below limit, %f < %f\n", name, (*goal)(0, 0),
- values.intake.limits.lower);
- (*goal)(0, 0) = values.intake.limits.lower;
- }
-}
-
-void Intake::ForceGoal(double goal) {
- set_unprofiled_goal(goal);
- loop_->mutable_R() = unprofiled_goal_;
- loop_->mutable_next_R() = loop_->R();
-
- profile_.MoveCurrentState(loop_->R().block<2, 1>(0, 0));
-}
-
-void Intake::set_unprofiled_goal(double unprofiled_goal) {
- unprofiled_goal_(0, 0) = unprofiled_goal;
- unprofiled_goal_(1, 0) = 0.0;
- unprofiled_goal_(2, 0) = 0.0;
- CapGoal("unprofiled R", &unprofiled_goal_);
-}
-
-void Intake::Update(bool disable) {
- if (!disable) {
- ::Eigen::Matrix<double, 2, 1> goal_state =
- profile_.Update(unprofiled_goal_(0, 0), unprofiled_goal_(1, 0));
-
- loop_->mutable_next_R(0, 0) = goal_state(0, 0);
- loop_->mutable_next_R(1, 0) = goal_state(1, 0);
- loop_->mutable_next_R(2, 0) = 0.0;
- CapGoal("next R", &loop_->mutable_next_R());
- }
-
- loop_->Update(disable);
-
- if (!disable && loop_->U(0, 0) != loop_->U_uncapped(0, 0)) {
- profile_.MoveCurrentState(loop_->R().block<2, 1>(0, 0));
- }
-}
-
-bool Intake::CheckHardLimits() {
- const auto &values = constants::GetValues();
- // Returns whether hard limits have been exceeded.
-
- if (angle() >= values.intake.limits.upper_hard ||
- angle() <= values.intake.limits.lower_hard) {
- LOG(ERROR, "Intake at %f out of bounds [%f, %f], ESTOPing\n", angle(),
- values.intake.limits.lower_hard, values.intake.limits.upper_hard);
- return true;
- }
-
- return false;
-}
-
-void Intake::set_max_voltage(double voltage) {
- loop_->set_max_voltage(voltage);
-}
-
-void Intake::AdjustProfile(double max_angular_velocity,
- double max_angular_acceleration) {
- profile_.set_maximum_velocity(UseUnlessZero(max_angular_velocity, 10.0));
- profile_.set_maximum_acceleration(
- UseUnlessZero(max_angular_acceleration, 10.0));
-}
-
-void Intake::Reset() {
- estimator_.Reset();
- initialized_ = false;
- zeroed_ = false;
-}
-
-EstimatorState Intake::IntakeEstimatorState() {
- EstimatorState estimator_state;
- ::frc971::zeroing::PopulateEstimatorState(estimator_, &estimator_state);
-
- return estimator_state;
-}
-
-Arm::Arm()
- : loop_(new DoubleCappedStateFeedbackLoop(
- ::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);
-}
-
-void Arm::UpdateWristOffset(double offset) {
- const double doffset = offset - offset_(1, 0);
- LOG(INFO, "Adjusting Wrist offset from %f to %f\n", offset_(1, 0), offset);
-
- loop_->mutable_X_hat()(2, 0) += doffset;
- Y_(1, 0) += doffset;
- loop_->mutable_R(2, 0) += doffset;
- loop_->mutable_next_R(2, 0) += doffset;
- unprofiled_goal_(2, 0) += doffset;
-
- wrist_profile_.MoveGoal(doffset);
- offset_(1, 0) = offset;
-
- CapGoal("R", &loop_->mutable_R());
- CapGoal("unprofiled R", &loop_->mutable_next_R());
-}
-
-void Arm::UpdateShoulderOffset(double offset) {
- const double doffset = offset - offset_(0, 0);
- LOG(INFO, "Adjusting Shoulder offset from %f to %f\n", offset_(0, 0), offset);
-
- loop_->mutable_X_hat()(0, 0) += doffset;
- loop_->mutable_X_hat()(2, 0) += doffset;
- Y_(0, 0) += doffset;
- loop_->mutable_R(0, 0) += doffset;
- loop_->mutable_R(2, 0) += doffset;
- loop_->mutable_next_R(0, 0) += doffset;
- loop_->mutable_next_R(2, 0) += doffset;
- unprofiled_goal_(0, 0) += doffset;
- unprofiled_goal_(2, 0) += doffset;
-
- shoulder_profile_.MoveGoal(doffset);
- wrist_profile_.MoveGoal(doffset);
- offset_(0, 0) = offset;
-
- CapGoal("R", &loop_->mutable_R());
- CapGoal("unprofiled R", &loop_->mutable_next_R());
-}
-
-// TODO(austin): Handle zeroing errors.
-
-void Arm::Correct(PotAndIndexPosition position_shoulder,
- PotAndIndexPosition position_wrist) {
- shoulder_estimator_.UpdateEstimate(position_shoulder);
- wrist_estimator_.UpdateEstimate(position_wrist);
-
- if (!initialized_) {
- if (shoulder_estimator_.offset_ready() && wrist_estimator_.offset_ready()) {
- UpdateShoulderOffset(shoulder_estimator_.offset());
- UpdateWristOffset(wrist_estimator_.offset());
- initialized_ = true;
- }
- }
-
- if (!shoulder_zeroed_ && shoulder_estimator_.zeroed()) {
- UpdateShoulderOffset(shoulder_estimator_.offset());
- shoulder_zeroed_ = true;
- }
- if (!wrist_zeroed_ && wrist_estimator_.zeroed()) {
- UpdateWristOffset(wrist_estimator_.offset());
- wrist_zeroed_ = true;
- }
-
- {
- Y_ << position_shoulder.encoder, position_wrist.encoder;
- Y_ += offset_;
- loop_->Correct(Y_);
- }
-}
-
-void Arm::CapGoal(const char *name, Eigen::Matrix<double, 6, 1> *goal) {
- // Limit the goals to min/max allowable angles.
- const auto &values = constants::GetValues();
-
- if ((*goal)(0, 0) >= values.shoulder.limits.upper) {
- LOG(WARNING, "Shoulder goal %s above limit, %f > %f\n", name, (*goal)(0, 0),
- values.shoulder.limits.upper);
- (*goal)(0, 0) = values.shoulder.limits.upper;
- }
- if ((*goal)(0, 0) <= values.shoulder.limits.lower) {
- LOG(WARNING, "Shoulder goal %s below limit, %f < %f\n", name, (*goal)(0, 0),
- values.shoulder.limits.lower);
- (*goal)(0, 0) = values.shoulder.limits.lower;
- }
-
- const double wrist_goal_angle_ungrounded = (*goal)(2, 0) - (*goal)(0, 0);
-
- if (wrist_goal_angle_ungrounded >= values.wrist.limits.upper) {
- LOG(WARNING, "Wrist goal %s above limit, %f > %f\n", name,
- wrist_goal_angle_ungrounded, values.wrist.limits.upper);
- (*goal)(2, 0) = values.wrist.limits.upper + (*goal)(0, 0);
- }
- if (wrist_goal_angle_ungrounded <= values.wrist.limits.lower) {
- LOG(WARNING, "Wrist goal %s below limit, %f < %f\n", name,
- wrist_goal_angle_ungrounded, values.wrist.limits.lower);
- (*goal)(2, 0) = values.wrist.limits.lower + (*goal)(0, 0);
- }
-}
-
-void Arm::ForceGoal(double goal_shoulder, double goal_wrist) {
- set_unprofiled_goal(goal_shoulder, goal_wrist);
- loop_->mutable_R() = unprofiled_goal_;
- loop_->mutable_next_R() = loop_->R();
-
- shoulder_profile_.MoveCurrentState(loop_->R().block<2, 1>(0, 0));
- wrist_profile_.MoveCurrentState(loop_->R().block<2, 1>(2, 0));
-}
-
-void Arm::set_unprofiled_goal(double unprofiled_goal_shoulder,
- double unprofiled_goal_wrist) {
- unprofiled_goal_ << unprofiled_goal_shoulder, 0.0, unprofiled_goal_wrist, 0.0,
- 0.0, 0.0;
- CapGoal("unprofiled R", &unprofiled_goal_);
-}
-
-void Arm::AdjustProfile(double max_angular_velocity_shoulder,
- double max_angular_acceleration_shoulder,
- double max_angular_velocity_wrist,
- double max_angular_acceleration_wrist) {
- shoulder_profile_.set_maximum_velocity(
- UseUnlessZero(max_angular_velocity_shoulder, 10.0));
- shoulder_profile_.set_maximum_acceleration(
- UseUnlessZero(max_angular_acceleration_shoulder, 10.0));
- wrist_profile_.set_maximum_velocity(
- UseUnlessZero(max_angular_velocity_wrist, 10.0));
- wrist_profile_.set_maximum_acceleration(
- UseUnlessZero(max_angular_acceleration_wrist, 10.0));
-}
-
-bool Arm::CheckHardLimits() {
- const auto &values = constants::GetValues();
- if (shoulder_angle() >= values.shoulder.limits.upper_hard ||
- shoulder_angle() <= values.shoulder.limits.lower_hard) {
- LOG(ERROR, "Shoulder at %f out of bounds [%f, %f], ESTOPing\n",
- shoulder_angle(), values.shoulder.limits.lower_hard,
- values.shoulder.limits.upper_hard);
- return true;
- }
-
- if (wrist_angle() - shoulder_angle() >= values.wrist.limits.upper_hard ||
- wrist_angle() - shoulder_angle() <= values.wrist.limits.lower_hard) {
- LOG(ERROR, "Wrist at %f out of bounds [%f, %f], ESTOPing\n",
- wrist_angle() - shoulder_angle(), values.wrist.limits.lower_hard,
- values.wrist.limits.upper_hard);
- return true;
- }
-
- return false;
-}
-
-void Arm::Update(bool disable) {
- if (!disable) {
- // Compute next goal.
- ::Eigen::Matrix<double, 2, 1> goal_state_shoulder =
- shoulder_profile_.Update(unprofiled_goal_(0, 0),
- unprofiled_goal_(1, 0));
- loop_->mutable_next_R(0, 0) = goal_state_shoulder(0, 0);
- loop_->mutable_next_R(1, 0) = goal_state_shoulder(1, 0);
-
- ::Eigen::Matrix<double, 2, 1> goal_state_wrist =
- wrist_profile_.Update(unprofiled_goal_(2, 0), unprofiled_goal_(3, 0));
- loop_->mutable_next_R(2, 0) = goal_state_wrist(0, 0);
- loop_->mutable_next_R(3, 0) = goal_state_wrist(1, 0);
-
- loop_->mutable_next_R(4, 0) = unprofiled_goal_(4, 0);
- loop_->mutable_next_R(5, 0) = unprofiled_goal_(5, 0);
- CapGoal("next R", &loop_->mutable_next_R());
- }
-
- // Move loop
- loop_->Update(disable);
-
- // Shoulder saturated
- if (!disable && loop_->U(0, 0) != loop_->U_uncapped(0, 0)) {
- shoulder_profile_.MoveCurrentState(loop_->R().block<2, 1>(0, 0));
- }
-
- // Wrist saturated
- if (!disable && loop_->U(1, 0) != loop_->U_uncapped(1, 0)) {
- wrist_profile_.MoveCurrentState(loop_->R().block<2, 1>(2, 0));
- }
-}
-
-void Arm::set_max_voltage(double shoulder_max_voltage,
- double wrist_max_voltage) {
- loop_->set_max_voltage(shoulder_max_voltage, wrist_max_voltage);
-}
-
-void Arm::Reset() {
- shoulder_estimator_.Reset();
- wrist_estimator_.Reset();
- initialized_ = false;
- shoulder_zeroed_ = false;
- wrist_zeroed_ = false;
-}
-
-EstimatorState Arm::ShoulderEstimatorState() {
- EstimatorState estimator_state;
- ::frc971::zeroing::PopulateEstimatorState(shoulder_estimator_,
- &estimator_state);
-
- return estimator_state;
-}
-
-EstimatorState Arm::WristEstimatorState() {
- EstimatorState estimator_state;
- ::frc971::zeroing::PopulateEstimatorState(wrist_estimator_, &estimator_state);
-
- return estimator_state;
-}
-
-// ///// Superstructure /////
Superstructure::Superstructure(
control_loops::SuperstructureQueue *superstructure_queue)
: aos::controls::ControlLoop<control_loops::SuperstructureQueue>(
diff --git a/y2016/control_loops/superstructure/superstructure.h b/y2016/control_loops/superstructure/superstructure.h
index 3e5cde4..9a04c3c 100644
--- a/y2016/control_loops/superstructure/superstructure.h
+++ b/y2016/control_loops/superstructure/superstructure.h
@@ -4,11 +4,12 @@
#include <memory>
#include "aos/common/controls/control_loop.h"
-#include "frc971/control_loops/state_feedback_loop.h"
#include "aos/common/util/trapezoid_profile.h"
+#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/zeroing/zeroing.h"
#include "y2016/control_loops/superstructure/superstructure.q.h"
+#include "y2016/control_loops/superstructure/superstructure_controls.h"
namespace y2016 {
namespace control_loops {
@@ -17,213 +18,6 @@
class SuperstructureTest_DisabledGoalTest_Test;
} // namespace testing
-using ::frc971::PotAndIndexPosition;
-using ::frc971::EstimatorState;
-
-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();
- // Returns whether the estimators have been initialized and zeroed.
- bool initialized() const { return initialized_; }
- bool zeroed() const { return zeroed_; }
-
- // Updates our estimator with the latest position.
- void Correct(PotAndIndexPosition position);
-
- // 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);
-
- // Runs the controller and profile generator for a cycle.
- void Update(bool disabled);
-
- // Limits our profiles to a max velocity and acceleration for proper motion.
- void AdjustProfile(double max_angular_velocity,
- double max_angular_acceleration);
- // Sets the maximum voltage that will be commanded by the loop.
- void set_max_voltage(double voltage);
-
- // 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.
- 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 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); }
-
- 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<SimpleCappedStateFeedbackLoop> 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 Arm {
- public:
- Arm();
- // 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_; }
-
- // Updates our estimator with the latest position.
- void Correct(PotAndIndexPosition position_shoulder,
- PotAndIndexPosition position_wrist);
-
- // Forces the goal to be the provided goal.
- void ForceGoal(double unprofiled_goal_shoulder, double unprofiled_goal_wrist);
- // Sets the unprofiled goal. The profiler will generate a profile to go to
- // this goal.
- void set_unprofiled_goal(double unprofiled_goal_shoulder,
- double unprofiled_goal_wrist);
-
- // Runs the controller and profile generator for a cycle.
- void Update(bool disabled);
-
- // Limits our profiles to a max velocity and acceleration for proper motion.
- void AdjustProfile(double max_angular_velocity_shoulder,
- double max_angular_acceleration_shoulder,
- double max_angular_velocity_wrist,
- double max_angular_acceleration_wrist);
- void set_max_voltage(double shoulder_max_voltage, double wrist_max_voltage);
-
- // 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.
- EstimatorState ShoulderEstimatorState();
- EstimatorState WristEstimatorState();
-
- // Returns the requested shoulder and wrist voltages.
- double shoulder_voltage() const { return loop_->U(0, 0); }
- double wrist_voltage() const { return loop_->U(1, 0); }
-
- // Returns the current positions.
- double shoulder_angle() const { return Y_(0, 0); }
- double wrist_angle() const { return Y_(1, 0) + Y_(0, 0); }
-
- // 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); }
-
- 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, 6, 1> *goal);
-
- // Updates the offset
- void UpdateWristOffset(double offset);
- void UpdateShoulderOffset(double offset);
-
- friend class testing::SuperstructureTest_DisabledGoalTest_Test;
- ::std::unique_ptr<DoubleCappedStateFeedbackLoop> loop_;
-
- aos::util::TrapezoidProfile shoulder_profile_, wrist_profile_;
- ::frc971::zeroing::ZeroingEstimator shoulder_estimator_, wrist_estimator_;
-
- // Current measurement.
- 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;
-};
-
class Superstructure
: public ::aos::controls::ControlLoop<control_loops::SuperstructureQueue> {
public:
diff --git a/y2016/control_loops/superstructure/superstructure_controls.cc b/y2016/control_loops/superstructure/superstructure_controls.cc
new file mode 100644
index 0000000..7cd67e3
--- /dev/null
+++ b/y2016/control_loops/superstructure/superstructure_controls.cc
@@ -0,0 +1,399 @@
+#include "y2016/control_loops/superstructure/superstructure_controls.h"
+
+#include "aos/common/controls/control_loops.q.h"
+#include "aos/common/logging/logging.h"
+
+#include "y2016/control_loops/superstructure/integral_intake_plant.h"
+#include "y2016/control_loops/superstructure/integral_arm_plant.h"
+
+#include "y2016/constants.h"
+
+namespace y2016 {
+namespace control_loops {
+namespace superstructure {
+
+using ::frc971::PotAndIndexPosition;
+using ::frc971::EstimatorState;
+
+namespace {
+double UseUnlessZero(double target_value, double default_value) {
+ if (target_value != 0.0) {
+ return target_value;
+ } else {
+ return default_value;
+ }
+}
+} // 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()))),
+ estimator_(constants::GetValues().intake.zeroing),
+ profile_(::aos::controls::kLoopFrequency) {
+ Y_.setZero();
+ unprofiled_goal_.setZero();
+ offset_.setZero();
+ AdjustProfile(0.0, 0.0);
+}
+
+void Intake::UpdateIntakeOffset(double offset) {
+ const double doffset = offset - offset_(0, 0);
+ LOG(INFO, "Adjusting Intake offset from %f to %f\n", offset_(0, 0), offset);
+
+ loop_->mutable_X_hat()(0, 0) += doffset;
+ Y_(0, 0) += doffset;
+ loop_->mutable_R(0, 0) += doffset;
+
+ profile_.MoveGoal(doffset);
+ offset_(0, 0) = offset;
+
+ CapGoal("R", &loop_->mutable_R());
+}
+
+void Intake::Correct(PotAndIndexPosition position) {
+ estimator_.UpdateEstimate(position);
+
+ if (!initialized_) {
+ if (estimator_.offset_ready()) {
+ UpdateIntakeOffset(estimator_.offset());
+ initialized_ = true;
+ }
+ }
+
+ if (!zeroed_ && estimator_.zeroed()) {
+ UpdateIntakeOffset(estimator_.offset());
+ zeroed_ = true;
+ }
+
+ Y_ << position.encoder;
+ Y_ += offset_;
+ loop_->Correct(Y_);
+}
+
+void Intake::CapGoal(const char *name, Eigen::Matrix<double, 3, 1> *goal) {
+ const auto &values = constants::GetValues();
+
+ // Limit the goal to min/max allowable angles.
+ if ((*goal)(0, 0) >= values.intake.limits.upper) {
+ LOG(WARNING, "Intake goal %s above limit, %f > %f\n", name, (*goal)(0, 0),
+ values.intake.limits.upper);
+ (*goal)(0, 0) = values.intake.limits.upper;
+ }
+ if ((*goal)(0, 0) <= values.intake.limits.lower) {
+ LOG(WARNING, "Intake goal %s below limit, %f < %f\n", name, (*goal)(0, 0),
+ values.intake.limits.lower);
+ (*goal)(0, 0) = values.intake.limits.lower;
+ }
+}
+
+void Intake::ForceGoal(double goal) {
+ set_unprofiled_goal(goal);
+ loop_->mutable_R() = unprofiled_goal_;
+ loop_->mutable_next_R() = loop_->R();
+
+ profile_.MoveCurrentState(loop_->R().block<2, 1>(0, 0));
+}
+
+void Intake::set_unprofiled_goal(double unprofiled_goal) {
+ unprofiled_goal_(0, 0) = unprofiled_goal;
+ unprofiled_goal_(1, 0) = 0.0;
+ unprofiled_goal_(2, 0) = 0.0;
+ CapGoal("unprofiled R", &unprofiled_goal_);
+}
+
+void Intake::Update(bool disable) {
+ if (!disable) {
+ ::Eigen::Matrix<double, 2, 1> goal_state =
+ profile_.Update(unprofiled_goal_(0, 0), unprofiled_goal_(1, 0));
+
+ loop_->mutable_next_R(0, 0) = goal_state(0, 0);
+ loop_->mutable_next_R(1, 0) = goal_state(1, 0);
+ loop_->mutable_next_R(2, 0) = 0.0;
+ CapGoal("next R", &loop_->mutable_next_R());
+ }
+
+ loop_->Update(disable);
+
+ if (!disable && loop_->U(0, 0) != loop_->U_uncapped(0, 0)) {
+ profile_.MoveCurrentState(loop_->R().block<2, 1>(0, 0));
+ }
+}
+
+bool Intake::CheckHardLimits() {
+ const auto &values = constants::GetValues();
+ // Returns whether hard limits have been exceeded.
+
+ if (angle() >= values.intake.limits.upper_hard ||
+ angle() <= values.intake.limits.lower_hard) {
+ LOG(ERROR, "Intake at %f out of bounds [%f, %f], ESTOPing\n", angle(),
+ values.intake.limits.lower_hard, values.intake.limits.upper_hard);
+ return true;
+ }
+
+ return false;
+}
+
+void Intake::set_max_voltage(double voltage) {
+ loop_->set_max_voltage(voltage);
+}
+
+void Intake::AdjustProfile(double max_angular_velocity,
+ double max_angular_acceleration) {
+ profile_.set_maximum_velocity(UseUnlessZero(max_angular_velocity, 10.0));
+ profile_.set_maximum_acceleration(
+ UseUnlessZero(max_angular_acceleration, 10.0));
+}
+
+void Intake::Reset() {
+ estimator_.Reset();
+ initialized_ = false;
+ zeroed_ = false;
+}
+
+EstimatorState Intake::IntakeEstimatorState() {
+ EstimatorState estimator_state;
+ ::frc971::zeroing::PopulateEstimatorState(estimator_, &estimator_state);
+
+ return estimator_state;
+}
+
+Arm::Arm()
+ : loop_(new DoubleCappedStateFeedbackLoop(
+ ::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);
+}
+
+void Arm::UpdateWristOffset(double offset) {
+ const double doffset = offset - offset_(1, 0);
+ LOG(INFO, "Adjusting Wrist offset from %f to %f\n", offset_(1, 0), offset);
+
+ loop_->mutable_X_hat()(2, 0) += doffset;
+ Y_(1, 0) += doffset;
+ loop_->mutable_R(2, 0) += doffset;
+ loop_->mutable_next_R(2, 0) += doffset;
+ unprofiled_goal_(2, 0) += doffset;
+
+ wrist_profile_.MoveGoal(doffset);
+ offset_(1, 0) = offset;
+
+ CapGoal("R", &loop_->mutable_R());
+ CapGoal("unprofiled R", &loop_->mutable_next_R());
+}
+
+void Arm::UpdateShoulderOffset(double offset) {
+ const double doffset = offset - offset_(0, 0);
+ LOG(INFO, "Adjusting Shoulder offset from %f to %f\n", offset_(0, 0), offset);
+
+ loop_->mutable_X_hat()(0, 0) += doffset;
+ loop_->mutable_X_hat()(2, 0) += doffset;
+ Y_(0, 0) += doffset;
+ loop_->mutable_R(0, 0) += doffset;
+ loop_->mutable_R(2, 0) += doffset;
+ loop_->mutable_next_R(0, 0) += doffset;
+ loop_->mutable_next_R(2, 0) += doffset;
+ unprofiled_goal_(0, 0) += doffset;
+ unprofiled_goal_(2, 0) += doffset;
+
+ shoulder_profile_.MoveGoal(doffset);
+ wrist_profile_.MoveGoal(doffset);
+ offset_(0, 0) = offset;
+
+ CapGoal("R", &loop_->mutable_R());
+ CapGoal("unprofiled R", &loop_->mutable_next_R());
+}
+
+// TODO(austin): Handle zeroing errors.
+
+void Arm::Correct(PotAndIndexPosition position_shoulder,
+ PotAndIndexPosition position_wrist) {
+ shoulder_estimator_.UpdateEstimate(position_shoulder);
+ wrist_estimator_.UpdateEstimate(position_wrist);
+
+ if (!initialized_) {
+ if (shoulder_estimator_.offset_ready() && wrist_estimator_.offset_ready()) {
+ UpdateShoulderOffset(shoulder_estimator_.offset());
+ UpdateWristOffset(wrist_estimator_.offset());
+ initialized_ = true;
+ }
+ }
+
+ if (!shoulder_zeroed_ && shoulder_estimator_.zeroed()) {
+ UpdateShoulderOffset(shoulder_estimator_.offset());
+ shoulder_zeroed_ = true;
+ }
+ if (!wrist_zeroed_ && wrist_estimator_.zeroed()) {
+ UpdateWristOffset(wrist_estimator_.offset());
+ wrist_zeroed_ = true;
+ }
+
+ {
+ Y_ << position_shoulder.encoder, position_wrist.encoder;
+ Y_ += offset_;
+ loop_->Correct(Y_);
+ }
+}
+
+void Arm::CapGoal(const char *name, Eigen::Matrix<double, 6, 1> *goal) {
+ // Limit the goals to min/max allowable angles.
+ const auto &values = constants::GetValues();
+
+ if ((*goal)(0, 0) >= values.shoulder.limits.upper) {
+ LOG(WARNING, "Shoulder goal %s above limit, %f > %f\n", name, (*goal)(0, 0),
+ values.shoulder.limits.upper);
+ (*goal)(0, 0) = values.shoulder.limits.upper;
+ }
+ if ((*goal)(0, 0) <= values.shoulder.limits.lower) {
+ LOG(WARNING, "Shoulder goal %s below limit, %f < %f\n", name, (*goal)(0, 0),
+ values.shoulder.limits.lower);
+ (*goal)(0, 0) = values.shoulder.limits.lower;
+ }
+
+ const double wrist_goal_angle_ungrounded = (*goal)(2, 0) - (*goal)(0, 0);
+
+ if (wrist_goal_angle_ungrounded >= values.wrist.limits.upper) {
+ LOG(WARNING, "Wrist goal %s above limit, %f > %f\n", name,
+ wrist_goal_angle_ungrounded, values.wrist.limits.upper);
+ (*goal)(2, 0) = values.wrist.limits.upper + (*goal)(0, 0);
+ }
+ if (wrist_goal_angle_ungrounded <= values.wrist.limits.lower) {
+ LOG(WARNING, "Wrist goal %s below limit, %f < %f\n", name,
+ wrist_goal_angle_ungrounded, values.wrist.limits.lower);
+ (*goal)(2, 0) = values.wrist.limits.lower + (*goal)(0, 0);
+ }
+}
+
+void Arm::ForceGoal(double goal_shoulder, double goal_wrist) {
+ set_unprofiled_goal(goal_shoulder, goal_wrist);
+ loop_->mutable_R() = unprofiled_goal_;
+ loop_->mutable_next_R() = loop_->R();
+
+ shoulder_profile_.MoveCurrentState(loop_->R().block<2, 1>(0, 0));
+ wrist_profile_.MoveCurrentState(loop_->R().block<2, 1>(2, 0));
+}
+
+void Arm::set_unprofiled_goal(double unprofiled_goal_shoulder,
+ double unprofiled_goal_wrist) {
+ unprofiled_goal_ << unprofiled_goal_shoulder, 0.0, unprofiled_goal_wrist, 0.0,
+ 0.0, 0.0;
+ CapGoal("unprofiled R", &unprofiled_goal_);
+}
+
+void Arm::AdjustProfile(double max_angular_velocity_shoulder,
+ double max_angular_acceleration_shoulder,
+ double max_angular_velocity_wrist,
+ double max_angular_acceleration_wrist) {
+ shoulder_profile_.set_maximum_velocity(
+ UseUnlessZero(max_angular_velocity_shoulder, 10.0));
+ shoulder_profile_.set_maximum_acceleration(
+ UseUnlessZero(max_angular_acceleration_shoulder, 10.0));
+ wrist_profile_.set_maximum_velocity(
+ UseUnlessZero(max_angular_velocity_wrist, 10.0));
+ wrist_profile_.set_maximum_acceleration(
+ UseUnlessZero(max_angular_acceleration_wrist, 10.0));
+}
+
+bool Arm::CheckHardLimits() {
+ const auto &values = constants::GetValues();
+ if (shoulder_angle() >= values.shoulder.limits.upper_hard ||
+ shoulder_angle() <= values.shoulder.limits.lower_hard) {
+ LOG(ERROR, "Shoulder at %f out of bounds [%f, %f], ESTOPing\n",
+ shoulder_angle(), values.shoulder.limits.lower_hard,
+ values.shoulder.limits.upper_hard);
+ return true;
+ }
+
+ if (wrist_angle() - shoulder_angle() >= values.wrist.limits.upper_hard ||
+ wrist_angle() - shoulder_angle() <= values.wrist.limits.lower_hard) {
+ LOG(ERROR, "Wrist at %f out of bounds [%f, %f], ESTOPing\n",
+ wrist_angle() - shoulder_angle(), values.wrist.limits.lower_hard,
+ values.wrist.limits.upper_hard);
+ return true;
+ }
+
+ return false;
+}
+
+void Arm::Update(bool disable) {
+ if (!disable) {
+ // Compute next goal.
+ ::Eigen::Matrix<double, 2, 1> goal_state_shoulder =
+ shoulder_profile_.Update(unprofiled_goal_(0, 0),
+ unprofiled_goal_(1, 0));
+ loop_->mutable_next_R(0, 0) = goal_state_shoulder(0, 0);
+ loop_->mutable_next_R(1, 0) = goal_state_shoulder(1, 0);
+
+ ::Eigen::Matrix<double, 2, 1> goal_state_wrist =
+ wrist_profile_.Update(unprofiled_goal_(2, 0), unprofiled_goal_(3, 0));
+ loop_->mutable_next_R(2, 0) = goal_state_wrist(0, 0);
+ loop_->mutable_next_R(3, 0) = goal_state_wrist(1, 0);
+
+ loop_->mutable_next_R(4, 0) = unprofiled_goal_(4, 0);
+ loop_->mutable_next_R(5, 0) = unprofiled_goal_(5, 0);
+ CapGoal("next R", &loop_->mutable_next_R());
+ }
+
+ // Move loop
+ loop_->Update(disable);
+
+ // Shoulder saturated
+ if (!disable && loop_->U(0, 0) != loop_->U_uncapped(0, 0)) {
+ shoulder_profile_.MoveCurrentState(loop_->R().block<2, 1>(0, 0));
+ }
+
+ // Wrist saturated
+ if (!disable && loop_->U(1, 0) != loop_->U_uncapped(1, 0)) {
+ wrist_profile_.MoveCurrentState(loop_->R().block<2, 1>(2, 0));
+ }
+}
+
+void Arm::set_max_voltage(double shoulder_max_voltage,
+ double wrist_max_voltage) {
+ loop_->set_max_voltage(shoulder_max_voltage, wrist_max_voltage);
+}
+
+void Arm::Reset() {
+ shoulder_estimator_.Reset();
+ wrist_estimator_.Reset();
+ initialized_ = false;
+ shoulder_zeroed_ = false;
+ wrist_zeroed_ = false;
+}
+
+EstimatorState Arm::ShoulderEstimatorState() {
+ EstimatorState estimator_state;
+ ::frc971::zeroing::PopulateEstimatorState(shoulder_estimator_,
+ &estimator_state);
+
+ return estimator_state;
+}
+
+EstimatorState Arm::WristEstimatorState() {
+ EstimatorState estimator_state;
+ ::frc971::zeroing::PopulateEstimatorState(wrist_estimator_, &estimator_state);
+
+ return estimator_state;
+}
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2016
diff --git a/y2016/control_loops/superstructure/superstructure_controls.h b/y2016/control_loops/superstructure/superstructure_controls.h
new file mode 100644
index 0000000..d96e24d
--- /dev/null
+++ b/y2016/control_loops/superstructure/superstructure_controls.h
@@ -0,0 +1,229 @@
+#ifndef Y2016_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_CONTROLS_H_
+#define Y2016_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_CONTROLS_H_
+
+#include <memory>
+
+#include "aos/common/controls/control_loop.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "aos/common/util/trapezoid_profile.h"
+
+#include "frc971/zeroing/zeroing.h"
+#include "y2016/control_loops/superstructure/superstructure.q.h"
+
+namespace y2016 {
+namespace control_loops {
+namespace superstructure {
+namespace testing {
+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();
+ // Returns whether the estimators have been initialized and zeroed.
+ bool initialized() const { return initialized_; }
+ bool zeroed() const { return zeroed_; }
+
+ // Updates our estimator with the latest position.
+ void Correct(::frc971::PotAndIndexPosition position);
+
+ // 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);
+
+ // Runs the controller and profile generator for a cycle.
+ void Update(bool disabled);
+
+ // Limits our profiles to a max velocity and acceleration for proper motion.
+ void AdjustProfile(double max_angular_velocity,
+ double max_angular_acceleration);
+ // Sets the maximum voltage that will be commanded by the loop.
+ void set_max_voltage(double voltage);
+
+ // 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 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); }
+
+ 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<SimpleCappedStateFeedbackLoop> 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 Arm {
+ public:
+ Arm();
+ // 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_; }
+
+ // Updates our estimator with the latest position.
+ void Correct(::frc971::PotAndIndexPosition position_shoulder,
+ ::frc971::PotAndIndexPosition position_wrist);
+
+ // Forces the goal to be the provided goal.
+ void ForceGoal(double unprofiled_goal_shoulder, double unprofiled_goal_wrist);
+ // Sets the unprofiled goal. The profiler will generate a profile to go to
+ // this goal.
+ void set_unprofiled_goal(double unprofiled_goal_shoulder,
+ double unprofiled_goal_wrist);
+
+ // Runs the controller and profile generator for a cycle.
+ void Update(bool disabled);
+
+ // Limits our profiles to a max velocity and acceleration for proper motion.
+ void AdjustProfile(double max_angular_velocity_shoulder,
+ double max_angular_acceleration_shoulder,
+ double max_angular_velocity_wrist,
+ double max_angular_acceleration_wrist);
+ void set_max_voltage(double shoulder_max_voltage, double wrist_max_voltage);
+
+ // 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();
+ ::frc971::EstimatorState WristEstimatorState();
+
+ // Returns the requested shoulder and wrist voltages.
+ double shoulder_voltage() const { return loop_->U(0, 0); }
+ double wrist_voltage() const { return loop_->U(1, 0); }
+
+ // Returns the current positions.
+ double shoulder_angle() const { return Y_(0, 0); }
+ double wrist_angle() const { return Y_(1, 0) + Y_(0, 0); }
+
+ // 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); }
+
+ 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, 6, 1> *goal);
+
+ // Updates the offset
+ void UpdateWristOffset(double offset);
+ void UpdateShoulderOffset(double offset);
+
+ friend class testing::SuperstructureTest_DisabledGoalTest_Test;
+ ::std::unique_ptr<DoubleCappedStateFeedbackLoop> loop_;
+
+ aos::util::TrapezoidProfile shoulder_profile_, wrist_profile_;
+ ::frc971::zeroing::ZeroingEstimator shoulder_estimator_, wrist_estimator_;
+
+ // Current measurement.
+ 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
+} // namespace control_loops
+} // namespace y2016
+
+#endif // Y2016_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_CONTROLS_H_