diff --git a/y2016/control_loops/superstructure/BUILD b/y2016/control_loops/superstructure/BUILD
index 17653ad..f633d7b 100644
--- a/y2016/control_loops/superstructure/BUILD
+++ b/y2016/control_loops/superstructure/BUILD
@@ -77,6 +77,7 @@
     ':superstructure_plants',
     '//aos/common/controls:control_loop',
     '//aos/common/util:trapezoid_profile',
+    '//aos/common:math',
     '//frc971/control_loops:state_feedback_loop',
     '//frc971/control_loops:simple_capped_state_feedback_loop',
     '//frc971/zeroing',
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index b82d2d9..e85657c 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -1,6 +1,7 @@
 #include "y2016/control_loops/superstructure/superstructure.h"
 #include "y2016/control_loops/superstructure/superstructure_controls.h"
 
+#include "aos/common/commonmath.h"
 #include "aos/common/controls/control_loops.q.h"
 #include "aos/common/logging/logging.h"
 
@@ -34,6 +35,7 @@
                                     double intake_angle_goal) {
   const double original_shoulder_angle_goal = shoulder_angle_goal;
   const double original_intake_angle_goal = intake_angle_goal;
+  const double original_wrist_angle_goal = wrist_angle_goal;
 
   double shoulder_angle = arm_->shoulder_angle();
   double wrist_angle = arm_->wrist_angle();
@@ -50,20 +52,40 @@
   // 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 (intake_angle < kMaxIntakeAngleBeforeArmInterference + kSafetyMargin) {
+    if (shoulder_angle < kMinShoulderAngleForHorizontalShooter ||
+        original_shoulder_angle_goal < kMinShoulderAngleForHorizontalShooter) {
+      wrist_angle_goal = 0.0;
+    } else if (shoulder_angle < kMinShoulderAngleForIntakeInterference ||
+               original_shoulder_angle_goal <
+                   kMinShoulderAngleForIntakeInterference) {
+      wrist_angle_goal =
+          aos::Clip(original_wrist_angle_goal,
+                    kMinWristAngleForMovingByIntake + kSafetyMargin,
+                    kMaxWristAngleForMovingByIntake - kSafetyMargin);
+    }
+  } else {
+    if (shoulder_angle < kMinShoulderAngleForIntakeUpInterference ||
+        original_shoulder_angle_goal <
+            kMinShoulderAngleForIntakeUpInterference) {
+      wrist_angle_goal = 0.0;
+    }
+  }
+
   if (shoulder_angle < kMinShoulderAngleForIntakeUpInterference ||
       original_shoulder_angle_goal < kMinShoulderAngleForIntakeUpInterference) {
-    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 (intake_angle < kMaxIntakeAngleBeforeArmInterference + kSafetyMargin) {
-      if (::std::abs(wrist_angle) > kMaxWristAngleForMovingByIntake) {
+      if (wrist_angle > kMaxWristAngleForMovingByIntake ||
+          wrist_angle < kMinWristAngleForMovingByIntake) {
         shoulder_angle_goal =
           ::std::max(original_shoulder_angle_goal,
               kMinShoulderAngleForIntakeInterference + kSafetyMargin);
       }
     } else {
-      if (::std::abs(wrist_angle) > kMaxWristAngleForMovingByIntake) {
+      if (wrist_angle > kMaxWristAngleForMovingByIntake ||
+          wrist_angle < kMinWristAngleForMovingByIntake) {
         shoulder_angle_goal =
           ::std::max(original_shoulder_angle_goal,
               kMinShoulderAngleForIntakeUpInterference + kSafetyMargin);
@@ -112,9 +134,16 @@
       if (shoulder_angle >= kHalfwayPointBetweenSafeZones) {
         // The shoulder is closer to being above the collision area. Move it up
         // there.
-        shoulder_angle_goal =
-            ::std::max(original_shoulder_angle_goal,
-                       kMinShoulderAngleForIntakeInterference + kSafetyMargin);
+        if (intake_angle <
+            kMaxIntakeAngleBeforeArmInterference + kSafetyMargin) {
+          shoulder_angle_goal = ::std::max(
+              original_shoulder_angle_goal,
+              kMinShoulderAngleForIntakeInterference + kSafetyMargin);
+        } else {
+          shoulder_angle_goal = ::std::max(
+              original_shoulder_angle_goal,
+              kMinShoulderAngleForIntakeUpInterference + kSafetyMargin);
+        }
       } else {
         // The shoulder is closer to being below the collision zone (i.e. in
         // stowing/intake position), keep it there for now.
@@ -159,14 +188,18 @@
           CollisionAvoidance::kMinShoulderAngleForIntakeInterference &&
       intake_angle < CollisionAvoidance::kMaxIntakeAngleBeforeArmInterference &&
       intake_angle > Superstructure::kIntakeLowerClear &&
-      ::std::abs(wrist_angle) >
-          CollisionAvoidance::kMaxWristAngleForMovingByIntake) {
-    LOG(DEBUG, "Collided: Intake %f < %f < %f, and shoulder %f < %f < %f.\n",
+      (wrist_angle > CollisionAvoidance::kMaxWristAngleForMovingByIntake ||
+       wrist_angle < CollisionAvoidance::kMinWristAngleForMovingByIntake)) {
+    LOG(DEBUG,
+        "Collided: Intake %f < %f < %f, shoulder %f < %f < %f, and %f < %f < "
+        "%f.\n",
         Superstructure::kIntakeLowerClear, intake_angle,
         CollisionAvoidance::kMaxIntakeAngleBeforeArmInterference,
         CollisionAvoidance::kMaxShoulderAngleUntilSafeIntakeStowing,
         shoulder_angle,
-        CollisionAvoidance::kMinShoulderAngleForIntakeInterference);
+        CollisionAvoidance::kMinShoulderAngleForIntakeInterference,
+        CollisionAvoidance::kMinWristAngleForMovingByIntake, wrist_angle,
+        CollisionAvoidance::kMaxWristAngleForMovingByIntake);
     return true;
   }
 
diff --git a/y2016/control_loops/superstructure/superstructure.h b/y2016/control_loops/superstructure/superstructure.h
index fd00fd1..7dad0ec 100644
--- a/y2016/control_loops/superstructure/superstructure.h
+++ b/y2016/control_loops/superstructure/superstructure.h
@@ -77,7 +77,7 @@
 
   // The intake angle (in radians) above which the intake can interfere (i.e.
   // collide) with the arm and/or shooter.
-  static constexpr double kMaxIntakeAngleBeforeArmInterference = 1.12;
+  static constexpr double kMaxIntakeAngleBeforeArmInterference = 1.05;
 
   // The maximum absolute angle (in radians) that the wrist must be below in
   // order for the shouler to be allowed to move below
@@ -86,9 +86,12 @@
   // also be placed into the belly pan.
   static constexpr double kMaxWristAngleForSafeArmStowing = 0.05;
 
-  // The maximum absolute angle in radians that the wrist can be from horizontal
+  // The maximum angle in radians that the wrist can be from horizontal
   // while it is near the intake.
   static constexpr double kMaxWristAngleForMovingByIntake = 0.50;
+  // The minimum angle in radians that the wrist can be from horizontal
+  // while it is near the intake.
+  static constexpr double kMinWristAngleForMovingByIntake = -1.20;
 
   // 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
@@ -109,7 +112,7 @@
 
   static constexpr double kZeroingVoltage = 5.0;
   static constexpr double kOperatingVoltage = 12.0;
-  static constexpr double kLandingShoulderDownVoltage = -2.0;
+  static constexpr double kLandingShoulderDownVoltage = -1.5;
 
   // This is the angle above which we will do a HIGH_ARM_ZERO, and below which
   // we will do a LOW_ARM_ZERO.
diff --git a/y2016/control_loops/superstructure/superstructure_lib_test.cc b/y2016/control_loops/superstructure/superstructure_lib_test.cc
index d278eb6..9fdec26 100644
--- a/y2016/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2016/control_loops/superstructure/superstructure_lib_test.cc
@@ -539,7 +539,8 @@
   ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
                   .angle_intake(constants::Values::kIntakeRange.upper)
                   .angle_shoulder(constants::Values::kShoulderRange.upper)
-                  .angle_wrist(constants::Values::kWristRange.upper)
+                  .angle_wrist(constants::Values::kWristRange.upper +
+                               constants::Values::kShoulderRange.upper)
                   .Send());
   // We have to wait for it to put the elevator in a safe position as well.
   RunForTime(Time::InSeconds(15));
@@ -561,7 +562,7 @@
                   .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));
+  RunForTime(Time::InSeconds(20));
 
   VerifyNearGoal();
 }
@@ -1095,7 +1096,7 @@
           .angle_wrist(0.0)                                         // Stowed
           .Send());
 
