Explicitly land the shooter in the belly-pan.
Change-Id: Ib73b9627f7650a16f8a3b6f86a16a1b6531404a1
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index 1c5dbec..0e97762 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -426,50 +426,94 @@
}
break;
+ // These 4 cases are very similar.
case SLOW_RUNNING:
case RUNNING:
+ case LANDING_SLOW_RUNNING:
+ case LANDING_RUNNING: {
if (disable) {
- // TODO(austin): Enter SLOW_RUNNING if we are collided.
+ // If we are disabled, go to slow running (or landing slow running) if
+ // we are collided.
+ if (collided()) {
+ if (state_ == RUNNING) {
+ state_ = SLOW_RUNNING;
+ } else if (state_ == LANDING_RUNNING) {
+ state_ = LANDING_SLOW_RUNNING;
+ }
+ }
- // If we are disabled, reset the profile to the current position.
+ // Reset the profile to the current position so it moves well from here.
intake_.ForceGoal(intake_.angle());
arm_.ForceGoal(arm_.shoulder_angle(), arm_.wrist_angle());
} else {
+ // If we are in slow_running and are no longer collided, let 'er rip.
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 (!collided()) {
+ state_ = RUNNING;
+ }
+ } else if (state_ == LANDING_SLOW_RUNNING) {
+ if (!collided()) {
+ state_ = LANDING_RUNNING;
+ }
}
}
+ double requested_shoulder = constants::Values::kShoulderRange.lower;
+ double requested_wrist = 0.0;
+ double requested_intake = M_PI / 2.0;
+
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);
- intake_.AdjustProfile(unsafe_goal->max_angular_velocity_intake,
- unsafe_goal->max_angular_acceleration_intake);
+ // If we are in one of the landing states, limit the accelerations and
+ // velocities to land cleanly.
+ if (state_ == LANDING_SLOW_RUNNING || state_ == LANDING_RUNNING) {
+ arm_.AdjustProfile(0.5, // Shoulder Velocity
+ 4.0, // Shoulder acceleration,
+ 4.0, // Wrist velocity
+ 10.0); // Wrist acceleration.
+ intake_.AdjustProfile(unsafe_goal->max_angular_velocity_intake,
+ unsafe_goal->max_angular_acceleration_intake);
- if (collision_avoidance_enabled()) {
- collision_avoidance_.UpdateGoal(unsafe_goal->angle_shoulder,
- unsafe_goal->angle_wrist,
- unsafe_goal->angle_intake);
+ requested_shoulder =
+ ::std::max(unsafe_goal->angle_shoulder,
+ constants::Values::kShoulderRange.lower);
+ requested_wrist = 0.0;
+ requested_intake = unsafe_goal->angle_intake;
} else {
- arm_.set_unprofiled_goal(unsafe_goal->angle_shoulder,
- unsafe_goal->angle_wrist);
- intake_.set_unprofiled_goal(unsafe_goal->angle_intake);
+ // Otherwise, give the user what he asked for.
+ 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);
+ intake_.AdjustProfile(unsafe_goal->max_angular_velocity_intake,
+ unsafe_goal->max_angular_acceleration_intake);
+
+ // Except, don't let the shoulder go all the way down.
+ requested_shoulder = ::std::max(unsafe_goal->angle_shoulder,
+ kShoulderTransitionToLanded);
+ requested_wrist = unsafe_goal->angle_wrist;
+ requested_intake = unsafe_goal->angle_intake;
+
+ // Transition to landing once the profile is close to finished for the
+ // shoulder.
+ if (arm_.goal(0, 0) <= kShoulderTransitionToLanded + 1e-4) {
+ if (state_ == RUNNING) {
+ state_ = LANDING_RUNNING;
+ } else {
+ state_ = LANDING_SLOW_RUNNING;
+ }
+ }
}
}
- // 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;
- }
+ // Push the request out to hardware!
+ collision_avoidance_.UpdateGoal(requested_shoulder, requested_wrist,
+ requested_intake);
+
+ // ESTOP if we hit the hard limits.
+ if ((arm_.CheckHardLimits() || intake_.CheckHardLimits()) && output) {
+ state_ = ESTOP;
}
- break;
+ } break;
case ESTOP:
LOG(ERROR, "Estop\n");
@@ -478,7 +522,8 @@
}
// Set the voltage limits.
- const double max_voltage = state_ == RUNNING ? 12.0 : kZeroingVoltage;
+ const double max_voltage =
+ (state_ == RUNNING || state_ == LANDING_RUNNING) ? 12.0 : kZeroingVoltage;
arm_.set_max_voltage(max_voltage, max_voltage);
intake_.set_max_voltage(max_voltage);
@@ -502,7 +547,8 @@
// Save debug/internal state.
// TODO(austin): Save the voltage errors.
- status->zeroed = state_ == RUNNING;
+ status->zeroed = (state_ == RUNNING || state_ == LANDING_RUNNING ||
+ state_ == SLOW_RUNNING || state_ == LANDING_SLOW_RUNNING);
status->shoulder.angle = arm_.X_hat(0, 0);
status->shoulder.angular_velocity = arm_.X_hat(1, 0);
@@ -546,6 +592,7 @@
constexpr double Superstructure::kTightTolerance;
constexpr double Superstructure::kWristAlmostLevel;
constexpr double Superstructure::kShoulderWristClearAngle;
+constexpr double Superstructure::kShoulderTransitionToLanded;
} // namespace superstructure
} // namespace control_loops
diff --git a/y2016/control_loops/superstructure/superstructure.h b/y2016/control_loops/superstructure/superstructure.h
index 8d5048f..23491bf 100644
--- a/y2016/control_loops/superstructure/superstructure.h
+++ b/y2016/control_loops/superstructure/superstructure.h
@@ -65,7 +65,7 @@
// The shoulder angle (in radians) below which the shooter must be in a
// stowing position. In other words the wrist must be at angle zero if the
// shoulder is below this angle.
- static constexpr double kMinShoulderAngleForHorizontalShooter = 0.1;
+ static constexpr double kMinShoulderAngleForHorizontalShooter = 0.6;
// The shoulder angle (in radians) below which the arm as a whole has the
// potential to interfere with the intake.
@@ -123,13 +123,18 @@
// bellypan.
static constexpr double kShoulderLanded = -0.02;
+ // This is the angle below which the shoulder will slowly profile down and
+ // land.
+ static constexpr double kShoulderTransitionToLanded = 0.10;
+
// 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;
+ static constexpr double kShoulderWristClearAngle =
+ CollisionAvoidance::kMinShoulderAngleForHorizontalShooter;
enum State {
// Wait for all the filters to be ready before starting the initialization
@@ -157,8 +162,12 @@
SLOW_RUNNING = 12,
// Run with full power.
RUNNING = 13,
+ // Run, but limit power to zeroing voltages while landing.
+ LANDING_SLOW_RUNNING = 14,
+ // Run with full power while landing.
+ LANDING_RUNNING = 15,
// Internal error caused the superstructure to abort.
- ESTOP = 14,
+ ESTOP = 16,
};
State state() const { return state_; }
diff --git a/y2016/control_loops/superstructure/superstructure_controls.cc b/y2016/control_loops/superstructure/superstructure_controls.cc
index 0798824..0c4ac70 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.cc
+++ b/y2016/control_loops/superstructure/superstructure_controls.cc
@@ -129,7 +129,8 @@
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);
+ constants::Values::kIntakeRange.lower_hard,
+ constants::Values::kIntakeRange.upper_hard);
return true;
}
@@ -320,10 +321,13 @@
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,
+ wrist_angle() - shoulder_angle(),
+ constants::Values::kWristRange.lower_hard,
constants::Values::kWristRange.upper_hard);
return true;
}
diff --git a/y2016/control_loops/superstructure/superstructure_lib_test.cc b/y2016/control_loops/superstructure/superstructure_lib_test.cc
index 4861cff..3affe2f 100644
--- a/y2016/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2016/control_loops/superstructure/superstructure_lib_test.cc
@@ -974,6 +974,8 @@
ASSERT_FALSE(collided());
}
+// TODO(austin): Landed to unlanded needs to go fast!
+
} // namespace testing
} // namespace superstructure
} // namespace control_loops