Removed ability to disable collision detection.
Change-Id: I0f9075f4b23a45382322c79663494e6a0a93c255
diff --git a/y2016/control_loops/superstructure/superstructure.h b/y2016/control_loops/superstructure/superstructure.h
index 25b7939..8d5048f 100644
--- a/y2016/control_loops/superstructure/superstructure.h
+++ b/y2016/control_loops/superstructure/superstructure.h
@@ -172,9 +172,6 @@
static double MoveButKeepAbove(double reference_angle, double current_angle,
double move_distance);
- // Returns true if collision avoidance is turned on. False if not.
- bool collision_avoidance_enabled() const { return collision_avoidance_enabled_; }
-
// Returns true if anything is currently considered "collided".
bool collided() const { return collision_avoidance_.collided(); }
@@ -196,9 +193,6 @@
CollisionAvoidance collision_avoidance_;
- // NOTE: Only touch this if you absolutely know what you're doing!
- bool collision_avoidance_enabled_ = true;
-
State state_ = UNINITIALIZED;
State last_state_ = UNINITIALIZED;
diff --git a/y2016/control_loops/superstructure/superstructure_lib_test.cc b/y2016/control_loops/superstructure/superstructure_lib_test.cc
index ea666de..4861cff 100644
--- a/y2016/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2016/control_loops/superstructure/superstructure_lib_test.cc
@@ -288,7 +288,6 @@
superstructure_plant_.wrist_angular_velocity());
if (check_for_collisions_) {
- ASSERT_TRUE(superstructure_.collision_avoidance_enabled());
ASSERT_FALSE(collided());
}
}
@@ -386,10 +385,6 @@
// Tests that the loop doesn't try and go beyond the physical range of the
// mechanisms.
TEST_F(SuperstructureTest, RespectsRange) {
- // Turn off collision avoidance for this test.
- superstructure_.collision_avoidance_enabled_ = false;
- check_for_collisions_ = false;
-
// Set some ridiculous goals to test upper limits.
ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
.angle_intake(M_PI * 10)
@@ -414,11 +409,36 @@
constants::Values::kShoulderRange.upper,
superstructure_queue_.status->wrist.angle, 0.001);
+ // Set some ridiculous goals to test limits.
+ ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
+ .angle_intake(M_PI * 10)
+ .angle_shoulder(M_PI * 10)
+ .angle_wrist(-M_PI * 10.0)
+ .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());
+
+ RunForTime(Time::InSeconds(10));
+
+ // Check that we are near our soft limit.
+ superstructure_queue_.status.FetchLatest();
+ EXPECT_NEAR(constants::Values::kIntakeRange.upper,
+ superstructure_queue_.status->intake.angle, 0.001);
+ EXPECT_NEAR(constants::Values::kShoulderRange.upper,
+ superstructure_queue_.status->shoulder.angle, 0.001);
+ EXPECT_NEAR(constants::Values::kWristRange.lower +
+ constants::Values::kShoulderRange.upper,
+ superstructure_queue_.status->wrist.angle, 0.001);
+
// Set some ridiculous goals to test lower limits.
ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
.angle_intake(-M_PI * 10)
.angle_shoulder(-M_PI * 10)
- .angle_wrist(-M_PI * 10)
+ .angle_wrist(0.0)
.max_angular_velocity_intake(20)
.max_angular_acceleration_intake(20)
.max_angular_velocity_shoulder(20)
@@ -435,9 +455,7 @@
superstructure_queue_.status->intake.angle, 0.001);
EXPECT_NEAR(constants::Values::kShoulderRange.lower,
superstructure_queue_.status->shoulder.angle, 0.001);
- EXPECT_NEAR(constants::Values::kWristRange.lower +
- constants::Values::kShoulderRange.lower,
- superstructure_queue_.status->wrist.angle, 0.001);
+ EXPECT_NEAR(0.0, superstructure_queue_.status->wrist.angle, 0.001);
}
// Tests that the loop zeroes when run for a while.
@@ -490,10 +508,6 @@
// Tests that starting at the upper hardstops doesn't cause an abort.
TEST_F(SuperstructureTest, UpperHardstopStartup) {
- // Turn off collision avoidance for this test.
- superstructure_.collision_avoidance_enabled_ = false;
- check_for_collisions_ = false;
-
superstructure_plant_.InitializeIntakePosition(
constants::Values::kIntakeRange.upper);
superstructure_plant_.InitializeShoulderPosition(
@@ -503,7 +517,7 @@
ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
.angle_intake(constants::Values::kIntakeRange.lower)
.angle_shoulder(constants::Values::kShoulderRange.lower)
- .angle_wrist(constants::Values::kWristRange.lower)
+ .angle_wrist(0.0)
.Send());
// We have to wait for it to put the elevator in a safe position as well.
RunForTime(Time::InSeconds(15));