-  RunForTime(Time::InSeconds(10));
+  RunForTime(Time::InSeconds(15));
 
   ASSERT_TRUE(
       superstructure_queue_.goal.MakeWithBuilder()
@@ -1104,7 +1105,7 @@
           .angle_wrist(M_PI / 2.0)     // down
           .Send());
 
-  RunForTime(Time::InSeconds(3));
+  RunForTime(Time::InSeconds(5));
 
   superstructure_queue_.status.FetchLatest();
   ASSERT_TRUE(superstructure_queue_.status.get() != nullptr);
@@ -1120,7 +1121,10 @@
   EXPECT_NEAR(M_PI / 4.0, superstructure_queue_.status->shoulder.angle, 0.001);
 
   // The wrist should be forced into a stowing position.
-  EXPECT_NEAR(0, superstructure_queue_.status->wrist.angle, 0.001);
+  // Since the intake is kicked out, we can be within
+  // kMaxWristAngleForMovingByIntake
+  EXPECT_NEAR(0, superstructure_queue_.status->wrist.angle,
+              CollisionAvoidance::kMaxWristAngleForMovingByIntake + 0.001);
 
   ASSERT_TRUE(
       superstructure_queue_.goal.MakeWithBuilder()
@@ -1129,7 +1133,7 @@
           .angle_wrist(M_PI)     // forward
           .Send());
 
-  RunForTime(Time::InSeconds(3));
+  RunForTime(Time::InSeconds(5));
   VerifyNearGoal();
 }