diff --git a/y2019/control_loops/superstructure/collision_avoidance.cc b/y2019/control_loops/superstructure/collision_avoidance.cc
index ed6787f..c999cac 100644
--- a/y2019/control_loops/superstructure/collision_avoidance.cc
+++ b/y2019/control_loops/superstructure/collision_avoidance.cc
@@ -52,7 +52,8 @@
   }
 
   // Elevator is down so the intake has to be at either extreme.
-  if (elevator_position < kElevatorClearIntakeHeight) {
+  if (elevator_position < kElevatorClearIntakeHeight &&
+      wrist_position > kWristMaxAngle) {
     if (intake_position < kIntakeOutAngle && intake_position > kIntakeInAngle) {
       return true;
     }
@@ -78,7 +79,9 @@
   // 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) {
+    if (wrist_position < (2.0 * kWristElevatorCollisionMinAngle +
+                          kWristElevatorCollisionMaxAngle) /
+                             3.0) {
       update_max_wrist_goal(kWristElevatorCollisionMinAngle - kEpsWrist);
     } else {
       update_min_wrist_goal(kWristElevatorCollisionMaxAngle + kEpsWrist);
@@ -97,7 +100,8 @@
 
   // If the elevator is too low, the intake can't transition from in to out or
   // back.
-  if (elevator_position < kElevatorClearIntakeHeight) {
+  if (elevator_position < kElevatorClearIntakeHeight &&
+      wrist_position > kWristElevatorCollisionMaxAngle) {
     // Figure out if the intake is in our out and keep it there.
     if (intake_position < kIntakeMiddleAngle) {
       update_max_intake_goal(kIntakeInAngle - kEpsIntake);
@@ -110,10 +114,18 @@
   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) {
+  if (intake_position > kIntakeInAngle && intake_position < kIntakeOutAngle &&
+      wrist_position > kWristElevatorCollisionMaxAngle) {
     update_min_elevator_goal(kElevatorClearIntakeHeight + kEps);
   }
 
+  // If the intake is in the collision range and the elevator is down, don't let
+  // the wrist go far down.
+  if (intake_position > kIntakeInAngle && intake_position < kIntakeOutAngle &&
+      elevator_position < kElevatorClearIntakeHeight) {
+    update_max_wrist_goal(kWristMaxAngle - kEpsWrist);
+  }
+
   // If the wrist is within the elevator collision range, don't let the elevator
   // go down.
   if (wrist_position > kWristElevatorCollisionMinAngle &&
@@ -141,7 +153,8 @@
 
     // 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) {
+    if (intake_needs_to_move &&
+        wrist_position > kWristElevatorCollisionMaxAngle) {
       update_min_elevator_goal(kElevatorClearIntakeHeight + kEps);
     }
     // If we need to move the wrist, we've got to shove the elevator up too. The
diff --git a/y2019/control_loops/superstructure/collision_avoidance.h b/y2019/control_loops/superstructure/collision_avoidance.h
index 5a8a772..ca29e7c 100644
--- a/y2019/control_loops/superstructure/collision_avoidance.h
+++ b/y2019/control_loops/superstructure/collision_avoidance.h
@@ -50,8 +50,6 @@
     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.35;
 
@@ -61,8 +59,8 @@
   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;
+  static constexpr double kWristMaxAngle = M_PI / 2.0 + 0.05;
+  static constexpr double kWristMinAngle = -M_PI / 2.0 - 0.05;
 
   // Angles outside of which the intake is fully clear of the wrist.
   static constexpr double kIntakeOutAngle = 0.3;
diff --git a/y2019/control_loops/superstructure/collision_avoidance_tests.cc b/y2019/control_loops/superstructure/collision_avoidance_tests.cc
index 17f9fdc..918bc5e 100644
--- a/y2019/control_loops/superstructure/collision_avoidance_tests.cc
+++ b/y2019/control_loops/superstructure/collision_avoidance_tests.cc
@@ -260,24 +260,25 @@
   // 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.unsafe_goal = avoidance.kWristMinAngle;
+  unsafe_goal.wrist.unsafe_goal =
+      avoidance.kWristMaxAngle - avoidance.kEpsWrist;
   unsafe_goal.elevator.unsafe_goal = 0.0;
   unsafe_goal.intake.unsafe_goal =
       (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.wrist.position = avoidance.kWristMinAngle + avoidance.kEpsWrist;
   status.elevator.position = 0.45;
   status.intake.position =
       (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
 
   Iterate();
 
-  ASSERT_NEAR(unsafe_goal.wrist.unsafe_goal, status.wrist.position, 0.001);
-  ASSERT_NEAR((avoidance.kElevatorClearIntakeHeight + avoidance.kEps),
+  EXPECT_NEAR(unsafe_goal.wrist.unsafe_goal, status.wrist.position, 0.001);
+  EXPECT_NEAR((avoidance.kElevatorClearIntakeHeight + avoidance.kEps),
               status.elevator.position, 0.001);
-  ASSERT_NEAR(unsafe_goal.intake.unsafe_goal, status.intake.position, 0.001);
+  EXPECT_NEAR(unsafe_goal.intake.unsafe_goal, status.intake.position, 0.001);
 }
 
 // Fix Collision Wrist in Elevator
@@ -304,13 +305,13 @@
 // Fix Collision Wrist in Intake
 TEST_F(CollisionAvoidanceTests, FixWristCollision) {
   // changes the goals
-  unsafe_goal.wrist.unsafe_goal = avoidance.kWristMinAngle + avoidance.kEpsWrist;
+  unsafe_goal.wrist.unsafe_goal = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
   unsafe_goal.elevator.unsafe_goal = 0.0;
   unsafe_goal.intake.unsafe_goal =
       (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
 
   // sets the status position messgaes
-  status.wrist.position = avoidance.kWristMinAngle + avoidance.kEpsWrist;
+  status.wrist.position = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
   status.elevator.position = 0.0;
   status.intake.position =
       (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;