Factored out 1 DOF profiled subsystem from 2016
This should make all the motion profiled zeroing easy.
Change-Id: I1941ee8e5b4e14e611bb2f0df86c60e9c055d7af
diff --git a/y2016/control_loops/superstructure/BUILD b/y2016/control_loops/superstructure/BUILD
index 6a10b61..af02d14 100644
--- a/y2016/control_loops/superstructure/BUILD
+++ b/y2016/control_loops/superstructure/BUILD
@@ -78,10 +78,11 @@
'//aos/common/controls:control_loop',
'//aos/common/util:trapezoid_profile',
'//aos/common:math',
- '//y2016/queues:ball_detector',
- '//frc971/control_loops:state_feedback_loop',
+ '//frc971/control_loops:profiled_subsystem',
'//frc971/control_loops:simple_capped_state_feedback_loop',
+ '//frc971/control_loops:state_feedback_loop',
'//frc971/zeroing',
+ '//y2016/queues:ball_detector',
'//y2016:constants',
],
)
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index 0c1ac6a..cb09b42 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -648,11 +648,11 @@
}
}
arm_.set_max_voltage(
- kill_shoulder_ ? 0.0 : max_voltage,
- kill_shoulder_ ? (arm_.X_hat(0, 0) < 0.05 ? kShooterHangingLowVoltage
- : kShooterHangingVoltage)
- : max_voltage);
- intake_.set_max_voltage(max_voltage);
+ {{kill_shoulder_ ? 0.0 : max_voltage,
+ kill_shoulder_ ? (arm_.X_hat(0, 0) < 0.05 ? kShooterHangingLowVoltage
+ : kShooterHangingVoltage)
+ : max_voltage}});
+ intake_.set_max_voltage({{max_voltage}});
if (IsRunning() && !kill_shoulder_) {
// We don't want lots of negative voltage when we are near the bellypan on
@@ -743,7 +743,7 @@
status->shoulder.voltage_error = arm_.X_hat(4, 0);
status->shoulder.calculated_velocity =
(arm_.shoulder_angle() - last_shoulder_angle_) / 0.005;
- status->shoulder.estimator_state = arm_.ShoulderEstimatorState();
+ status->shoulder.estimator_state = arm_.EstimatorState(0);
status->wrist.angle = arm_.X_hat(2, 0);
status->wrist.angular_velocity = arm_.X_hat(3, 0);
@@ -754,7 +754,7 @@
status->wrist.voltage_error = arm_.X_hat(5, 0);
status->wrist.calculated_velocity =
(arm_.wrist_angle() - last_wrist_angle_) / 0.005;
- status->wrist.estimator_state = arm_.WristEstimatorState();
+ status->wrist.estimator_state = arm_.EstimatorState(1);
status->intake.angle = intake_.X_hat(0, 0);
status->intake.angular_velocity = intake_.X_hat(1, 0);
@@ -766,7 +766,7 @@
status->intake.calculated_velocity =
(intake_.angle() - last_intake_angle_) / 0.005;
status->intake.voltage_error = intake_.X_hat(2, 0);
- status->intake.estimator_state = intake_.IntakeEstimatorState();
+ status->intake.estimator_state = intake_.EstimatorState(0);
status->intake.feedforwards_power = intake_.controller().ff_U(0, 0);
status->shoulder_controller_index = arm_.controller_index();
diff --git a/y2016/control_loops/superstructure/superstructure_controls.cc b/y2016/control_loops/superstructure/superstructure_controls.cc
index 7d4dea5..ba7576a 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.cc
+++ b/y2016/control_loops/superstructure/superstructure_controls.cc
@@ -13,7 +13,6 @@
namespace superstructure {
using ::frc971::PotAndIndexPosition;
-using ::frc971::EstimatorState;
namespace {
double UseUnlessZero(double target_value, double default_value) {
@@ -30,148 +29,23 @@
// Intake
Intake::Intake()
- : ProfiledSubsystem(
+ : ::frc971::control_loops::SingleDOFProfiledSubsystem(
::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();
- 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 (estimator_.error()) {
- LOG(ERROR, "zeroing error with intake_estimator\n");
- return;
- }
-
- if (!initialized_) {
- if (estimator_.offset_ready()) {
- UpdateIntakeOffset(estimator_.offset());
- initialized_ = true;
- }
- }
-
- if (!zeroed(0) && estimator_.zeroed()) {
- UpdateIntakeOffset(estimator_.offset());
- set_zeroed(0, true);
- }
-
- Y_ << position.encoder;
- Y_ += offset_;
- loop_->Correct(Y_);
-}
-
-void Intake::CapGoal(const char *name, Eigen::Matrix<double, 3, 1> *goal) {
- // Limit the goal to min/max allowable angles.
- if ((*goal)(0, 0) > constants::Values::kIntakeRange.upper) {
- LOG(WARNING, "Intake goal %s above limit, %f > %f\n", name, (*goal)(0, 0),
- constants::Values::kIntakeRange.upper);
- (*goal)(0, 0) = constants::Values::kIntakeRange.upper;
- }
- if ((*goal)(0, 0) < constants::Values::kIntakeRange.lower) {
- LOG(WARNING, "Intake goal %s below limit, %f < %f\n", name, (*goal)(0, 0),
- constants::Values::kIntakeRange.lower);
- (*goal)(0, 0) = constants::Values::kIntakeRange.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() {
- // Returns whether hard limits have been exceeded.
-
- if (angle() > constants::Values::kIntakeRange.upper_hard ||
- angle() < constants::Values::kIntakeRange.lower_hard) {
- LOG(ERROR, "Intake at %f out of bounds [%f, %f], ESTOPing\n", angle(),
- constants::Values::kIntakeRange.lower_hard,
- constants::Values::kIntakeRange.upper_hard);
- return true;
- }
-
- return false;
-}
-
-void Intake::set_max_voltage(double voltage) {
- loop_->set_max_voltage(0, 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::DoReset() {
- estimator_.Reset();
-}
-
-EstimatorState Intake::IntakeEstimatorState() {
- EstimatorState estimator_state;
- ::frc971::zeroing::PopulateEstimatorState(estimator_, &estimator_state);
-
- return estimator_state;
-}
+ MakeIntegralIntakeLoop())),
+ constants::GetValues().intake.zeroing,
+ constants::Values::kIntakeRange, 10.0, 10.0) {}
Arm::Arm()
- : ProfiledSubsystem(::std::unique_ptr<ArmControlLoop>(new ArmControlLoop(
- ::y2016::control_loops::superstructure::MakeIntegralArmLoop()))),
+ : ProfiledSubsystem(
+ ::std::unique_ptr<ArmControlLoop>(new ArmControlLoop(
+ ::y2016::control_loops::superstructure::MakeIntegralArmLoop())),
+ {{constants::GetValues().shoulder.zeroing,
+ constants::GetValues().wrist.zeroing}}),
shoulder_profile_(::aos::controls::kLoopFrequency),
- wrist_profile_(::aos::controls::kLoopFrequency),
- shoulder_estimator_(constants::GetValues().shoulder.zeroing),
- wrist_estimator_(constants::GetValues().wrist.zeroing) {
+ wrist_profile_(::aos::controls::kLoopFrequency) {
Y_.setZero();
offset_.setZero();
AdjustProfile(0.0, 0.0, 0.0, 0.0);
@@ -220,33 +94,34 @@
void Arm::Correct(PotAndIndexPosition position_shoulder,
PotAndIndexPosition position_wrist) {
- shoulder_estimator_.UpdateEstimate(position_shoulder);
- wrist_estimator_.UpdateEstimate(position_wrist);
+ estimators_[kShoulderIndex].UpdateEstimate(position_shoulder);
+ estimators_[kWristIndex].UpdateEstimate(position_wrist);
// Handle zeroing errors
- if (shoulder_estimator_.error()) {
+ if (estimators_[kShoulderIndex].error()) {
LOG(ERROR, "zeroing error with shoulder_estimator\n");
return;
}
- if (wrist_estimator_.error()) {
+ if (estimators_[kWristIndex].error()) {
LOG(ERROR, "zeroing error with wrist_estimator\n");
return;
}
if (!initialized_) {
- if (shoulder_estimator_.offset_ready() && wrist_estimator_.offset_ready()) {
- UpdateShoulderOffset(shoulder_estimator_.offset());
- UpdateWristOffset(wrist_estimator_.offset());
+ if (estimators_[kShoulderIndex].offset_ready() &&
+ estimators_[kWristIndex].offset_ready()) {
+ UpdateShoulderOffset(estimators_[kShoulderIndex].offset());
+ UpdateWristOffset(estimators_[kWristIndex].offset());
initialized_ = true;
}
}
- if (!zeroed(kShoulderIndex) && shoulder_estimator_.zeroed()) {
- UpdateShoulderOffset(shoulder_estimator_.offset());
+ if (!zeroed(kShoulderIndex) && estimators_[kShoulderIndex].zeroed()) {
+ UpdateShoulderOffset(estimators_[kShoulderIndex].offset());
set_zeroed(kShoulderIndex, true);
}
- if (!zeroed(kWristIndex) && wrist_estimator_.zeroed()) {
- UpdateWristOffset(wrist_estimator_.offset());
+ if (!zeroed(kWristIndex) && estimators_[kWristIndex].zeroed()) {
+ UpdateWristOffset(estimators_[kWristIndex].offset());
set_zeroed(kWristIndex, true);
}
@@ -370,32 +245,6 @@
}
}
-void Arm::set_max_voltage(double shoulder_max_voltage,
- double wrist_max_voltage) {
- loop_->set_max_voltage(0, shoulder_max_voltage);
- loop_->set_max_voltage(1, wrist_max_voltage);
-}
-
-void Arm::DoReset() {
- shoulder_estimator_.Reset();
- wrist_estimator_.Reset();
-}
-
-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
index adb40e0..7f7370f 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.h
+++ b/y2016/control_loops/superstructure/superstructure_controls.h
@@ -4,13 +4,14 @@
#include <memory>
#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/control_loops/profiled_subsystem.h"
+#include "frc971/control_loops/simple_capped_state_feedback_loop.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/integral_arm_plant.h"
+#include "y2016/control_loops/superstructure/superstructure.q.h"
namespace y2016 {
namespace control_loops {
@@ -66,7 +67,8 @@
const double overage_amount = U(0, 0) - max_voltage(0);
mutable_U(0, 0) = max_voltage(0);
const double coupled_amount =
- (Kff().block<1, 2>(1, 2) * B().block<2, 1>(2, 0))(0, 0) * overage_amount;
+ (Kff().block<1, 2>(1, 2) * B().block<2, 1>(2, 0))(0, 0) *
+ overage_amount;
LOG(DEBUG, "Removing coupled amount %f\n", coupled_amount);
mutable_U(1, 0) += coupled_amount;
}
@@ -96,149 +98,15 @@
}
};
-template <int number_of_states, int number_of_axes>
-class ProfiledSubsystem {
- public:
- 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 {
- 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> {
+class Intake : public ::frc971::control_loops::SingleDOFProfiledSubsystem {
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> {
+
+class Arm : public ::frc971::control_loops::ProfiledSubsystem<6, 2> {
public:
Arm();
- // Returns whether an error has occured
- bool error() const {
- return shoulder_estimator_.error() || wrist_estimator_.error();
- }
// Updates our estimator with the latest position.
void Correct(::frc971::PotAndIndexPosition position_shoulder,
@@ -259,7 +127,6 @@
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);
void set_shoulder_asymetric_limits(double shoulder_min_voltage,
double shoulder_max_voltage) {
@@ -269,10 +136,6 @@
// Returns true if we have exceeded any hard limits.
bool CheckHardLimits();
- // 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); }
@@ -283,12 +146,9 @@
// For testing:
// Triggers an estimator error.
- void TriggerEstimatorError() { shoulder_estimator_.TriggerError(); }
+ void TriggerEstimatorError() { estimators_[0].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);
@@ -300,7 +160,6 @@
friend class testing::SuperstructureTest_DisabledGoalTest_Test;
aos::util::TrapezoidProfile shoulder_profile_, wrist_profile_;
- ::frc971::zeroing::ZeroingEstimator shoulder_estimator_, wrist_estimator_;
// Current measurement.
Eigen::Matrix<double, 2, 1> Y_;