diff --git a/y2019/control_loops/superstructure/BUILD b/y2019/control_loops/superstructure/BUILD
index 5d07e17..453c680 100644
--- a/y2019/control_loops/superstructure/BUILD
+++ b/y2019/control_loops/superstructure/BUILD
@@ -39,3 +39,31 @@
         "//aos/events:shm-event-loop",
     ],
 )
+
+cc_library(
+    name = "collision_avoidance",
+    srcs = [
+        "collision_avoidance.cc",
+    ],
+    hdrs = [
+        "collision_avoidance.h",
+    ],
+    deps = [
+        ":superstructure_queue",
+        "//aos/controls:control_loop_queues",
+        "//frc971:constants",
+    ],
+)
+
+cc_test(
+    name = "collision_avoidance_tests",
+    srcs = [
+        "collision_avoidance_tests.cc",
+    ],
+    deps = [
+        ":collision_avoidance",
+        ":superstructure_queue",
+        "//aos:math",
+        "//aos/testing:googletest",
+    ],
+)
diff --git a/y2019/control_loops/superstructure/collision_avoidance.cc b/y2019/control_loops/superstructure/collision_avoidance.cc
new file mode 100644
index 0000000..7a26b90
--- /dev/null
+++ b/y2019/control_loops/superstructure/collision_avoidance.cc
@@ -0,0 +1,162 @@
+#include "y2019/control_loops/superstructure/collision_avoidance.h"
+
+#include <cmath>
+#include "y2019/control_loops/superstructure/superstructure.q.h"
+
+namespace y2019 {
+namespace control_loops {
+namespace superstructure {
+
+constexpr double CollisionAvoidance::kElevatorClearHeight;
+constexpr double CollisionAvoidance::kElevatorClearWristDownHeight;
+constexpr double CollisionAvoidance::kElevatorClearIntakeHeight;
+constexpr double CollisionAvoidance::kWristMaxAngle;
+constexpr double CollisionAvoidance::kWristMinAngle;
+constexpr double CollisionAvoidance::kIntakeOutAngle;
+constexpr double CollisionAvoidance::kIntakeInAngle;
+constexpr double CollisionAvoidance::kWristElevatorCollisionMinAngle;
+constexpr double CollisionAvoidance::kWristElevatorCollisionMaxAngle;
+constexpr double CollisionAvoidance::kEps;
+constexpr double CollisionAvoidance::kEpsIntake;
+constexpr double CollisionAvoidance::kEpsWrist;
+
+CollisionAvoidance::CollisionAvoidance() {
+  clear_min_wrist_goal();
+  clear_max_wrist_goal();
+  clear_min_elevator_goal();
+  clear_min_intake_goal();
+  clear_max_intake_goal();
+}
+
+bool CollisionAvoidance::IsCollided(const SuperstructureQueue::Status *status) {
+  const double wrist_position = status->wrist.position;
+  const double elevator_position = status->elevator.position;
+  const double intake_position = status->intake.position;
+
+  // Elevator is down, so the wrist can't be close to vertical.
+  if (elevator_position < kElevatorClearHeight) {
+    if (wrist_position < kWristElevatorCollisionMaxAngle &&
+        wrist_position > kWristElevatorCollisionMinAngle) {
+      return true;
+    }
+  }
+
+  // Elevator is down so wrist can't go below horizontal in either direction.
+  if (elevator_position < kElevatorClearWristDownHeight) {
+    if (wrist_position > kWristMaxAngle) {
+      return true;
+    }
+    if (wrist_position < kWristMinAngle) {
+      return true;
+    }
+  }
+
+  // Elevator is down so the intake has to be at either extreme.
+  if (elevator_position < kElevatorClearIntakeHeight) {
+    if (intake_position < kIntakeOutAngle && intake_position > kIntakeInAngle) {
+      return true;
+    }
+  }
+
+  // Nothing is hitting, we must be good.
+  return false;
+}
+
+void CollisionAvoidance::UpdateGoal(
+    const SuperstructureQueue::Status *status,
+    const SuperstructureQueue::Goal *unsafe_goal) {
+  const double wrist_position = status->wrist.position;
+  const double elevator_position = status->elevator.position;
+  const double intake_position = status->intake.position;
+
+  // Start with our constraints being wide open.
+  clear_max_wrist_goal();
+  clear_min_wrist_goal();
+  clear_max_intake_goal();
+  clear_min_intake_goal();
+
+  // If the elevator is low enough, we also can't transition the wrist.
+  if (elevator_position < kElevatorClearHeight) {
+    // Figure out which side the wrist is on and stay there.
+    if (wrist_position < 0.0) {
+      update_max_wrist_goal(kWristElevatorCollisionMinAngle - kEpsWrist);
+    } else {
+      update_min_wrist_goal(kWristElevatorCollisionMaxAngle + kEpsWrist);
+    }
+  }
+
+  // If the elevator is too low, the wrist needs to be above the clearance
+  // angles to avoid crashing the frame.
+  if (elevator_position < kElevatorClearWristDownHeight) {
+    update_min_wrist_goal(kWristMinAngle + kEpsWrist);
+    update_max_wrist_goal(kWristMaxAngle - kEpsWrist);
+  }
+
+  constexpr double kIntakeMiddleAngle =
+      (kIntakeOutAngle + kIntakeInAngle) / 2.0;
+
+  // If the elevator is too low, the intake can't transition from in to out or
+  // back.
+  if (elevator_position < kElevatorClearIntakeHeight) {
+    // Figure out if the intake is in our out and keep it there.
+    if (intake_position < kIntakeMiddleAngle) {
+      update_max_intake_goal(kIntakeInAngle - kEpsIntake);
+    } else {
+      update_min_intake_goal(kIntakeOutAngle + kEpsIntake);
+    }
+  }
+
+  // Start with an unconstrained elevator.
+  clear_min_elevator_goal();
+
+  // If the intake is within the collision range, don't let the elevator down.
+  if (intake_position > kIntakeInAngle && intake_position < kIntakeOutAngle) {
+    update_min_elevator_goal(kElevatorClearIntakeHeight + kEps);
+  }
+
+  // If the wrist is within the elevator collision range, don't let the elevator
+  // go down.
+  if (wrist_position > kWristElevatorCollisionMinAngle &&
+      wrist_position < kWristElevatorCollisionMaxAngle) {
+    update_min_elevator_goal(kElevatorClearHeight + kEps);
+  }
+
+  // If the wrist is far enough down that we are going to hit the frame, don't
+  // let the elevator go too far down.
+  if (wrist_position > kWristMaxAngle || wrist_position < kWristMinAngle) {
+    update_min_elevator_goal(kElevatorClearWristDownHeight + kEps);
+  }
+
+  if (unsafe_goal) {
+    const double wrist_goal = unsafe_goal->wrist.angle;
+    const double intake_goal = unsafe_goal->intake.joint_angle;
+
+    // Compute if we need to move the intake.
+    const bool intake_needs_to_move = (intake_position < kIntakeMiddleAngle) ^
+                                      (intake_goal < kIntakeMiddleAngle);
+
+    // Compute if we need to move the wrist across 0.
+    const bool wrist_needs_to_move =
+        (wrist_position < 0.0) ^ (wrist_goal < 0.0);
+
+    // If we need to move the intake, we've got to shove the elevator up.  The
+    // intake is already constrained so it can't hit anything until it's clear.
+    if (intake_needs_to_move && wrist_position > 0) {
+      update_min_elevator_goal(kElevatorClearIntakeHeight + kEps);
+    }
+    // If we need to move the wrist, we've got to shove the elevator up too. The
+    // wrist is already constrained so it can't hit anything until it's clear.
+    // If both the intake and wrist need to move, figure out which one will
+    // require the higher motion and move that.
+    if (wrist_needs_to_move) {
+      update_min_elevator_goal(kElevatorClearHeight + kEps);
+    }
+
+    // TODO(austin): We won't shove the elevator up if the wrist is asked to go
+    // down below horizontal.  I think that's fine.
+  }
+}
+
+}  // namespace superstructure
+}  // namespace control_loops
+}  // namespace y2019
diff --git a/y2019/control_loops/superstructure/collision_avoidance.h b/y2019/control_loops/superstructure/collision_avoidance.h
new file mode 100644
index 0000000..1ad9582
--- /dev/null
+++ b/y2019/control_loops/superstructure/collision_avoidance.h
@@ -0,0 +1,111 @@
+#ifndef Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_COLLISION_AVOIDANCE_H_
+#define Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_COLLISION_AVOIDANCE_H_
+
+#include <cmath>
+#include "aos/controls/control_loops.q.h"
+#include "frc971/constants.h"
+#include "y2019/control_loops/superstructure/superstructure.q.h"
+
+namespace y2019 {
+namespace control_loops {
+namespace superstructure {
+
+// CollisionAvoidance computes the min and max allowable ranges for various
+// subsystems to avoid collisions.  It also shoves the elevator up to let the
+// intake go in and out, and to let the wrist switch sides.
+class CollisionAvoidance {
+ public:
+  CollisionAvoidance();
+
+  // Reports if the superstructure is collided.
+  bool IsCollided(const SuperstructureQueue::Status *status);
+
+  // Checks and alters goals to make sure they're safe.
+  // TODO(austin): Either we will have a unit delay because this has to happen
+  // after the controls, or we need to be more clever about restructuring.
+  void UpdateGoal(const SuperstructureQueue::Status *status,
+                  const SuperstructureQueue::Goal *unsafe_goal);
+
+  // Returns the goals to give to the respective control loops in
+  // superstructure.
+  double min_wrist_goal() const { return min_wrist_goal_; }
+  double max_wrist_goal() const { return max_wrist_goal_; }
+  double min_elevator_goal() const { return min_elevator_goal_; }
+  double min_intake_goal() const { return min_intake_goal_; }
+  double max_intake_goal() const { return max_intake_goal_; }
+
+  void update_max_wrist_goal(double max_wrist_goal) {
+    max_wrist_goal_ = ::std::min(max_wrist_goal, max_wrist_goal_);
+  }
+  void update_min_wrist_goal(double min_wrist_goal) {
+    min_wrist_goal_ = ::std::max(min_wrist_goal, min_wrist_goal_);
+  }
+  void update_max_intake_goal(double max_intake_goal) {
+    max_intake_goal_ = ::std::min(max_intake_goal, max_intake_goal_);
+  }
+  void update_min_intake_goal(double min_intake_goal) {
+    min_intake_goal_ = ::std::max(min_intake_goal, min_intake_goal_);
+  }
+  void update_min_elevator_goal(double min_elevator_goal) {
+    min_elevator_goal_ = ::std::max(min_elevator_goal, min_elevator_goal_);
+  }
+
+  // TODO(sabina): set all the numbers to correctly match the robot.
+
+  // Height above which we can move the wrist freely.
+  static constexpr double kElevatorClearHeight = 0.5;
+
+  // Height above which we can move the wrist down.
+  static constexpr double kElevatorClearWristDownHeight = 0.3;
+  // Height the carriage needs to be above to move the intake.
+  static constexpr double kElevatorClearIntakeHeight = 0.4;
+
+  // Angle constraints for the wrist when below kElevatorClearDownHeight
+  static constexpr double kWristMaxAngle = M_PI / 2.0;
+  static constexpr double kWristMinAngle = -M_PI / 2.0;
+
+  // Angles outside of which the intake is fully clear of the wrist.
+  static constexpr double kIntakeOutAngle = M_PI / 6.0;
+  static constexpr double kIntakeInAngle = -M_PI / 3.0;
+
+  // Angles within which we will crash the wrist into the elevator if the
+  // elevator is below kElevatorClearHeight.
+  static constexpr double kWristElevatorCollisionMinAngle = -M_PI / 4.0;
+  static constexpr double kWristElevatorCollisionMaxAngle = M_PI / 4.0;
+
+  // Tolerance for the elevator.
+  static constexpr double kEps = 0.02;
+  // Tolerance for the intake.
+  static constexpr double kEpsIntake = 0.05;
+  // Tolerance for the wrist.
+  static constexpr double kEpsWrist = 0.05;
+
+ private:
+  void clear_min_wrist_goal() {
+    min_wrist_goal_ = -::std::numeric_limits<double>::infinity();
+  }
+  void clear_max_wrist_goal() {
+    max_wrist_goal_ = ::std::numeric_limits<double>::infinity();
+  }
+  void clear_min_elevator_goal() {
+    min_elevator_goal_ = -::std::numeric_limits<double>::infinity();
+  }
+  void clear_min_intake_goal() {
+    min_intake_goal_ = -::std::numeric_limits<double>::infinity();
+  }
+  void clear_max_intake_goal() {
+    max_intake_goal_ = ::std::numeric_limits<double>::infinity();
+  }
+
+  double min_wrist_goal_;
+  double max_wrist_goal_;
+  double min_elevator_goal_;
+  double min_intake_goal_;
+  double max_intake_goal_;
+};
+
+}  // namespace superstructure
+}  // namespace control_loops
+}  // namespace y2019
+
+#endif  // Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_COLLISION_AVOIDANCE_H_
diff --git a/y2019/control_loops/superstructure/collision_avoidance_tests.cc b/y2019/control_loops/superstructure/collision_avoidance_tests.cc
new file mode 100644
index 0000000..834a251
--- /dev/null
+++ b/y2019/control_loops/superstructure/collision_avoidance_tests.cc
@@ -0,0 +1,281 @@
+#include "y2019/control_loops/superstructure/collision_avoidance.h"
+
+#include "aos/commonmath.h"
+#include "gtest/gtest.h"
+#include "y2019/control_loops/superstructure/superstructure.q.h"
+
+namespace y2019 {
+namespace control_loops {
+namespace superstructure {
+namespace testing {
+
+/*
+Test List:
+    FullClockwiseRotationFromBottomBackIntakeIn
+    QuarterClockwiseRotationFromMiddleFrontIntakeOut
+    QuarterClockwiseRotationFromMiddleFrontIntakeMiddle
+    QuarterClockwiseRotationFromMiddleFrontIntakeMoving
+
+    FullCounterClockwiseRotationFromBottomFrontIntakeIn
+    QuarterCounterClockwiseRotationFromBottomFrontIntakeOut
+    QuarterCounterClockwiseRotationFromBottomFrontIntakeMiddle
+    QuarterCounterClockwiseRotationFromBottomFrontIntakeMoving
+*/
+
+class CollisionAvoidanceTests : public ::testing::Test {
+ public:
+  void Iterate() {
+    SuperstructureQueue::Goal safe_goal;
+    while (true) {
+      avoidance.UpdateGoal(&status, &unsafe_goal);
+
+      EXPECT_FALSE(avoidance.IsCollided(&status));
+      safe_goal.wrist.angle =
+          ::aos::Clip(unsafe_goal.wrist.angle, avoidance.min_wrist_goal(),
+                      avoidance.max_wrist_goal());
+
+      safe_goal.elevator.height = ::std::max(unsafe_goal.elevator.height,
+                                             avoidance.min_elevator_goal());
+
+      safe_goal.intake.joint_angle =
+          ::aos::Clip(unsafe_goal.intake.joint_angle,
+                      avoidance.min_intake_goal(), avoidance.max_intake_goal());
+
+      LimitedMove(&status.wrist.position, safe_goal.wrist.angle);
+      LimitedMove(&status.elevator.position, safe_goal.elevator.height);
+      LimitedMove(&status.intake.position, safe_goal.intake.joint_angle);
+      if (IsMoving()) {
+        break;
+      }
+      past_status = status;
+    }
+  }
+
+  bool IsMoving() {
+    if ((past_status.wrist.position == status.wrist.position) &&
+        (past_status.elevator.position == status.elevator.position) &&
+        (past_status.intake.position == status.intake.position)) {
+      return true;
+    } else {
+      return false;
+    }
+  }
+
+  // provide goals and status messages
+  SuperstructureQueue::Goal unsafe_goal;
+  SuperstructureQueue::Status status;
+  SuperstructureQueue::Status past_status;
+
+ protected:
+  // setup for all tests
+  CollisionAvoidance avoidance;
+
+  void CheckGoals() {
+    // check to see if we reached the goals
+    ASSERT_NEAR(unsafe_goal.wrist.angle, status.wrist.position, 0.001);
+    ASSERT_NEAR(unsafe_goal.elevator.height, status.elevator.position, 0.001);
+    ASSERT_NEAR(unsafe_goal.intake.joint_angle, status.intake.position, 0.001);
+  }
+
+ private:
+  void LimitedMove(float *position, double goal) {
+    if (*position + kIterationMove < goal) {
+      *position += kIterationMove;
+    } else if (*position - kIterationMove > goal) {
+      *position -= kIterationMove;
+    } else {
+      *position = goal;
+    }
+  }
+
+  static constexpr double kIterationMove = 0.001;
+};
+
+// It is trying to rotate from far back to front low.
+TEST_F(CollisionAvoidanceTests, FullClockwiseRotationFromBottomBackIntakeIn) {
+  // changes the goals to be in the position where the angle is low front and
+  // the elevator is all the way at the bottom with the intake attempting to be
+  // in.
+  unsafe_goal.wrist.angle = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+  unsafe_goal.elevator.height = 0.0;
+  unsafe_goal.intake.joint_angle =
+      avoidance.kIntakeInAngle - avoidance.kEpsIntake;
+
+  // sets the status position messgaes to be have the elevator at the bottom
+  // with the intake in and the wrist low back
+  status.wrist.position = avoidance.kWristMinAngle + avoidance.kEpsWrist;
+  status.elevator.position = 0.0;
+  status.intake.position = avoidance.kIntakeInAngle - avoidance.kEpsIntake;
+
+  Iterate();
+
+  CheckGoals();
+}
+
+// It is trying to rotate from the front middle to front bottom.
+TEST_F(CollisionAvoidanceTests,
+       QuarterClockwiseRotationFromMiddleFrontIntakeOut) {
+  // changes the goals to be in the position where the angle is low front and
+  // the elevator is all the way at the bottom with the intake attempting to be
+  // out.
+  unsafe_goal.wrist.angle = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+  unsafe_goal.elevator.height = 0.0;
+  unsafe_goal.intake.joint_angle =
+      avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+
+  // sets the status position messgaes to be have the elevator at the half way
+  // with the intake in and the wrist middle front
+  status.wrist.position =
+      avoidance.kWristMaxAngle - (avoidance.kEpsWrist * 2.0);
+  status.elevator.position = 0.0;
+  status.intake.position = avoidance.kIntakeOutAngle;
+
+  Iterate();
+
+  CheckGoals();
+}
+// It is trying to rotate from the front middle to front bottom.
+TEST_F(CollisionAvoidanceTests,
+       QuarterClockwiseRotationFromMiddleFrontIntakeMiddle) {
+  // changes the goals to be in the position where the angle is low front and
+  // the elevator is all the way at the bottom with the intake attempting to be
+  // in.
+  status.wrist.position = avoidance.kWristMaxAngle / 2.0;
+  status.elevator.position = 0.5;
+  status.intake.position =
+      (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
+
+  // sets the status position messgaes to be have the elevator at the half way
+  // with the intake in and the wrist middle front
+  unsafe_goal.wrist.angle = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+  unsafe_goal.elevator.height = 0.0;
+  unsafe_goal.intake.joint_angle =
+      avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+
+  Iterate();
+
+  CheckGoals();
+}
+
+// It is trying to rotate from front low to far back.
+TEST_F(CollisionAvoidanceTests,
+       FullCounterClockwiseRotationFromBottomBackIntakeIn) {
+  // sets the status position messgaes to be have the elevator at the bottom
+  // with the intake in and the wrist low back
+  status.wrist.position = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+  status.elevator.position = 0.0;
+  status.intake.position = avoidance.kIntakeInAngle - avoidance.kEpsIntake;
+
+  // changes the goals to be in the position where the angle is low front and
+  // the elevator is all the way at the bottom with the intake attempting to be
+  // in.
+  unsafe_goal.wrist.angle = avoidance.kWristMinAngle + avoidance.kEpsWrist;
+  unsafe_goal.elevator.height = 0.0;
+  unsafe_goal.intake.joint_angle =
+      avoidance.kIntakeInAngle - avoidance.kEpsIntake;
+
+  Iterate();
+
+  CheckGoals();
+}
+
+// It is trying to rotate from the front bottom to front middle.
+TEST_F(CollisionAvoidanceTests,
+       QuarterCounterClockwiseRotationFromMiddleFrontIntakeOut) {
+  // changes the goals to be in the position where the angle is low front and
+  // the elevator is all the way at the bottom with the intake attempting to be
+  // out.
+  unsafe_goal.wrist.angle =
+      avoidance.kWristMaxAngle - (avoidance.kEpsWrist * 2.0);
+  unsafe_goal.elevator.height = 0.0;
+  unsafe_goal.intake.joint_angle =
+      avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+
+  // sets the status position messgaes to be have the elevator at the half way
+  // with the intake in and the wrist middle front
+  status.wrist.position = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+  status.elevator.position = 0.0;
+  status.intake.position = avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+
+  Iterate();
+
+  CheckGoals();
+}
+
+// It is trying to rotate from the front bottom to front middle.
+TEST_F(CollisionAvoidanceTests,
+       QuarterCounterClockwiseRotationFromBottomFrontIntakeMiddle) {
+  // changes the goals to be in the position where the angle is low front and
+  // the elevator is all the way at the bottom with the intake attempting to be
+  // out.
+  unsafe_goal.wrist.angle =
+      avoidance.kWristMaxAngle - (avoidance.kEpsWrist * 2.0);
+  unsafe_goal.elevator.height = 0.0;
+  unsafe_goal.intake.joint_angle =
+      avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+
+  // sets the status position messgaes to be have the elevator at the half way
+  // with the intake in and the wrist middle front
+  status.wrist.position = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+  status.elevator.position = 0.5;
+  status.intake.position =
+      (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
+
+  Iterate();
+
+  CheckGoals();
+}
+
+// Unreasonable Elevator Goal
+TEST_F(CollisionAvoidanceTests, UnreasonableElevatorGoal) {
+  // changes the goals to be in the position where the angle is low front and
+  // the elevator is all the way at the bottom with the intake attempting to be
+  // out.
+  unsafe_goal.wrist.angle = 4.0;
+  unsafe_goal.elevator.height = 0.0;
+  unsafe_goal.intake.joint_angle =
+      avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+
+  // sets the status position messgaes to be have the elevator at the half way
+  // with the intake in and the wrist middle front
+  status.wrist.position = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+  status.elevator.position = 0.45;
+  status.intake.position = avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+
+  Iterate();
+
+  ASSERT_NEAR(unsafe_goal.wrist.angle, status.wrist.position, 0.001);
+  ASSERT_NEAR((avoidance.kElevatorClearWristDownHeight + avoidance.kEps),
+              status.elevator.position, 0.001);
+  ASSERT_NEAR(unsafe_goal.intake.joint_angle, status.intake.position, 0.001);
+}
+
+// Unreasonable Wrist Goal
+TEST_F(CollisionAvoidanceTests, UnreasonableWristGoal) {
+  // changes the goals to be in the position where the angle is low front and
+  // the elevator is all the way at the bottom with the intake attempting to be
+  // out.
+  unsafe_goal.wrist.angle = avoidance.kWristMinAngle;
+  unsafe_goal.elevator.height = 0.0;
+  unsafe_goal.intake.joint_angle =
+      (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
+
+  // sets the status position messgaes to be have the elevator at the half way
+  // with the intake in and the wrist middle front
+  status.wrist.position = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+  status.elevator.position = 0.45;
+  status.intake.position =
+      (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
+
+  Iterate();
+
+  ASSERT_NEAR(unsafe_goal.wrist.angle, status.wrist.position, 0.001);
+  ASSERT_NEAR((avoidance.kElevatorClearIntakeHeight + avoidance.kEps),
+              status.elevator.position, 0.001);
+  ASSERT_NEAR(unsafe_goal.intake.joint_angle, status.intake.position, 0.001);
+}
+
+}  // namespace testing
+}  // namespace superstructure
+}  // namespace control_loops
+}  // namespace y2019