diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index a2b7a9e..d4b5578 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -110,6 +110,34 @@
   intake_->set_unprofiled_goal(intake_angle_goal);
 }
 
+bool CollisionAvoidance::collided() const {
+  return collided_with_given_angles(arm_->shoulder_angle(), arm_->wrist_angle(),
+                                    intake_->angle());
+}
+
+bool CollisionAvoidance::collided_with_given_angles(double shoulder_angle,
+                                                    double wrist_angle,
+                                                    double intake_angle) {
+  // The arm and the intake must not hit.
+  if (shoulder_angle >=
+          CollisionAvoidance::kMaxShoulderAngleUntilSafeIntakeStowing &&
+      shoulder_angle <=
+          CollisionAvoidance::kMinShoulderAngleForIntakeInterference &&
+      intake_angle > CollisionAvoidance::kMaxIntakeAngleBeforeArmInterference) {
+    return true;
+  }
+
+  // The wrist must go back to zero when the shoulder is moving the arm into
+  // a stowed/intaking position.
+  if (shoulder_angle <
+          CollisionAvoidance::kMinShoulderAngleForHorizontalShooter &&
+      ::std::abs(wrist_angle) > kMaxWristAngleForSafeArmStowing) {
+    return true;
+  }
+
+  return false;
+}
+
 constexpr double CollisionAvoidance::kMinShoulderAngleForHorizontalShooter;
 constexpr double CollisionAvoidance::kMinShoulderAngleForIntakeInterference;
 constexpr double CollisionAvoidance::kMaxIntakeAngleBeforeArmInterference;
diff --git a/y2016/control_loops/superstructure/superstructure.h b/y2016/control_loops/superstructure/superstructure.h
index 346a33b..25b7939 100644
--- a/y2016/control_loops/superstructure/superstructure.h
+++ b/y2016/control_loops/superstructure/superstructure.h
@@ -48,6 +48,17 @@
   void UpdateGoal(double shoulder_angle_goal, double wrist_angle_goal,
                   double intake_angle_goal);
 
+  // Returns true if any of the limbs and frame are somehow currently
+  // interfering with one another. This is based purely on the angles that the
+  // limbs are reporting.
+  bool collided() const;
+
+  // Detects collision with the specified angles. This is especially useful for
+  // unit testing where we have proper ground truth for all the angles.
+  static bool collided_with_given_angles(double shoulder_angle,
+                                         double wrist_angle,
+                                         double intake_angle);
+
   // TODO(phil): Verify that these numbers actually make sense. Someone needs
   // to verify these either on the CAD or the real robot.
 
@@ -162,7 +173,10 @@
                                  double move_distance);
 
   // Returns true if collision avoidance is turned on. False if not.
-  bool collision_avoidance_enabled() { return collision_avoidance_enabled_; }
+  bool collision_avoidance_enabled() const { return collision_avoidance_enabled_; }
+
+  // Returns true if anything is currently considered "collided".
+  bool collided() const { return collision_avoidance_.collided(); }
 
  protected:
   virtual void RunIteration(
diff --git a/y2016/control_loops/superstructure/superstructure_lib_test.cc b/y2016/control_loops/superstructure/superstructure_lib_test.cc
index 0ab4eb7..ea666de 100644
--- a/y2016/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2016/control_loops/superstructure/superstructure_lib_test.cc
@@ -242,31 +242,6 @@
     TickTime();
   }
 
-  void VerifyAbsenceOfCollisions() {
-    superstructure_queue_.status.FetchLatest();
-    ASSERT_TRUE(superstructure_queue_.status.get() != nullptr);
-
-    double intake_angle = superstructure_queue_.status->intake.angle;
-    double shoulder_angle = superstructure_queue_.status->shoulder.angle;
-    double wrist_angle = superstructure_queue_.status->wrist.angle;
-
-    // The arm and the intake must not hit.
-    if (shoulder_angle >=
-            CollisionAvoidance::kMaxShoulderAngleUntilSafeIntakeStowing &&
-        shoulder_angle <=
-            CollisionAvoidance::kMinShoulderAngleForIntakeInterference) {
-      EXPECT_LT(intake_angle,
-                CollisionAvoidance::kMaxIntakeAngleBeforeArmInterference);
-    }
-
-    // The wrist must go back to zero when the shoulder is moving the arm into
-    // a stowed/intaking position.
-    if (shoulder_angle <
-        CollisionAvoidance::kMaxShoulderAngleUntilSafeIntakeStowing) {
-      EXPECT_NEAR(0.0, wrist_angle, 0.01);
-    }
-  }
-
   // Runs iterations until the specified amount of simulated time has elapsed.
   void RunForTime(const Time &run_for, bool enabled = true) {
     const auto start_time = Time::Now();
@@ -313,11 +288,22 @@
                 superstructure_plant_.wrist_angular_velocity());
 
       if (check_for_collisions_) {
-        VerifyAbsenceOfCollisions();
+        ASSERT_TRUE(superstructure_.collision_avoidance_enabled());
+        ASSERT_FALSE(collided());
       }
     }
   }
 
+  // Helper function to quickly check if either the estimation detected a
+  // collision or if there's a collision using ground-truth plant values.
+  bool collided() const {
+    return superstructure_.collided() ||
+           CollisionAvoidance::collided_with_given_angles(
+               superstructure_plant_.shoulder_angle(),
+               superstructure_plant_.wrist_angle(),
+               superstructure_plant_.intake_angle());
+  }
+
   // Runs iterations while watching the average acceleration per cycle and
   // making sure it doesn't exceed the provided bounds.
   void set_peak_intake_acceleration(double value) {
@@ -534,12 +520,11 @@
   superstructure_plant_.InitializeRelativeWristPosition(
       constants::Values::kWristRange.upper);
 
-  ASSERT_TRUE(
-      superstructure_queue_.goal.MakeWithBuilder()
-          .angle_intake(constants::Values::kIntakeRange.lower + 0.3)
-          .angle_shoulder(constants::Values::kShoulderRange.lower + 0.3)
-          .angle_wrist(constants::Values::kWristRange.lower + 0.3)
-          .Send());
+  ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
+                  .angle_intake(constants::Values::kIntakeRange.lower + 0.3)
+                  .angle_shoulder(constants::Values::kShoulderRange.lower + 0.3)
+                  .angle_wrist(constants::Values::kWristRange.lower + 0.3)
+                  .Send());
   RunForTime(Time::InSeconds(15));
 
   EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
@@ -955,6 +940,26 @@
   EXPECT_NEAR(0.0, superstructure_queue_.status->wrist.angle, 0.001);
 }
 
+// Make sure that we can properly detect a collision.
+TEST_F(SuperstructureTest, DetectAndFixCollisionBetweenArmAndIntake) {
+  superstructure_plant_.InitializeIntakePosition(
+      constants::Values::kIntakeRange.upper);                     // upper limit
+  superstructure_plant_.InitializeShoulderPosition(M_PI * 0.25);  // 45° up
+  superstructure_plant_.InitializeAbsoluteWristPosition(0);       // level
+
+  // Since we're explicitly checking for collisions, we don't want to fail the
+  // test because of collisions.
+  check_for_collisions_ = false;
+
+  RunForTime(Time::InSeconds(1));
+  ASSERT_TRUE(collided());
+
+  // Make sure that the collision avoidance will properly move the limbs out of
+  // the collision area.
+  RunForTime(Time::InSeconds(10));
+  ASSERT_FALSE(collided());
+}
+
 }  // namespace testing
 }  // namespace superstructure
 }  // namespace control_loops