diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index c7127cc..a2b7a9e 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -24,10 +24,103 @@
 constexpr double kZeroingVoltage = 4.0;
 }  // namespace
 
+// ///// CollisionAvoidance /////
+
+void CollisionAvoidance::UpdateGoal(double shoulder_angle_goal,
+                                    double wrist_angle_goal,
+                                    double intake_angle_goal) {
+  double shoulder_angle = arm_->shoulder_angle();
+  double wrist_angle = arm_->wrist_angle();
+  double intake_angle = intake_->angle();
+
+  // TODO(phil): This may need tuning to account for bounciness in the limbs or
+  // some other thing that I haven't thought of. At the very least,
+  // incorporating a small safety margin makes writing test cases much easier
+  // since you can directly compare statuses against the constants in the
+  // CollisionAvoidance class.
+  constexpr double kSafetyMargin = 0.01;  // radians
+
+  // Avoid colliding the shooter with the frame.
+  // If the shoulder is below a certain angle or we want to move it below
+  // that angle, then the shooter has to stay level to the ground. Otherwise,
+  // it will crash into the frame.
+  if (shoulder_angle < kMinShoulderAngleForHorizontalShooter ||
+      shoulder_angle_goal < kMinShoulderAngleForHorizontalShooter) {
+    wrist_angle_goal = 0.0;
+
+    // Make sure that we don't move the shoulder below a certain angle until
+    // the wrist is level with the ground.
+    if (::std::abs(wrist_angle) > kMaxWristAngleForSafeArmStowing) {
+      shoulder_angle_goal =
+          ::std::max(shoulder_angle_goal,
+                     kMinShoulderAngleForHorizontalShooter + kSafetyMargin);
+    }
+  }
+
+  // Is the arm where it could interfere with the intake right now?
+  bool shoulder_is_in_danger =
+      (shoulder_angle < kMinShoulderAngleForIntakeInterference &&
+       shoulder_angle > kMaxShoulderAngleUntilSafeIntakeStowing);
+
+  // Is the arm moving into collision zone from above?
+  bool shoulder_moving_into_danger_from_above =
+      (shoulder_angle >= kMinShoulderAngleForIntakeInterference &&
+       shoulder_angle_goal <= kMinShoulderAngleForIntakeInterference);
+
+  // Is the arm moving into collision zone from below?
+  bool shoulder_moving_into_danger_from_below =
+      (shoulder_angle <= kMaxShoulderAngleUntilSafeIntakeStowing &&
+       shoulder_angle_goal >= kMaxShoulderAngleUntilSafeIntakeStowing);
+
+  // Avoid colliding the arm with the intake.
+  if (shoulder_is_in_danger || shoulder_moving_into_danger_from_above ||
+      shoulder_moving_into_danger_from_below) {
+    // If the arm could collide with the intake, we make sure to move the
+    // intake out of the way. The arm has priority.
+    intake_angle_goal =
+        ::std::min(intake_angle_goal,
+                   kMaxIntakeAngleBeforeArmInterference - kSafetyMargin);
+
+    // Don't let the shoulder move into the collision area until the intake is
+    // out of the way.
+    if (intake_angle > kMaxIntakeAngleBeforeArmInterference) {
+      const double kHalfwayPointBetweenSafeZones =
+          (kMinShoulderAngleForIntakeInterference +
+           kMaxShoulderAngleUntilSafeIntakeStowing) /
+          2.0;
+
+      if (shoulder_angle >= kHalfwayPointBetweenSafeZones) {
+        // The shoulder is closer to being above the collision area. Move it up
+        // there.
+        shoulder_angle_goal =
+            ::std::max(shoulder_angle_goal,
+                       kMinShoulderAngleForIntakeInterference + kSafetyMargin);
+      } else {
+        // The shoulder is closer to being below the collision zone (i.e. in
+        // stowing/intake position), keep it there for now.
+        shoulder_angle_goal =
+            ::std::min(shoulder_angle_goal,
+                       kMaxShoulderAngleUntilSafeIntakeStowing - kSafetyMargin);
+      }
+    }
+  }
+
+  // Send the possibly adjusted goals to the components.
+  arm_->set_unprofiled_goal(shoulder_angle_goal, wrist_angle_goal);
+  intake_->set_unprofiled_goal(intake_angle_goal);
+}
+
+constexpr double CollisionAvoidance::kMinShoulderAngleForHorizontalShooter;
+constexpr double CollisionAvoidance::kMinShoulderAngleForIntakeInterference;
+constexpr double CollisionAvoidance::kMaxIntakeAngleBeforeArmInterference;
+constexpr double CollisionAvoidance::kMaxWristAngleForSafeArmStowing;
+constexpr double CollisionAvoidance::kMaxShoulderAngleUntilSafeIntakeStowing;
+
 Superstructure::Superstructure(
     control_loops::SuperstructureQueue *superstructure_queue)
     : aos::controls::ControlLoop<control_loops::SuperstructureQueue>(
-          superstructure_queue) {}
+          superstructure_queue),
+      collision_avoidance_(&intake_, &arm_) {}
 
 bool Superstructure::IsArmNear(double shoulder_tolerance,
                                double wrist_tolerance) {
@@ -81,16 +174,6 @@
   }
 }
 
