Added a zeroing procedure.
Change-Id: Ib329f1a02772e36d737a0a1d0f5114b2b5acc029
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index 214fc6c..c4ef468 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,58 +110,221 @@
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:
+ // TODO(austin): Exit SLOW_RUNNING if we are not collided.
if (unsafe_goal) {
arm_.AdjustProfile(unsafe_goal->max_angular_velocity_shoulder,
- unsafe_goal->max_angular_acceleration_shoulder,
- unsafe_goal->max_angular_velocity_wrist,
- unsafe_goal->max_angular_acceleration_wrist);
+ unsafe_goal->max_angular_acceleration_shoulder,
+ unsafe_goal->max_angular_velocity_wrist,
+ unsafe_goal->max_angular_acceleration_wrist);
intake_.AdjustProfile(unsafe_goal->max_angular_velocity_wrist,
- unsafe_goal->max_angular_acceleration_intake);
+ unsafe_goal->max_angular_acceleration_intake);
arm_.set_unprofiled_goal(unsafe_goal->angle_shoulder,
- unsafe_goal->angle_wrist);
+ unsafe_goal->angle_wrist);
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 +334,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 +384,7 @@
status->state = state_;
- last_state_ = state_;
+ last_state_ = state_before_switch;
}
} // namespace superstructure