Tests that zeroing while disabled works.
- Fixes a bug where the profile didn't move when in RUNNING or SLOW_RUNNING
and disabled.
- Tested that we can zero when disabled.
Change-Id: I9bf67dd4019cc6cf487ef24ac97e213ec2fb70a7
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index c4ef468..cdbceb4 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -304,17 +304,30 @@
case SLOW_RUNNING:
case RUNNING:
- // TODO(austin): Exit SLOW_RUNNING if we are not collided.
+ 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,
- 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);
}
diff --git a/y2016/control_loops/superstructure/superstructure_lib_test.cc b/y2016/control_loops/superstructure/superstructure_lib_test.cc
index 2c988cc..6d8c18f 100644
--- a/y2016/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2016/control_loops/superstructure/superstructure_lib_test.cc
@@ -485,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