-constexpr double Superstructure::kShoulderMiddleAngle;
-constexpr double Superstructure::kLooseTolerance;
-constexpr double Superstructure::kIntakeUpperClear;
-constexpr double Superstructure::kIntakeLowerClear;
-constexpr double Superstructure::kShoulderUpAngle;
-constexpr double Superstructure::kShoulderLanded;
-constexpr double Superstructure::kTightTolerance;
-constexpr double Superstructure::kWristAlmostLevel;
-constexpr double Superstructure::kShoulderWristClearAngle;
-
 void Superstructure::RunIteration(
     const control_loops::SuperstructureQueue::Goal *unsafe_goal,
     const control_loops::SuperstructureQueue::Position *position,
@@ -330,9 +413,15 @@
         intake_.AdjustProfile(unsafe_goal->max_angular_velocity_intake,
                               unsafe_goal->max_angular_acceleration_intake);
 
-        arm_.set_unprofiled_goal(unsafe_goal->angle_shoulder,
-                                 unsafe_goal->angle_wrist);
-        intake_.set_unprofiled_goal(unsafe_goal->angle_intake);
+        if (collision_avoidance_enabled()) {
+          collision_avoidance_.UpdateGoal(unsafe_goal->angle_shoulder,
+                                          unsafe_goal->angle_wrist,
+                                          unsafe_goal->angle_intake);
+        } else {
+          arm_.set_unprofiled_goal(unsafe_goal->angle_shoulder,
+                                   unsafe_goal->angle_wrist);
+          intake_.set_unprofiled_goal(unsafe_goal->angle_intake);
+        }
       }
 
       // ESTOP if we hit any of the limits.  It is safe(ish) to hit the limits
@@ -404,6 +493,16 @@
   last_state_ = state_before_switch;
 }
 
+constexpr double Superstructure::kShoulderMiddleAngle;
+constexpr double Superstructure::kLooseTolerance;
+constexpr double Superstructure::kIntakeUpperClear;
+constexpr double Superstructure::kIntakeLowerClear;
+constexpr double Superstructure::kShoulderUpAngle;
+constexpr double Superstructure::kShoulderLanded;
+constexpr double Superstructure::kTightTolerance;
+constexpr double Superstructure::kWristAlmostLevel;
+constexpr double Superstructure::kShoulderWristClearAngle;
+
 }  // namespace superstructure
 }  // namespace control_loops
 }  // namespace y2016
