Tests for disabling while zeroing
Change-Id: I43c7d99c5783b818d2309df0a221e9afe1dc5e03
diff --git a/y2016/control_loops/superstructure/superstructure.h b/y2016/control_loops/superstructure/superstructure.h
index 823e130..0b72c77 100644
--- a/y2016/control_loops/superstructure/superstructure.h
+++ b/y2016/control_loops/superstructure/superstructure.h
@@ -20,6 +20,8 @@
class SuperstructureTest_ArmZeroingErrorTest_Test;
class SuperstructureTest_IntakeZeroingErrorTest_Test;
class SuperstructureTest_UpperHardstopStartup_Test;
+class SuperstructureTest_DisabledWhileZeroingHigh_Test;
+class SuperstructureTest_DisabledWhileZeroingLow_Test;
} // namespace testing
// Helper class to prevent parts from crashing into each other. The parts in
@@ -202,6 +204,8 @@
friend class testing::SuperstructureTest_IntakeZeroingErrorTest_Test;
friend class testing::SuperstructureTest_RespectsRange_Test;
friend class testing::SuperstructureTest_UpperHardstopStartup_Test;
+ friend class testing::SuperstructureTest_DisabledWhileZeroingHigh_Test;
+ friend class testing::SuperstructureTest_DisabledWhileZeroingLow_Test;
Intake intake_;
Arm arm_;
diff --git a/y2016/control_loops/superstructure/superstructure_lib_test.cc b/y2016/control_loops/superstructure/superstructure_lib_test.cc
index 2ffd295..bc33c9b 100644
--- a/y2016/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2016/control_loops/superstructure/superstructure_lib_test.cc
@@ -600,6 +600,135 @@
EXPECT_NE(0.0, superstructure_.arm_.goal(2, 0));
}
+// Tests that disabling while zeroing at any state restarts from beginning
+TEST_F(SuperstructureTest, DisabledWhileZeroingHigh) {
+ superstructure_plant_.InitializeIntakePosition(
+ constants::Values::kIntakeRange.upper);
+ superstructure_plant_.InitializeShoulderPosition(
+ constants::Values::kShoulderRange.upper);
+ superstructure_plant_.InitializeAbsoluteWristPosition(
+ constants::Values::kWristRange.upper);
+
+ ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
+ .angle_intake(constants::Values::kIntakeRange.upper)
+ .angle_shoulder(constants::Values::kShoulderRange.upper)
+ .angle_wrist(constants::Values::kWristRange.upper)
+ .max_angular_velocity_intake(20)
+ .max_angular_acceleration_intake(20)
+ .max_angular_velocity_shoulder(20)
+ .max_angular_acceleration_shoulder(20)
+ .max_angular_velocity_wrist(20)
+ .max_angular_acceleration_wrist(20)
+ .Send());
+
+ // Expected states to cycle through and check in order.
+ Superstructure::State ExpectedStateOrder[] = {
+ Superstructure::DISABLED_INITIALIZED,
+ Superstructure::HIGH_ARM_ZERO_LIFT_ARM,
+ Superstructure::HIGH_ARM_ZERO_LEVEL_SHOOTER,
+ Superstructure::HIGH_ARM_ZERO_MOVE_INTAKE_OUT,
+ Superstructure::HIGH_ARM_ZERO_LOWER_ARM,
+ };
+
+ // Cycle through until arm_ and intake_ are initialized in superstructure.cc
+ while (superstructure_.state() < Superstructure::DISABLED_INITIALIZED) {
+ RunIteration(true);
+ }
+
+ static const int kNumberOfStates =
+ sizeof(ExpectedStateOrder) / sizeof(ExpectedStateOrder[0]);
+
+ // Next state when reached to disable
+ for (int i = 0; i < kNumberOfStates; i++) {
+ // Next expected state after being disabled that is expected until next
+ // state to disable at is reached
+ for (int j = 0; superstructure_.state() != ExpectedStateOrder[i] && j <= i;
+ j++) {
+ // RunIteration until next expected state is reached with a maximum
+ // of 10000 times to ensure a breakout
+ for (int o = 0;
+ superstructure_.state() < ExpectedStateOrder[j] && o < 10000; o++) {
+ RunIteration(true);
+ }
+ EXPECT_EQ(ExpectedStateOrder[j], superstructure_.state());
+ }
+
+ EXPECT_EQ(ExpectedStateOrder[i], superstructure_.state());
+
+ // Disable
+ RunIteration(false);
+
+ EXPECT_EQ(Superstructure::DISABLED_INITIALIZED, superstructure_.state());
+ }
+
+ RunForTime(Time::InSeconds(10));
+ EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
+}
+
+// Tests that disabling while zeroing at any state restarts from beginning
+TEST_F(SuperstructureTest, DisabledWhileZeroingLow) {
+ superstructure_plant_.InitializeIntakePosition(
+ constants::Values::kIntakeRange.lower);
+ superstructure_plant_.InitializeShoulderPosition(
+ constants::Values::kShoulderRange.lower);
+ superstructure_plant_.InitializeAbsoluteWristPosition(0.0);
+
+ ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
+ .angle_intake(constants::Values::kIntakeRange.lower)
+ .angle_shoulder(constants::Values::kShoulderRange.lower)
+ .angle_wrist(constants::Values::kWristRange.lower)
+ .max_angular_velocity_intake(20)
+ .max_angular_acceleration_intake(20)
+ .max_angular_velocity_shoulder(20)
+ .max_angular_acceleration_shoulder(20)
+ .max_angular_velocity_wrist(20)
+ .max_angular_acceleration_wrist(20)
+ .Send());
+
+ // Expected states to cycle through and check in order.
+ Superstructure::State ExpectedStateOrder[] = {
+ Superstructure::DISABLED_INITIALIZED,
+ Superstructure::LOW_ARM_ZERO_LOWER_INTAKE,
+ Superstructure::LOW_ARM_ZERO_MAYBE_LEVEL_SHOOTER,
+ Superstructure::LOW_ARM_ZERO_LIFT_SHOULDER,
+ Superstructure::LOW_ARM_ZERO_LEVEL_SHOOTER,
+ };
+
+ // Cycle through until arm_ and intake_ are initialized in superstructure.cc
+ while (superstructure_.state() < Superstructure::DISABLED_INITIALIZED) {
+ RunIteration(true);
+ }
+
+ static const int kNumberOfStates =
+ sizeof(ExpectedStateOrder) / sizeof(ExpectedStateOrder[0]);
+
+ // Next state when reached to disable
+ for (int i = 0; i < kNumberOfStates; i++) {
+ // Next expected state after being disabled that is expected until next
+ // state to disable at is reached
+ for (int j = 0; superstructure_.state() != ExpectedStateOrder[i] && j <= i;
+ j++) {
+ // RunIteration until next expected state is reached with a maximum
+ // of 10000 times to ensure a breakout
+ for (int o = 0;
+ superstructure_.state() < ExpectedStateOrder[j] && o < 10000; o++) {
+ RunIteration(true);
+ }
+ EXPECT_EQ(ExpectedStateOrder[j], superstructure_.state());
+ }
+
+ EXPECT_EQ(ExpectedStateOrder[i], superstructure_.state());
+
+ // Disable
+ RunIteration(false);
+
+ EXPECT_EQ(Superstructure::DISABLED_INITIALIZED, superstructure_.state());
+ }
+
+ RunForTime(Time::InSeconds(10));
+ EXPECT_EQ(Superstructure::LANDING_RUNNING, superstructure_.state());
+}
+
// Tests that MoveButKeepBelow returns sane values.
TEST_F(SuperstructureTest, MoveButKeepBelowTest) {
EXPECT_EQ(1.0, Superstructure::MoveButKeepBelow(1.0, 10.0, 1.0));