Merge "Add encoder filters and update max pulses."
diff --git a/frc971/constants.h b/frc971/constants.h
index 890c63c..ad3ce71 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -16,6 +16,15 @@
double allowable_encoder_error;
};
+// Defines a range of motion for a subsystem.
+// These are all absolute positions in scaled units.
+struct Range {
+ double lower_hard;
+ double upper_hard;
+ double lower;
+ double upper;
+};
+
} // namespace constants
} // namespace frc971
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/tools/bazel.rc b/tools/bazel.rc
index 9ec4887..7df645e 100644
--- a/tools/bazel.rc
+++ b/tools/bazel.rc
@@ -43,6 +43,6 @@
build:msan --test_env MSAN_SYMBOLIZER_PATH=/usr/bin/llvm-symbolizer-3.6
# Show paths to a few more than just 1 target.
-build --show_result 15
+build --show_result 5
# Dump the output of the failing test to stdout.
test --test_output=errors
diff --git a/y2016/constants.cc b/y2016/constants.cc
index 56fb2ad..8af39a1 100644
--- a/y2016/constants.cc
+++ b/y2016/constants.cc
@@ -36,8 +36,8 @@
Values::kIntakeEncoderIndexDifference,
Values::kShoulderEncoderIndexDifference,
Values::kWristEncoderIndexDifference;
-constexpr Values::Range Values::kIntakeRange, Values::kShoulderRange,
- Values::kWristRange;
+constexpr ::frc971::constants::Range Values::kIntakeRange,
+ Values::kShoulderRange, Values::kWristRange;
namespace {
const uint16_t kCompTeamNumber = 971;
@@ -47,7 +47,33 @@
const Values *DoGetValuesForTeam(uint16_t team) {
switch (team) {
- case 1:
+ case 1: // for tests
+ return new Values{
+ 5.0, // drivetrain max speed
+
+ // Intake
+ {
+ 0.0,
+ {Values::kZeroingSampleSize, Values::kIntakeEncoderIndexDifference,
+ 0.0, 0.3},
+ },
+
+ // Shoulder
+ {
+ 0.0,
+ {Values::kZeroingSampleSize, Values::kShoulderEncoderIndexDifference,
+ 0.0, 0.3},
+ },
+
+ // Wrist
+ {
+ 0.0,
+ {Values::kZeroingSampleSize, Values::kWristEncoderIndexDifference,
+ 0.0, 0.3},
+ },
+ };
+ break;
+
case kCompTeamNumber:
return new Values{
5.0, // drivetrain max speed
diff --git a/y2016/constants.h b/y2016/constants.h
index ed86d0c..c2fc4ca 100644
--- a/y2016/constants.h
+++ b/y2016/constants.h
@@ -22,15 +22,6 @@
// This structure contains current values for all of the things that change.
struct Values {
- // Defines a range of motion for a subsystem.
- // These are all absolute positions in scaled units.
- struct Range {
- double lower_hard;
- double upper_hard;
- double lower;
- double upper;
- };
-
// ///// Mutual constants between robots. /////
// TODO(constants): Update/check these with what we're using this year.
static const int kZeroingSampleSize = 200;
@@ -69,9 +60,11 @@
// Subsystem motion ranges, in whatever units that their respective queues say
// the use.
- static constexpr Range kIntakeRange{-0.270, 2.0, -0.200, 1.9};
- static constexpr Range kShoulderRange{-0.050, 2.0, 0.000, 1.9};
- static constexpr Range kWristRange{-2.0, 2.0, -1.9, 1.0};
+ static constexpr ::frc971::constants::Range kIntakeRange{-0.270, 2.0, -0.200,
+ 1.9};
+ static constexpr ::frc971::constants::Range kShoulderRange{-0.050, 2.0, 0.000,
+ 1.9};
+ static constexpr ::frc971::constants::Range kWristRange{-2.0, 2.0, -1.9, 1.0};
// ///// Dynamic constants. /////
double drivetrain_max_speed;
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.cc b/y2016/control_loops/superstructure/superstructure.cc
index 214fc6c..cdbceb4 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -14,6 +14,13 @@
namespace superstructure {
namespace {
+constexpr double kIntakeEncoderIndexDifference =
+ constants::Values::kIntakeEncoderIndexDifference;
+constexpr double kWristEncoderIndexDifference =
+ constants::Values::kWristEncoderIndexDifference;
+constexpr double kShoulderEncoderIndexDifference =
+ constants::Values::kShoulderEncoderIndexDifference;
+
constexpr double kZeroingVoltage = 4.0;
} // namespace
@@ -22,25 +29,74 @@
: aos::controls::ControlLoop<control_loops::SuperstructureQueue>(
superstructure_queue) {}
-void Superstructure::UpdateZeroingState() {
- // TODO(austin): Explicit state transitions instead of this.
- // TODO(adam): Change this once we have zeroing written.
- if (!arm_.initialized() || !intake_.initialized()) {
- state_ = INITIALIZING;
- } else if (!intake_.zeroed()) {
- state_ = ZEROING_INTAKE;
- } else if (!arm_.zeroed()) {
- state_ = ZEROING_ARM;
+bool Superstructure::IsArmNear(double shoulder_tolerance,
+ double wrist_tolerance) {
+ return ((arm_.unprofiled_goal() - arm_.X_hat())
+ .block<2, 1>(0, 0)
+ .lpNorm<Eigen::Infinity>() < shoulder_tolerance) &&
+ ((arm_.unprofiled_goal() - arm_.X_hat())
+ .block<4, 1>(0, 0)
+ .lpNorm<Eigen::Infinity>() < wrist_tolerance) &&
+ ((arm_.unprofiled_goal() - arm_.goal())
+ .block<4, 1>(0, 0)
+ .lpNorm<Eigen::Infinity>() < 1e-6);
+}
+
+bool Superstructure::IsArmNear(double tolerance) {
+ return ((arm_.unprofiled_goal() - arm_.X_hat())
+ .block<4, 1>(0, 0)
+ .lpNorm<Eigen::Infinity>() < tolerance) &&
+ ((arm_.unprofiled_goal() - arm_.goal())
+ .block<4, 1>(0, 0)
+ .lpNorm<Eigen::Infinity>() < 1e-6);
+}
+
+bool Superstructure::IsIntakeNear(double tolerance) {
+ return ((intake_.unprofiled_goal() - intake_.X_hat())
+ .block<2, 1>(0, 0)
+ .lpNorm<Eigen::Infinity>() < tolerance);
+}
+
+double Superstructure::MoveButKeepAbove(double reference_angle,
+ double current_angle,
+ double move_distance) {
+ return -MoveButKeepBelow(-reference_angle, -current_angle, -move_distance);
+}
+
+double Superstructure::MoveButKeepBelow(double reference_angle,
+ double current_angle,
+ double move_distance) {
+ // There are 3 interesting places to move to.
+ const double small_negative_move = current_angle - move_distance;
+ const double small_positive_move = current_angle + move_distance;
+ // And the reference angle.
+
+ // Move the the highest one that is below reference_angle.
+ if (small_negative_move > reference_angle) {
+ return reference_angle;
+ } else if (small_positive_move > reference_angle) {
+ return small_negative_move;
} else {
- state_ = RUNNING;
+ return small_positive_move;
}
}
+constexpr double Superstructure::kShoulderMiddleAngle;
+constexpr double Superstructure::kLooseTolerance;
+constexpr double Superstructure::kIntakeUpperClear;
+constexpr double Superstructure::kIntakeLowerClear;
+constexpr double Superstructure::kShoulderUpAngle;
+constexpr double Superstructure::kShoulderLanded;
+constexpr double Superstructure::kTightTolerance;
+constexpr double Superstructure::kWristAlmostLevel;
+constexpr double Superstructure::kShoulderWristClearAngle;
+
void Superstructure::RunIteration(
const control_loops::SuperstructureQueue::Goal *unsafe_goal,
const control_loops::SuperstructureQueue::Position *position,
control_loops::SuperstructureQueue::Output *output,
control_loops::SuperstructureQueue::Status *status) {
+ const State state_before_switch = state_;
if (WasReset()) {
LOG(ERROR, "WPILib reset, restarting\n");
arm_.Reset();
@@ -54,40 +110,214 @@
arm_.Correct(position->shoulder, position->wrist);
intake_.Correct(position->intake);
- // Zeroing will work as follows:
- // Start with the intake. Move it towards the center. Once zeroed, move it
- // back to the bottom. Rotate the shoulder towards the center. Once zeroed,
- // move it up enough to rotate the wrist towards the center.
-
- // We'll then need code to do sanity checking on values.
+ // There are 2 main zeroing paths, HIGH_ARM_ZERO and LOW_ARM_ZERO.
+ //
+ // HIGH_ARM_ZERO works by lifting the arm all the way up so it is clear,
+ // moving the shooter to be horizontal, moving the intake out, and then moving
+ // the arm back down.
+ //
+ // LOW_ARM_ZERO works by moving the intake out of the way, lifting the arm up,
+ // leveling the shooter, and then moving back down.
switch (state_) {
case UNINITIALIZED:
- LOG(DEBUG, "Uninitialized\n");
- state_ = INITIALIZING;
+ // Wait in the uninitialized state until both the arm and intake are
+ // initialized.
+ LOG(DEBUG, "Uninitialized, waiting for intake and arm\n");
+ if (arm_.initialized() && intake_.initialized()) {
+ state_ = DISABLED_INITIALIZED;
+ }
disable = true;
break;
- case INITIALIZING:
- LOG(DEBUG, "Waiting for accurate initial position.\n");
+ case DISABLED_INITIALIZED:
+ // Wait here until we are either fully zeroed while disabled, or we become
+ // enabled. At that point, figure out if we should HIGH_ARM_ZERO or
+ // LOW_ARM_ZERO.
+ if (disable) {
+ if (arm_.zeroed() && intake_.zeroed()) {
+ state_ = SLOW_RUNNING;
+ }
+ } else {
+ if (arm_.shoulder_angle() >= kShoulderMiddleAngle) {
+ state_ = HIGH_ARM_ZERO_LIFT_ARM;
+ } else {
+ state_ = LOW_ARM_ZERO_LOWER_INTAKE;
+ }
+ }
+
+ // Set the goals to where we are now so when we start back up, we don't
+ // jump.
+ intake_.ForceGoal(intake_.angle());
+ arm_.ForceGoal(arm_.shoulder_angle(), arm_.wrist_angle());
+ // Set up the profile to be the zeroing profile.
+ intake_.AdjustProfile(0.5, 10);
+ arm_.AdjustProfile(0.5, 10, 0.5, 10);
+
+ // We are not ready to start doing anything yet.
disable = true;
- // Update state_ to accurately represent the state of the zeroing
- // estimators.
- UpdateZeroingState();
- if (state_ != INITIALIZING) {
- // Set the goals to where we are now.
- intake_.ForceGoal(intake_.angle());
- arm_.ForceGoal(arm_.shoulder_angle(), arm_.wrist_angle());
+ break;
+
+ case HIGH_ARM_ZERO_LIFT_ARM:
+ if (disable) {
+ state_ = DISABLED_INITIALIZED;
+ } else {
+ // Raise the shoulder up out of the way.
+ arm_.set_unprofiled_goal(kShoulderUpAngle, arm_.wrist_angle());
+ if (IsArmNear(kLooseTolerance)) {
+ // Close enough, start the next move.
+ state_ = HIGH_ARM_ZERO_LEVEL_SHOOTER;
+ }
}
break;
- case ZEROING_INTAKE:
- case ZEROING_ARM:
- // TODO(adam): Add your magic here.
- state_ = RUNNING;
+ case HIGH_ARM_ZERO_LEVEL_SHOOTER:
+ if (disable) {
+ state_ = DISABLED_INITIALIZED;
+ } else {
+ // Move the shooter to be level.
+ arm_.set_unprofiled_goal(kShoulderUpAngle, 0.0);
+
+ if (IsArmNear(kLooseTolerance)) {
+ // Close enough, start the next move.
+ state_ = HIGH_ARM_ZERO_MOVE_INTAKE_OUT;
+ }
+ }
break;
+ case HIGH_ARM_ZERO_MOVE_INTAKE_OUT:
+ if (disable) {
+ state_ = DISABLED_INITIALIZED;
+ } else {
+ // If we were just asked to move the intake, make sure it moves far
+ // enough.
+ if (last_state_ != HIGH_ARM_ZERO_MOVE_INTAKE_OUT) {
+ intake_.set_unprofiled_goal(
+ MoveButKeepBelow(kIntakeUpperClear, intake_.angle(),
+ kIntakeEncoderIndexDifference * 2.5));
+ }
+
+ if (IsIntakeNear(kLooseTolerance)) {
+ // Close enough, start the next move.
+ state_ = HIGH_ARM_ZERO_LOWER_ARM;
+ }
+ }
+ break;
+
+ case HIGH_ARM_ZERO_LOWER_ARM:
+ if (disable) {
+ state_ = DISABLED_INITIALIZED;
+ } else {
+ // Land the shooter in the belly-pan. It should be zeroed by the time
+ // it gets there. If not, just estop.
+ arm_.set_unprofiled_goal(kShoulderLanded, 0.0);
+ if (arm_.zeroed() && intake_.zeroed()) {
+ state_ = RUNNING;
+ } else if (IsArmNear(kLooseTolerance)) {
+ LOG(ERROR,
+ "Failed to zero while executing the HIGH_ARM_ZERO sequence. "
+ "Arm: %d Intake %d\n",
+ arm_.zeroed(), intake_.zeroed());
+ state_ = ESTOP;
+ }
+ }
+ break;
+
+ case LOW_ARM_ZERO_LOWER_INTAKE:
+ if (disable) {
+ state_ = DISABLED_INITIALIZED;
+ } else {
+ // Move the intake down out of the way of the arm. Make sure to move it
+ // far enough to zero.
+ if (last_state_ != LOW_ARM_ZERO_LOWER_INTAKE) {
+ intake_.set_unprofiled_goal(
+ MoveButKeepBelow(kIntakeLowerClear, intake_.angle(),
+ kIntakeEncoderIndexDifference * 2.5));
+ }
+ if (IsIntakeNear(kLooseTolerance)) {
+ if (::std::abs(arm_.wrist_angle()) < kWristAlmostLevel) {
+ state_ = LOW_ARM_ZERO_MAYBE_LEVEL_SHOOTER;
+ } else {
+ state_ = LOW_ARM_ZERO_LIFT_SHOULDER;
+ }
+ }
+ }
+ break;
+
+ case LOW_ARM_ZERO_MAYBE_LEVEL_SHOOTER:
+ if (disable) {
+ state_ = DISABLED_INITIALIZED;
+ } else {
+ // If we are supposed to level the shooter, set it to level, and wait
+ // until it is very close to level.
+ arm_.set_unprofiled_goal(arm_.unprofiled_goal(0, 0), 0.0);
+ if (IsArmNear(kLooseTolerance, kTightTolerance)) {
+ state_ = LOW_ARM_ZERO_LIFT_SHOULDER;
+ }
+ }
+ break;
+
+ case LOW_ARM_ZERO_LIFT_SHOULDER:
+ if (disable) {
+ state_ = DISABLED_INITIALIZED;
+ } else {
+ // Decide where to move to. We need to move far enough to see an index
+ // pulse, but must also get high enough that we can safely level the
+ // shooter.
+ if (last_state_ != LOW_ARM_ZERO_LIFT_SHOULDER) {
+ arm_.set_unprofiled_goal(
+ MoveButKeepAbove(kShoulderWristClearAngle, arm_.shoulder_angle(),
+ ::std::max(kWristEncoderIndexDifference,
+ kShoulderEncoderIndexDifference) *
+ 2.5),
+ arm_.unprofiled_goal(2, 0));
+ }
+
+ // Wait until we are level and then go for it.
+ if (IsArmNear(kLooseTolerance)) {
+ state_ = LOW_ARM_ZERO_LEVEL_SHOOTER;
+ }
+ }
+ break;
+
+ case LOW_ARM_ZERO_LEVEL_SHOOTER:
+ if (disable) {
+ state_ = DISABLED_INITIALIZED;
+ } else {
+ // Move the shooter level (and keep the same height). We don't want to
+ // got to RUNNING until we are completely level so that we don't
+ // give control back in a weird case where we might crash.
+ arm_.set_unprofiled_goal(arm_.unprofiled_goal(0, 0), 0.0);
+ if (IsArmNear(kLooseTolerance)) {
+ if (arm_.zeroed() && intake_.zeroed()) {
+ state_ = RUNNING;
+ } else {
+ LOG(ERROR,
+ "Failed to zero while executing the LOW_ARM_ZERO sequence. "
+ "Arm: %d Intake %d\n",
+ arm_.zeroed(), intake_.zeroed());
+ state_ = ESTOP;
+ }
+ }
+ }
+ break;
+
+ case SLOW_RUNNING:
case RUNNING:
+ if (disable) {
+ // TODO(austin): Enter SLOW_RUNNING if we are collided.
+
+ // If we are disabled, reset the profile to the current position.
+ intake_.ForceGoal(intake_.angle());
+ arm_.ForceGoal(arm_.shoulder_angle(), arm_.wrist_angle());
+ } else {
+ if (state_ == SLOW_RUNNING) {
+ // TODO(austin): Exit SLOW_RUNNING if we are not collided.
+ LOG(ERROR, "Need to transition on non-collided, not all the time.\n");
+ state_ = RUNNING;
+ }
+ }
+
if (unsafe_goal) {
arm_.AdjustProfile(unsafe_goal->max_angular_velocity_shoulder,
unsafe_goal->max_angular_acceleration_shoulder,
@@ -101,11 +331,13 @@
intake_.set_unprofiled_goal(unsafe_goal->angle_intake);
}
- // Update state_ to accurately represent the state of the zeroing
- // estimators.
-
- if (state_ != RUNNING && state_ != ESTOP) {
- state_ = UNINITIALIZED;
+ // ESTOP if we hit any of the limits. It is safe(ish) to hit the limits
+ // while zeroing since we use such low power.
+ if (state_ == RUNNING || state_ == SLOW_RUNNING) {
+ // ESTOP if we hit the hard limits.
+ if ((arm_.CheckHardLimits() || intake_.CheckHardLimits()) && output) {
+ state_ = ESTOP;
+ }
}
break;
@@ -115,15 +347,6 @@
break;
}
- // ESTOP if we hit any of the limits. It is safe(ish) to hit the limits while
- // zeroing since we use such low power.
- if (state_ == RUNNING) {
- // ESTOP if we hit the hard limits.
- if ((arm_.CheckHardLimits() || intake_.CheckHardLimits()) && output) {
- state_ = ESTOP;
- }
- }
-
// Set the voltage limits.
const double max_voltage = state_ == RUNNING ? 12.0 : kZeroingVoltage;
arm_.set_max_voltage(max_voltage, max_voltage);
@@ -174,7 +397,7 @@
status->state = state_;
- last_state_ = state_;
+ last_state_ = state_before_switch;
}
} // namespace superstructure
diff --git a/y2016/control_loops/superstructure/superstructure.h b/y2016/control_loops/superstructure/superstructure.h
index 9a04c3c..1350a15 100644
--- a/y2016/control_loops/superstructure/superstructure.h
+++ b/y2016/control_loops/superstructure/superstructure.h
@@ -24,23 +24,80 @@
explicit Superstructure(
control_loops::SuperstructureQueue *my_superstructure =
&control_loops::superstructure_queue);
+
+ // This is the angle above which we will do a HIGH_ARM_ZERO, and below which
+ // we will do a LOW_ARM_ZERO.
+ static constexpr double kShoulderMiddleAngle = M_PI / 4.0;
+ // This is the large scale movement tolerance.
+ static constexpr double kLooseTolerance = 0.05;
+
+ // This is the small scale movement tolerance.
+ static constexpr double kTightTolerance = 0.01;
+
+ // This is the angle such that the intake will clear the arm when the shooter
+ // is level.
+ static constexpr double kIntakeUpperClear = 1.1;
+ // This is the angle such that the intake will clear the arm when the shooter
+ // is at almost any position.
+ static constexpr double kIntakeLowerClear = 0.5;
+
+ // This is the angle that the shoulder will go to when doing the
+ // HIGH_ARM_ZERO.
+ static constexpr double kShoulderUpAngle = M_PI / 2.0;
+
+ // This is the angle that the shoulder will go down to when landing in the
+ // bellypan.
+ static constexpr double kShoulderLanded = -0.02;
+
+ // This is the angle below which we consider the wrist close enough to level
+ // that we should move it to level before doing anything.
+ static constexpr double kWristAlmostLevel = 0.10;
+
+ // This is the angle that the shoulder will go down to when raising up before
+ // leveling the shooter for calibration.
+ static constexpr double kShoulderWristClearAngle = 0.6;
+
enum State {
- // Waiting to receive data before doing anything.
+ // Wait for all the filters to be ready before starting the initialization
+ // process.
UNINITIALIZED = 0,
- // Estimating the starting location.
- INITIALIZING = 1,
- // Moving the intake to find an index pulse.
- ZEROING_INTAKE = 2,
- // Moving the arm to find an index pulse.
- ZEROING_ARM = 3,
- // All good!
- RUNNING = 4,
+
+ // We now are ready to decide how to zero. Decide what to do once we are
+ // enabled.
+ DISABLED_INITIALIZED = 1,
+
+ // Lift the arm up out of the way.
+ HIGH_ARM_ZERO_LIFT_ARM = 2,
+
+ HIGH_ARM_ZERO_LEVEL_SHOOTER = 3,
+
+ HIGH_ARM_ZERO_MOVE_INTAKE_OUT = 4,
+
+ HIGH_ARM_ZERO_LOWER_ARM = 6,
+
+ LOW_ARM_ZERO_LOWER_INTAKE = 7,
+ LOW_ARM_ZERO_MAYBE_LEVEL_SHOOTER = 8,
+ LOW_ARM_ZERO_LIFT_SHOULDER = 9,
+ LOW_ARM_ZERO_LEVEL_SHOOTER = 11,
+ // Run, but limit power to zeroing voltages.
+ SLOW_RUNNING = 12,
+ // Run with full power.
+ RUNNING = 13,
// Internal error caused the superstructure to abort.
- ESTOP = 5,
+ ESTOP = 14,
};
State state() const { return state_; }
+ // Returns the value to move the joint to such that it will stay below
+ // reference_angle starting at current_angle, but move at least move_distance
+ static double MoveButKeepBelow(double reference_angle, double current_angle,
+ double move_distance);
+ // Returns the value to move the joint to such that it will stay above
+ // reference_angle starting at current_angle, but move at least move_distance
+ static double MoveButKeepAbove(double reference_angle, double current_angle,
+ double move_distance);
+
protected:
virtual void RunIteration(
const control_loops::SuperstructureQueue::Goal *unsafe_goal,
@@ -56,9 +113,11 @@
State state_ = UNINITIALIZED;
State last_state_ = UNINITIALIZED;
- // Sets state_ to the correct state given the current state of the zeroing
- // estimators.
- void UpdateZeroingState();
+ // Returns true if the profile has finished, and the joint is within the
+ // specified tolerance.
+ bool IsArmNear(double tolerance);
+ bool IsArmNear(double shoulder_tolerance, double wrist_tolerance);
+ bool IsIntakeNear(double tolerance);
DISALLOW_COPY_AND_ASSIGN(Superstructure);
};
diff --git a/y2016/control_loops/superstructure/superstructure_controls.cc b/y2016/control_loops/superstructure/superstructure_controls.cc
index e43c245..a8953de 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();
@@ -85,12 +73,12 @@
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) {
+ 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) {
+ 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;
@@ -133,8 +121,8 @@
bool Intake::CheckHardLimits() {
// Returns whether hard limits have been exceeded.
- if (angle() >= constants::Values::kIntakeRange.upper_hard ||
- angle() <= constants::Values::kIntakeRange.lower_hard) {
+ 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;
@@ -144,7 +132,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,
@@ -168,7 +156,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),
@@ -253,12 +241,12 @@
void Arm::CapGoal(const char *name, Eigen::Matrix<double, 6, 1> *goal) {
// Limit the goals to min/max allowable angles.
- if ((*goal)(0, 0) >= constants::Values::kShoulderRange.upper) {
+ if ((*goal)(0, 0) > constants::Values::kShoulderRange.upper) {
LOG(WARNING, "Shoulder goal %s above limit, %f > %f\n", name, (*goal)(0, 0),
constants::Values::kShoulderRange.upper);
(*goal)(0, 0) = constants::Values::kShoulderRange.upper;
}
- if ((*goal)(0, 0) <= constants::Values::kShoulderRange.lower) {
+ if ((*goal)(0, 0) < constants::Values::kShoulderRange.lower) {
LOG(WARNING, "Shoulder goal %s below limit, %f < %f\n", name, (*goal)(0, 0),
constants::Values::kShoulderRange.lower);
(*goal)(0, 0) = constants::Values::kShoulderRange.lower;
@@ -266,12 +254,12 @@
const double wrist_goal_angle_ungrounded = (*goal)(2, 0) - (*goal)(0, 0);
- if (wrist_goal_angle_ungrounded >= constants::Values::kWristRange.upper) {
+ if (wrist_goal_angle_ungrounded > constants::Values::kWristRange.upper) {
LOG(WARNING, "Wrist goal %s above limit, %f > %f\n", name,
wrist_goal_angle_ungrounded, constants::Values::kWristRange.upper);
(*goal)(2, 0) = constants::Values::kWristRange.upper + (*goal)(0, 0);
}
- if (wrist_goal_angle_ungrounded <= constants::Values::kWristRange.lower) {
+ if (wrist_goal_angle_ungrounded < constants::Values::kWristRange.lower) {
LOG(WARNING, "Wrist goal %s below limit, %f < %f\n", name,
wrist_goal_angle_ungrounded, constants::Values::kWristRange.lower);
(*goal)(2, 0) = constants::Values::kWristRange.lower + (*goal)(0, 0);
@@ -309,16 +297,16 @@
}
bool Arm::CheckHardLimits() {
- if (shoulder_angle() >= constants::Values::kShoulderRange.upper_hard ||
- shoulder_angle() <= constants::Values::kShoulderRange.lower_hard) {
+ if (shoulder_angle() > constants::Values::kShoulderRange.upper_hard ||
+ shoulder_angle() < constants::Values::kShoulderRange.lower_hard) {
LOG(ERROR, "Shoulder at %f out of bounds [%f, %f], ESTOPing\n",
shoulder_angle(), constants::Values::kShoulderRange.lower_hard,
constants::Values::kShoulderRange.upper_hard);
return true;
}
- if (wrist_angle() - shoulder_angle() >= constants::Values::kWristRange.upper_hard ||
- wrist_angle() - shoulder_angle() <= constants::Values::kWristRange.lower_hard) {
+ if (wrist_angle() - shoulder_angle() > constants::Values::kWristRange.upper_hard ||
+ wrist_angle() - shoulder_angle() < constants::Values::kWristRange.lower_hard) {
LOG(ERROR, "Wrist at %f out of bounds [%f, %f], ESTOPing\n",
wrist_angle() - shoulder_angle(), constants::Values::kWristRange.lower_hard,
constants::Values::kWristRange.upper_hard);
@@ -363,7 +351,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..d20b345 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();
@@ -61,21 +27,19 @@
// 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);
-
- // 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();
@@ -114,7 +78,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 +169,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_;
diff --git a/y2016/control_loops/superstructure/superstructure_lib_test.cc b/y2016/control_loops/superstructure/superstructure_lib_test.cc
index 1bd4eca..6d8c18f 100644
--- a/y2016/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2016/control_loops/superstructure/superstructure_lib_test.cc
@@ -80,7 +80,7 @@
static constexpr double kNoiseScalar = 0.1;
SuperstructureSimulation()
: intake_plant_(new IntakePlant(MakeIntakePlant())),
- plant_arm_(new ArmPlant(MakeArmPlant())),
+ arm_plant_(new ArmPlant(MakeArmPlant())),
pot_encoder_intake_(constants::Values::kIntakeEncoderIndexDifference),
pot_encoder_shoulder_(
constants::Values::kShoulderEncoderIndexDifference),
@@ -103,16 +103,16 @@
}
void InitializeShoulderPosition(double start_pos) {
- plant_arm_->mutable_X(0, 0) = start_pos;
- plant_arm_->mutable_X(1, 0) = 0.0;
+ arm_plant_->mutable_X(0, 0) = start_pos;
+ arm_plant_->mutable_X(1, 0) = 0.0;
pot_encoder_shoulder_.Initialize(start_pos, kNoiseScalar);
}
// Must be called after any changes to InitializeShoulderPosition.
void InitializeWristPosition(double start_pos) {
- plant_arm_->mutable_X(2, 0) = start_pos + plant_arm_->X(0, 0);
- plant_arm_->mutable_X(3, 0) = 0.0;
+ arm_plant_->mutable_X(2, 0) = start_pos + arm_plant_->X(0, 0);
+ arm_plant_->mutable_X(3, 0) = 0.0;
pot_encoder_wrist_.Initialize(start_pos, kNoiseScalar);
}
@@ -129,13 +129,17 @@
position.Send();
}
+ double shoulder_angle() const { return arm_plant_->X(0, 0); }
+ double wrist_angle() const { return arm_plant_->X(2, 0); }
+ double intake_angle() const { return intake_plant_->X(0, 0); }
+
// Sets the difference between the commanded and applied powers.
// This lets us test that the integrators work.
void set_power_error(double power_error_intake, double power_error_shoulder,
double power_error_wrist) {
intake_plant_->set_voltage_offset(power_error_intake);
- plant_arm_->set_shoulder_voltage_offset(power_error_shoulder);
- plant_arm_->set_wrist_voltage_offset(power_error_wrist);
+ arm_plant_->set_shoulder_voltage_offset(power_error_shoulder);
+ arm_plant_->set_wrist_voltage_offset(power_error_wrist);
}
// Simulates for a single timestep.
@@ -146,19 +150,19 @@
intake_plant_->mutable_U() << superstructure_queue_.output->voltage_intake +
intake_plant_->voltage_offset();
- plant_arm_->mutable_U() << superstructure_queue_.output->voltage_shoulder +
- plant_arm_->shoulder_voltage_offset(),
+ arm_plant_->mutable_U() << superstructure_queue_.output->voltage_shoulder +
+ arm_plant_->shoulder_voltage_offset(),
superstructure_queue_.output->voltage_wrist +
- plant_arm_->wrist_voltage_offset();
+ arm_plant_->wrist_voltage_offset();
// Use the plant to generate the next physical state given the voltages to
// the motors.
intake_plant_->Update();
- plant_arm_->Update();
+ arm_plant_->Update();
const double angle_intake = intake_plant_->Y(0, 0);
- const double angle_shoulder = plant_arm_->Y(0, 0);
- const double angle_wrist = plant_arm_->Y(1, 0);
+ const double angle_shoulder = arm_plant_->Y(0, 0);
+ const double angle_wrist = arm_plant_->Y(1, 0);
// Use the physical state to simulate sensor readings.
pot_encoder_intake_.MoveTo(angle_intake);
@@ -176,7 +180,7 @@
private:
::std::unique_ptr<IntakePlant> intake_plant_;
- ::std::unique_ptr<ArmPlant> plant_arm_;
+ ::std::unique_ptr<ArmPlant> arm_plant_;
PositionSensorSimulator pot_encoder_intake_;
PositionSensorSimulator pot_encoder_shoulder_;
@@ -209,6 +213,13 @@
superstructure_queue_.status->shoulder.angle, 0.001);
EXPECT_NEAR(superstructure_queue_.goal->angle_wrist,
superstructure_queue_.status->wrist.angle, 0.001);
+
+ EXPECT_NEAR(superstructure_queue_.goal->angle_intake,
+ superstructure_plant_.intake_angle(), 0.001);
+ EXPECT_NEAR(superstructure_queue_.goal->angle_shoulder,
+ superstructure_plant_.shoulder_angle(), 0.001);
+ EXPECT_NEAR(superstructure_queue_.goal->angle_wrist,
+ superstructure_plant_.wrist_angle(), 0.001);
}
// Runs one iteration of the whole simulation and checks that separation
@@ -340,7 +351,7 @@
ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
.angle_intake(constants::Values::kIntakeRange.lower)
.angle_shoulder(constants::Values::kShoulderRange.lower)
- .angle_wrist(constants::Values::kWristRange.lower)
+ .angle_wrist(0.0)
.max_angular_velocity_intake(20)
.max_angular_acceleration_intake(20)
.max_angular_velocity_shoulder(20)
@@ -349,7 +360,7 @@
.max_angular_acceleration_wrist(20)
.Send());
- RunForTime(Time::InSeconds(5));
+ RunForTime(Time::InSeconds(10));
VerifyNearGoal();
}
@@ -375,7 +386,7 @@
.angle_wrist(constants::Values::kWristRange.lower)
.Send());
// We have to wait for it to put the elevator in a safe position as well.
- RunForTime(Time::InSeconds(5));
+ RunForTime(Time::InSeconds(10));
VerifyNearGoal();
}
@@ -394,7 +405,7 @@
.angle_wrist(constants::Values::kWristRange.lower)
.Send());
// We have to wait for it to put the elevator in a safe position as well.
- RunForTime(Time::InSeconds(5));
+ RunForTime(Time::InSeconds(10));
VerifyNearGoal();
}
@@ -413,14 +424,14 @@
.angle_shoulder(constants::Values::kShoulderRange.lower + 0.3)
.angle_wrist(constants::Values::kWristRange.lower + 0.3)
.Send());
- RunForTime(Time::InSeconds(5));
+ RunForTime(Time::InSeconds(10));
EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
VerifyNearGoal();
SimulateSensorReset();
RunForTime(Time::InMS(100));
EXPECT_NE(Superstructure::RUNNING, superstructure_.state());
- RunForTime(Time::InMS(6000));
+ RunForTime(Time::InMS(10000));
EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
VerifyNearGoal();
}
@@ -446,6 +457,14 @@
EXPECT_NE(0.0, superstructure_.arm_.goal(2, 0));
}
+// Tests that MoveButKeepBelow returns sane values.
+TEST_F(SuperstructureTest, MoveButKeepBelowTest) {
+ EXPECT_EQ(1.0, Superstructure::MoveButKeepBelow(1.0, 10.0, 1.0));
+ EXPECT_EQ(1.0, Superstructure::MoveButKeepBelow(1.0, 2.0, 1.0));
+ EXPECT_EQ(0.0, Superstructure::MoveButKeepBelow(1.0, 1.0, 1.0));
+ EXPECT_EQ(1.0, Superstructure::MoveButKeepBelow(1.0, 0.0, 1.0));
+}
+
// Tests that the integrators works.
TEST_F(SuperstructureTest, IntegratorTest) {
superstructure_plant_.InitializeIntakePosition(
@@ -466,6 +485,35 @@
VerifyNearGoal();
}
+// Tests that zeroing while disabled works. Starts the superstructure near a
+// pulse, lets it initialize, moves it past the pulse, enables, and then make
+// sure it goes to the right spot.
+TEST_F(SuperstructureTest, DisabledZeroTest) {
+ superstructure_plant_.InitializeIntakePosition(-0.001);
+ superstructure_plant_.InitializeShoulderPosition(
+ constants::Values::kShoulderEncoderIndexDifference * 10 - 0.001);
+ superstructure_plant_.InitializeWristPosition(-0.001);
+
+ superstructure_queue_.goal.MakeWithBuilder()
+ .angle_intake(0.0)
+ .angle_shoulder(constants::Values::kShoulderEncoderIndexDifference * 10)
+ .angle_wrist(0.0)
+ .Send();
+
+ // Run disabled for 2 seconds
+ RunForTime(Time::InSeconds(2), false);
+ EXPECT_EQ(Superstructure::DISABLED_INITIALIZED, superstructure_.state());
+
+ superstructure_plant_.set_power_error(1.0, 1.0, 1.0);
+
+ RunForTime(Time::InSeconds(1), false);
+
+ EXPECT_EQ(Superstructure::SLOW_RUNNING, superstructure_.state());
+ RunForTime(Time::InSeconds(1), true);
+
+ VerifyNearGoal();
+}
+
} // namespace testing
} // namespace superstructure
} // namespace control_loops