diff --git a/y2016/control_loops/superstructure/superstructure.h b/y2016/control_loops/superstructure/superstructure.h
index a860f09..346a33b 100644
--- a/y2016/control_loops/superstructure/superstructure.h
+++ b/y2016/control_loops/superstructure/superstructure.h
@@ -15,11 +15,72 @@
 namespace control_loops {
 namespace superstructure {
 namespace testing {
+class SuperstructureTest_RespectsRange_Test;
 class SuperstructureTest_DisabledGoalTest_Test;
 class SuperstructureTest_ArmZeroingErrorTest_Test;
 class SuperstructureTest_IntakeZeroingErrorTest_Test;
+class SuperstructureTest_UpperHardstopStartup_Test;
 }  // namespace testing
 
+// Helper class to prevent parts from crashing into each other. The parts in
+// question here are: the frame, the arm (plus shooter), and the intake.
+// Assumptions:
+// - The shoulder, the wrist, and intake are horizontal when at angle 0.
+// - The arm (i.e. shoulder) and shooter (i.e. wrist) are stored when they are
+//   both at zero degrees.
+// - The intake at angle 0 is in a position to help get a ball in the robot.
+// - The intake at angle PI is in a "stowed" position. In other words, it is
+//   folded over the arm and shooter when they are also in a stowed position.
+// - The shooter must remain horizontal when the arm is folding into the robot.
+//   Otherwise, the shooter will collide with the frame.
+// - The arm has priority over the intake. If the arm wants to move in such a
+//   way that interferes with the intake's movement then the intake must move
+//   out of the way.
+class CollisionAvoidance {
+ public:
+  // Set up collision avoidance for an arm and intake.
+  CollisionAvoidance(Intake *intake, Arm *arm) : intake_(intake), arm_(arm) {}
+
+  // This function accepts goals for the intake and the arm and modifies them
+  // in such a way that collisions between all the different parts of the robot
+  // are avoided. The modified goals are then sent to the arm and intake as
+  // unprofiled goals.
+  void UpdateGoal(double shoulder_angle_goal, double wrist_angle_goal,
+                  double intake_angle_goal);
+
+  // TODO(phil): Verify that these numbers actually make sense. Someone needs
+  // to verify these either on the CAD or the real robot.
+
+  // The shoulder angle (in radians) below which the shooter must be in a
+  // stowing position. In other words the wrist must be at angle zero if the
+  // shoulder is below this angle.
+  static constexpr double kMinShoulderAngleForHorizontalShooter = 0.1;
+
+  // The shoulder angle (in radians) below which the arm as a whole has the
+  // potential to interfere with the intake.
+  static constexpr double kMinShoulderAngleForIntakeInterference = M_PI / 3.0;
+
+  // The intake angle (in radians) above which the intake can interfere (i.e.
+  // collide) with the arm and/or shooter.
+  static constexpr double kMaxIntakeAngleBeforeArmInterference = M_PI / 2.0;
+
+  // The maximum absolute angle (in radians) that the wrist must be below in
+  // order for the shouler to be allowed to move below
+  // kMinShoulderAngleForHorizontalShooter.  In other words, only allow the arm
+  // to move down into the belly pan if the shooter is horizontal, ready to
+  // also be placed into the belly pan.
+  static constexpr double kMaxWristAngleForSafeArmStowing = 0.01;
+
+  // The shoulder angle (in radians) below which the intake can safely move
+  // into the collision zone. This is necessary when the robot wants to fold up
+  // completely (i.e. stow the arm, shooter, and intake).
+  static constexpr double kMaxShoulderAngleUntilSafeIntakeStowing = 0.01;
+
+ private:
+  Intake *intake_;
+  Arm *arm_;
+};
+
 class Superstructure
     : public ::aos::controls::ControlLoop<control_loops::SuperstructureQueue> {
  public:
@@ -100,6 +161,9 @@
   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() { return collision_avoidance_enabled_; }
+
  protected:
   virtual void RunIteration(
       const control_loops::SuperstructureQueue::Goal *unsafe_goal,
@@ -111,9 +175,16 @@
   friend class testing::SuperstructureTest_DisabledGoalTest_Test;
   friend class testing::SuperstructureTest_ArmZeroingErrorTest_Test;
   friend class testing::SuperstructureTest_IntakeZeroingErrorTest_Test;
+  friend class testing::SuperstructureTest_RespectsRange_Test;
+  friend class testing::SuperstructureTest_UpperHardstopStartup_Test;
   Intake intake_;
   Arm arm_;
 
+  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 14dc0e1..0ab4eb7 100644
--- a/y2016/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2016/control_loops/superstructure/superstructure_lib_test.cc
@@ -117,6 +117,11 @@
     pot_encoder_wrist_.Initialize(start_pos, kNoiseScalar);
   }
 
+  // Must be called after any changes to InitializeShoulderPosition.
+  void InitializeAbsoluteWristPosition(double start_pos) {
+    InitializeRelativeWristPosition(start_pos - arm_plant_->X(0, 0));
+  }
+
   // Sends a queue message with the position.
   void SendPositionMessage() {
     ::aos::ScopedMessagePtr<control_loops::SuperstructureQueue::Position>
@@ -237,6 +242,31 @@
     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();
@@ -281,6 +311,10 @@
                 superstructure_plant_.wrist_angular_velocity());
       EXPECT_LE(-peak_wrist_velocity_,
                 superstructure_plant_.wrist_angular_velocity());
+
+      if (check_for_collisions_) {
+        VerifyAbsenceOfCollisions();
+      }
     }
   }
 
@@ -301,6 +335,8 @@
   }
   void set_peak_wrist_velocity(double value) { peak_wrist_velocity_ = value; }
 
+  bool check_for_collisions_ = true;
+
   // Create a new instance of the test queue so that it invalidates the queue
   // that it points to.  Otherwise, we will have a pointed to
   // shared memory that is no longer valid.
@@ -364,6 +400,10 @@
 // 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)
@@ -376,7 +416,6 @@
                   .max_angular_velocity_wrist(20)
                   .max_angular_acceleration_wrist(20)
                   .Send());
