Let the superstructure out of LANDING_RUNNING
Change-Id: Ia4793c3c4fd79d4ed66b04538d85bd4db3296353
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index 43a95d9..7c370bd 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -481,6 +481,16 @@
constants::Values::kShoulderRange.lower);
requested_wrist = 0.0;
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 ||
+ arm_.unprofiled_goal(0, 0) > kShoulderTransitionToLanded + 1e-4) {
+ if (state_ == LANDING_RUNNING) {
+ state_ = RUNNING;
+ } else {
+ state_ = SLOW_RUNNING;
+ }
+ }
} else {
// Otherwise, give the user what he asked for.
arm_.AdjustProfile(unsafe_goal->max_angular_velocity_shoulder,
@@ -498,7 +508,9 @@
// Transition to landing once the profile is close to finished for the
// shoulder.
- if (arm_.goal(0, 0) <= kShoulderTransitionToLanded + 1e-4) {
+ if (arm_.goal(0, 0) <= kShoulderTransitionToLanded + 1e-4 &&
+ arm_.unprofiled_goal(0, 0) <=
+ kShoulderTransitionToLanded + 1e-4) {
if (state_ == RUNNING) {
state_ = LANDING_RUNNING;
} else {
diff --git a/y2016/control_loops/superstructure/superstructure_lib_test.cc b/y2016/control_loops/superstructure/superstructure_lib_test.cc
index 1ff958d..ab8ddf4 100644
--- a/y2016/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2016/control_loops/superstructure/superstructure_lib_test.cc
@@ -905,12 +905,21 @@
ASSERT_TRUE(
superstructure_queue_.goal.MakeWithBuilder()
+ .angle_intake(constants::Values::kIntakeRange.upper) // stowed
+ .angle_shoulder(constants::Values::kShoulderRange.lower) // Down
+ .angle_wrist(0.0) // Stowed
+ .Send());
+
+ RunForTime(Time::InSeconds(10));
+
+ ASSERT_TRUE(
+ superstructure_queue_.goal.MakeWithBuilder()
.angle_intake(constants::Values::kIntakeRange.upper) // stowed
.angle_shoulder(M_PI / 4.0) // in the collision area
.angle_wrist(M_PI / 2.0) // down
.Send());
- RunForTime(Time::InSeconds(10));
+ RunForTime(Time::InSeconds(3));
superstructure_queue_.status.FetchLatest();
ASSERT_TRUE(superstructure_queue_.status.get() != nullptr);