-
   RunForTime(Time::InSeconds(10));
 
   // Check that we are near our soft limit.
@@ -443,6 +482,9 @@
 
 // Tests that starting at the lower hardstops doesn't cause an abort.
 TEST_F(SuperstructureTest, LowerHardstopStartup) {
+  // Don't check for collisions for this test.
+  check_for_collisions_ = false;
+
   superstructure_plant_.InitializeIntakePosition(
       constants::Values::kIntakeRange.lower);
   superstructure_plant_.InitializeShoulderPosition(
@@ -462,6 +504,10 @@
 
 // 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(
@@ -487,6 +533,7 @@
       constants::Values::kShoulderRange.upper);
   superstructure_plant_.InitializeRelativeWristPosition(
       constants::Values::kWristRange.upper);
+
   ASSERT_TRUE(
       superstructure_queue_.goal.MakeWithBuilder()
           .angle_intake(constants::Values::kIntakeRange.lower + 0.3)
@@ -507,6 +554,9 @@
 
 // Tests that the internal goals don't change while disabled.
 TEST_F(SuperstructureTest, DisabledGoalTest) {
+  // Don't check for collisions for this test.
+  check_for_collisions_ = false;
+
   ASSERT_TRUE(
       superstructure_queue_.goal.MakeWithBuilder()
           .angle_intake(constants::Values::kIntakeRange.lower + 0.03)
@@ -536,6 +586,9 @@
 
 // Tests that the integrators works.
 TEST_F(SuperstructureTest, IntegratorTest) {
+  // Don't check for collisions for this test.
+  check_for_collisions_ = false;
+
   superstructure_plant_.InitializeIntakePosition(
       constants::Values::kIntakeRange.lower);
   superstructure_plant_.InitializeShoulderPosition(
@@ -843,6 +896,65 @@
   VerifyNearGoal();
 }
 
+// Make sure that the intake moves out of the way when the arm wants to move
+// into a shooting position.
+TEST_F(SuperstructureTest, AvoidCollisionWhenMovingArmFromStart) {
+  superstructure_plant_.InitializeIntakePosition(
+      constants::Values::kIntakeRange.upper);
+  superstructure_plant_.InitializeShoulderPosition(0.0);
+  superstructure_plant_.InitializeAbsoluteWristPosition(0.0);
+
+  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));
+
+  superstructure_queue_.status.FetchLatest();
+  ASSERT_TRUE(superstructure_queue_.status.get() != nullptr);
+
+  // The intake should be out of the way despite being told to move to stowing.
+  EXPECT_LT(superstructure_queue_.status->intake.angle, M_PI);
+  EXPECT_LT(superstructure_queue_.status->intake.angle,
+            constants::Values::kIntakeRange.upper);
+  EXPECT_LT(superstructure_queue_.status->intake.angle,
+            CollisionAvoidance::kMaxIntakeAngleBeforeArmInterference);
+
+  // The arm should have reached its goal.
+  EXPECT_NEAR(M_PI / 4.0, superstructure_queue_.status->shoulder.angle, 0.001);
+  EXPECT_NEAR(M_PI / 2.0, superstructure_queue_.status->wrist.angle, 0.001);
+}
+
+// Make sure that the shooter holds itself level when the arm comes down
+// into a stowing/intaking position.
+TEST_F(SuperstructureTest, AvoidCollisionWhenStowingArm) {
+  superstructure_plant_.InitializeIntakePosition(0.0);           // forward
+  superstructure_plant_.InitializeShoulderPosition(M_PI / 2.0);  // up
+  superstructure_plant_.InitializeAbsoluteWristPosition(M_PI);   // forward
+
+  ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
+                  .angle_intake(0.0)
+                  .angle_shoulder(0.0)
+                  .angle_wrist(M_PI)  // intentionally asking for forward
+                  .Send());
+
+  RunForTime(Time::InSeconds(10));
+
+  superstructure_queue_.status.FetchLatest();
+  ASSERT_TRUE(superstructure_queue_.status.get() != nullptr);
+
+  // The intake should be in intaking position, as asked.
+  EXPECT_NEAR(0.0, superstructure_queue_.status->intake.angle, 0.001);
+
+  // The shoulder and wrist should both be at zero degrees (i.e.
+  // stowed/intaking position).
+  EXPECT_NEAR(0.0, superstructure_queue_.status->shoulder.angle, 0.001);
+  EXPECT_NEAR(0.0, superstructure_queue_.status->wrist.angle, 0.001);
+}
+
 }  // namespace testing
 }  // namespace superstructure
 }  // namespace control_loops