diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index 58b2ca0..cbd4d2a 100644
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -5,6 +5,7 @@
 #include "aos/common/control_loop/control_loops.q.h"
 #include "aos/common/logging/logging.h"
 #include "aos/common/logging/queue_logging.h"
+#include "aos/common/logging/matrix_logging.h"
 
 #include "frc971/constants.h"
 #include "frc971/control_loops/claw/claw_motor_plant.h"
@@ -44,6 +45,17 @@
 
 namespace frc971 {
 namespace control_loops {
+namespace {
+
+template <typename T> int sign(T val) {
+  if (val > T(0)) {
+    return 1;
+  } else {
+    return -1;
+  }
+}
+
+}  // namespace
 
 static const double kZeroingVoltage = 4.0;
 static const double kMaxVoltage = 12.0;
@@ -57,7 +69,14 @@
                0, 1,
                0, -1).finished(),
               (Eigen::Matrix<double, 4, 1>() << kMaxVoltage, kMaxVoltage,
-               kMaxVoltage, kMaxVoltage).finished()) {
+               kMaxVoltage, kMaxVoltage).finished()),
+      U_Poly_zeroing_((Eigen::Matrix<double, 4, 2>() << 1, 0,
+               -1, 0,
+               0, 1,
+               0, -1).finished(),
+              (Eigen::Matrix<double, 4, 1>() <<
+               kZeroingVoltage, kZeroingVoltage,
+               kZeroingVoltage, kZeroingVoltage).finished()) {
   ::aos::controls::HPolytope<0>::Init();
 }
 
@@ -79,6 +98,7 @@
   double max_voltage = is_zeroing_ ? kZeroingVoltage : kMaxVoltage;
 
   if (::std::abs(u_bottom) > max_voltage or ::std::abs(u_top) > max_voltage) {
+    LOG_MATRIX(DEBUG, "U at start", U);
     // H * U <= k
     // U = UPos + UVel
     // H * (UPos + UVel) <= k
@@ -100,22 +120,59 @@
     position_error << error(0, 0), error(1, 0);
     Eigen::Matrix<double, 2, 1> velocity_error;
     velocity_error << error(2, 0), error(3, 0);
+    LOG_MATRIX(DEBUG, "error", error);
 
-    Eigen::Matrix<double, 4, 1> pos_poly_k =
-        U_Poly_.k() - U_Poly_.H() * velocity_K * velocity_error;
-    Eigen::Matrix<double, 4, 2> pos_poly_H = U_Poly_.H() * position_K;
-    ::aos::controls::HPolytope<2> pos_poly(pos_poly_H, pos_poly_k);
+    const auto &poly = is_zeroing_ ? U_Poly_zeroing_ : U_Poly_;
+    const Eigen::Matrix<double, 4, 2> pos_poly_H = poly.H() * position_K;
+    const Eigen::Matrix<double, 4, 1> pos_poly_k =
+        poly.k() - poly.H() * velocity_K * velocity_error;
+    const ::aos::controls::HPolytope<2> pos_poly(pos_poly_H, pos_poly_k);
 
+    Eigen::Matrix<double, 2, 1> adjusted_pos_error;
+    {
+      const auto &P = position_error;
+      Eigen::Matrix<double, 1, 2> L45;
+      L45 << sign(P(1, 0)), -sign(P(0, 0));
+      const double w45 = 0;
 
-    Eigen::Matrix<double, 2, 1> adjusted_pos_error = CoerceGoal(
-        pos_poly, (Eigen::Matrix<double, 1, 2>() << position_error(1, 0),
-                   -position_error(0, 0)).finished(),
-        0.0, position_error);
+      Eigen::Matrix<double, 1, 2> LH;
+      if (::std::abs(P(0, 0)) > ::std::abs(P(1, 0))) {
+        LH << 0, 1;
+      } else {
+        LH << 1, 0;
+      }
+      const double wh = LH.dot(P);
 
-    LOG(DEBUG, "Capping U is now %f %f\n", U(0, 0), U(1, 0));
+      Eigen::Matrix<double, 2, 2> standard;
+      standard << L45, LH;
+      Eigen::Matrix<double, 2, 1> W;
+      W << w45, wh;
+      const Eigen::Matrix<double, 2, 1> intersection = standard.inverse() * W;
+
+      bool is_inside_h;
+      const auto adjusted_pos_error_h =
+          DoCoerceGoal(pos_poly, LH, wh, position_error, &is_inside_h);
+      const auto adjusted_pos_error_45 =
+          DoCoerceGoal(pos_poly, L45, w45, intersection, nullptr);
+      if (pos_poly.IsInside(intersection)) {
+        adjusted_pos_error = adjusted_pos_error_h;
+      } else {
+        if (is_inside_h) {
+          if (adjusted_pos_error_h.norm() > adjusted_pos_error_45.norm()) {
+            adjusted_pos_error = adjusted_pos_error_h;
+          } else {
+            adjusted_pos_error = adjusted_pos_error_45;
+          }
+        } else {
+          adjusted_pos_error = adjusted_pos_error_45;
+        }
+      }
+    }
+
+    LOG_MATRIX(DEBUG, "adjusted_pos_error", adjusted_pos_error);
     U = velocity_K * velocity_error + position_K * adjusted_pos_error;
+    LOG_MATRIX(DEBUG, "U is now", U);
   }
-
 }
 
 ZeroedStateFeedbackLoop::ZeroedStateFeedbackLoop(const char *name,
@@ -741,13 +798,12 @@
     case FINE_TUNE_BOTTOM:
     case FINE_TUNE_TOP:
     case UNKNOWN_LOCATION: {
-      Eigen::Matrix<double, 2, 1> U = claw_.K() * (claw_.R - claw_.X_hat);
-      LOG(DEBUG, "Uncapped voltages: Top: %f, Bottom: %f\n", U(1, 0), U(0, 0));
+      LOG(DEBUG, "Uncapped voltages: Top: %f, Bottom: %f\n", claw_.U_uncapped(1, 0), claw_.U_uncapped(0, 0));
       if (claw_.uncapped_average_voltage() > values.claw.max_zeroing_voltage) {
-        double dx_bot = (U(0, 0) -
+        double dx_bot = (claw_.U_uncapped(0, 0) -
                      values.claw.max_zeroing_voltage) /
                     claw_.K(0, 0);
-        double dx_top = (U(1, 0) -
+        double dx_top = (claw_.U_uncapped(1, 0) -
                      values.claw.max_zeroing_voltage) /
                     claw_.K(0, 0);
         double dx = ::std::max(dx_top, dx_bot);
@@ -755,7 +811,7 @@
         top_claw_goal_ -= dx;
         Eigen::Matrix<double, 4, 1> R;
         R << bottom_claw_goal_, top_claw_goal_ - bottom_claw_goal_, claw_.R(2, 0), claw_.R(3, 0);
-        U = claw_.K() * (R - claw_.X_hat);
+        claw_.U = claw_.K() * (R - claw_.X_hat);
         capped_goal_ = true;
         LOG(DEBUG, "Moving the goal by %f to prevent windup."
             " Uncapped is %f, max is %f, difference is %f\n",
@@ -765,10 +821,10 @@
              values.claw.max_zeroing_voltage));
       } else if (claw_.uncapped_average_voltage() <
                  -values.claw.max_zeroing_voltage) {
-        double dx_bot = (U(0, 0) +
+        double dx_bot = (claw_.U_uncapped(0, 0) +
                      values.claw.max_zeroing_voltage) /
                     claw_.K(0, 0);
-        double dx_top = (U(1, 0) +
+        double dx_top = (claw_.U_uncapped(1, 0) +
                      values.claw.max_zeroing_voltage) /
                     claw_.K(0, 0);
         double dx = ::std::min(dx_top, dx_bot);
@@ -776,7 +832,7 @@
         top_claw_goal_ -= dx;
         Eigen::Matrix<double, 4, 1> R;
         R << bottom_claw_goal_, top_claw_goal_ - bottom_claw_goal_, claw_.R(2, 0), claw_.R(3, 0);
-        U = claw_.K() * (R - claw_.X_hat);
+        claw_.U = claw_.K() * (R - claw_.X_hat);
         capped_goal_ = true;
         LOG(DEBUG, "Moving the goal by %f to prevent windup\n", dx);
       }
diff --git a/frc971/control_loops/claw/claw.gyp b/frc971/control_loops/claw/claw.gyp
index bdce229..5982378 100644
--- a/frc971/control_loops/claw/claw.gyp
+++ b/frc971/control_loops/claw/claw.gyp
@@ -32,6 +32,7 @@
         '<(DEPTH)/aos/build/externals.gyp:libcdd',
         '<(DEPTH)/frc971/control_loops/control_loops.gyp:coerce_goal',
         '<(AOS)/common/logging/logging.gyp:queue_logging',
+        '<(AOS)/common/logging/logging.gyp:matrix_logging',
       ],
       'export_dependent_settings': [
         'claw_loop',
diff --git a/frc971/control_loops/claw/claw.h b/frc971/control_loops/claw/claw.h
index dab1dca..6dbc408 100644
--- a/frc971/control_loops/claw/claw.h
+++ b/frc971/control_loops/claw/claw.h
@@ -39,7 +39,7 @@
   double uncapped_average_voltage_;
   bool is_zeroing_;
 
-  const ::aos::controls::HPolytope<2> U_Poly_;
+  const ::aos::controls::HPolytope<2> U_Poly_, U_Poly_zeroing_;
 };
 
 class ClawMotor;
diff --git a/frc971/control_loops/claw/claw_lib_test.cc b/frc971/control_loops/claw/claw_lib_test.cc
index 8704ad0..61813bc 100644
--- a/frc971/control_loops/claw/claw_lib_test.cc
+++ b/frc971/control_loops/claw/claw_lib_test.cc
@@ -304,7 +304,7 @@
         claw_motor_plant_.GetAbsolutePosition(TOP_CLAW) - bottom;
     EXPECT_NEAR(claw_queue_group.goal->bottom_angle, bottom, 1e-4);
     EXPECT_NEAR(claw_queue_group.goal->separation_angle, separation, 1e-4);
-    EXPECT_TRUE(min_separation_ <= separation);
+    EXPECT_LE(min_separation_, separation);
   }
 
 
diff --git a/frc971/control_loops/claw/claw_motor_plant.cc b/frc971/control_loops/claw/claw_motor_plant.cc
index 8da2415..28151ad 100644
--- a/frc971/control_loops/claw/claw_motor_plant.cc
+++ b/frc971/control_loops/claw/claw_motor_plant.cc
@@ -9,9 +9,9 @@
 
 StateFeedbackPlantCoefficients<4, 2, 2> MakeClawPlantCoefficients() {
   Eigen::Matrix<double, 4, 4> A;
-  A << 1.0, 0.0, 0.00737284608086, 0.0, 0.0, 1.0, -0.00145272885484, 0.00592011722602, 0.0, 0.0, 0.525184383468, 0.0, 0.0, 0.0, -0.211450629042, 0.313733754426;
+  A << 1.0, 0.0, 0.00909331035298, 0.0, 0.0, 1.0, -6.04514323962e-05, 0.00903285892058, 0.0, 0.0, 0.824315642255, 0.0, 0.0, 0.0, -0.0112975266368, 0.813018115618;
   Eigen::Matrix<double, 4, 2> B;
-  B << 0.00102145540588, 0.0, -0.00102145540588, 0.00158628631709, 0.184611558069, 0.0, -0.184611558069, 0.26682500835;
+  B << 0.000352527133889, 0.0, -0.000352527133889, 0.000376031064118, 0.0683072794628, 0.0, -0.0683072794628, 0.0726998350615;
   Eigen::Matrix<double, 2, 4> C;
   C << 1, 0, 0, 0, 1, 1, 0, 0;
   Eigen::Matrix<double, 2, 2> D;
@@ -25,9 +25,9 @@
 
 StateFeedbackController<4, 2, 2> MakeClawController() {
   Eigen::Matrix<double, 4, 2> L;
-  L << 1.48518438347, -2.35513868803e-16, -1.48518438347, 1.27373375443, 34.6171964667, -5.41681898246e-15, -34.6171964667, 14.5766570483;
+  L << 1.72431564225, 1.53329341668e-19, -1.72431564225, 1.71301811562, 65.9456997026, -0, -65.9456997026, 64.4642687194;
   Eigen::Matrix<double, 2, 4> K;
-  K << 104.272994613, 0.0, 1.72618753001, 0.0, 67.1443817466, 107.935909674, 0.195736876688, 0.983852673373;
+  K << 106.138028875, 0.0, 4.20012492658, 0.0, 99.5038407367, 99.7251230882, 3.77683310096, 3.78980738032;
   return StateFeedbackController<4, 2, 2>(L, K, MakeClawPlantCoefficients());
 }
 
diff --git a/frc971/control_loops/claw/claw_motor_plant.h b/frc971/control_loops/claw/claw_motor_plant.h
index 95d0fee..76e3fe7 100644
--- a/frc971/control_loops/claw/claw_motor_plant.h
+++ b/frc971/control_loops/claw/claw_motor_plant.h
@@ -14,7 +14,7 @@
 
 StateFeedbackLoop<4, 2, 2> MakeClawLoop();
 
-const double kClawMomentOfInertiaRatio = 0.555556;
+const double kClawMomentOfInertiaRatio = 0.933333;
 
 }  // namespace control_loops
 }  // namespace frc971
diff --git a/frc971/control_loops/coerce_goal.cc b/frc971/control_loops/coerce_goal.cc
index b32b590..de6b757 100644
--- a/frc971/control_loops/coerce_goal.cc
+++ b/frc971/control_loops/coerce_goal.cc
@@ -7,11 +7,13 @@
 namespace frc971 {
 namespace control_loops {
 
-Eigen::Matrix<double, 2, 1> CoerceGoal(aos::controls::HPolytope<2> &region,
-                                       const Eigen::Matrix<double, 1, 2> &K,
-                                       double w,
-                                       const Eigen::Matrix<double, 2, 1> &R) {
+Eigen::Matrix<double, 2, 1> DoCoerceGoal(const aos::controls::HPolytope<2> &region,
+                                         const Eigen::Matrix<double, 1, 2> &K,
+                                         double w,
+                                         const Eigen::Matrix<double, 2, 1> &R,
+                                         bool *is_inside) {
   if (region.IsInside(R)) {
+    if (is_inside) *is_inside = true;
     return R;
   }
   Eigen::Matrix<double, 2, 1> parallel_vector;
@@ -36,6 +38,7 @@
         min_distance_sqr = length;
       }
     }
+    if (is_inside) *is_inside = true;
     return closest_point;
   } else {
     Eigen::Matrix<double, 2, Eigen::Dynamic> region_vertices =
@@ -50,6 +53,7 @@
         min_distance = length;
       }
     }
+    if (is_inside) *is_inside = false;
     return (Eigen::Matrix<double, 2, 1>() << region_vertices(0, closest_i),
             region_vertices(1, closest_i)).finished();
   }
diff --git a/frc971/control_loops/coerce_goal.h b/frc971/control_loops/coerce_goal.h
index 43707b4..59049c5 100644
--- a/frc971/control_loops/coerce_goal.h
+++ b/frc971/control_loops/coerce_goal.h
@@ -8,14 +8,23 @@
 namespace frc971 {
 namespace control_loops {
 
+Eigen::Matrix<double, 2, 1> DoCoerceGoal(const aos::controls::HPolytope<2> &region,
+                                         const Eigen::Matrix<double, 1, 2> &K,
+                                         double w,
+                                         const Eigen::Matrix<double, 2, 1> &R,
+                                         bool *is_inside);
+
 // Intersects a line with a region, and finds the closest point to R.
 // Finds a point that is closest to R inside the region, and on the line
 // defined by K X = w.  If it is not possible to find a point on the line,
 // finds a point that is inside the region and closest to the line.
-Eigen::Matrix<double, 2, 1> CoerceGoal(aos::controls::HPolytope<2> &region,
-                                       const Eigen::Matrix<double, 1, 2> &K,
-                                       double w,
-                                       const Eigen::Matrix<double, 2, 1> &R);
+static inline Eigen::Matrix<double, 2, 1>
+    CoerceGoal(const aos::controls::HPolytope<2> &region,
+               const Eigen::Matrix<double, 1, 2> &K,
+               double w,
+               const Eigen::Matrix<double, 2, 1> &R) {
+  return DoCoerceGoal(region, K, w, R, nullptr);
+}
 
 }  // namespace control_loops
 }  // namespace frc971
diff --git a/frc971/control_loops/python/claw.py b/frc971/control_loops/python/claw.py
index cb11a8b..9a24e22 100755
--- a/frc971/control_loops/python/claw.py
+++ b/frc971/control_loops/python/claw.py
@@ -20,9 +20,8 @@
     # Free Current in Amps
     self.free_current = 2.7
     # Moment of inertia of the claw in kg m^2
-    # measured from CAD
-    self.J_top = 0.5
-    self.J_bottom = 0.9
+    self.J_top = 2.8
+    self.J_bottom = 3.0
 
     # Resistance of the motor
     self.R = 12.0 / self.stall_current
@@ -96,8 +95,8 @@
     self.A_diff, self.B_diff = controls.c2d(
         self.A_diff_cont, self.B_diff_cont, self.dt)
 
-    self.K_bottom = controls.dplace(self.A_bottom, self.B_bottom, [.65, .45])
-    self.K_diff = controls.dplace(self.A_diff, self.B_diff, [.60, .28])
+    self.K_bottom = controls.dplace(self.A_bottom, self.B_bottom, [.75 + 0.1j, .75 - 0.1j])
+    self.K_diff = controls.dplace(self.A_diff, self.B_diff, [.75 + 0.1j, .75 - 0.1j])
 
     print "K_diff", self.K_diff
     print "K_bottom", self.K_bottom
@@ -145,8 +144,8 @@
     print "eigenvalues"
     print numpy.linalg.eig(F)[0]
 
-    self.rpl = .02
-    self.ipl = 0.004
+    self.rpl = .05
+    self.ipl = 0.010
     self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
                              self.rpl + 1j * self.ipl,
                              self.rpl - 1j * self.ipl,
@@ -301,6 +300,7 @@
       claw: claw object containing moments of inertia and U limits.
       U: Input matrix to clip as necessary.
   """
+
   bottom_u = U[0, 0]
   top_u = U[1, 0]
 
@@ -338,8 +338,48 @@
         claw.U_poly.H * position_K,
         claw.U_poly.k - claw.U_poly.H * velocity_K * velocity_error)
 
-    adjusted_pos_error = polydrivetrain.CoerceGoal(pos_poly, numpy.matrix([[position_error[1, 0], -position_error[0, 0]]]), 0.0, position_error)
+    P = position_error
+    #K = numpy.matrix([[position_error[1, 0], -position_error[0, 0]]])
+    L45 = numpy.matrix([[numpy.sign(P[1, 0]), -numpy.sign(P[0, 0])]])
+    if L45[0, 1] == 0:
+      L45[0, 1] = 1
+    if L45[0, 0] == 0:
+      L45[0, 0] = 1
+    w45 = numpy.matrix([[0]])
 
+    if numpy.abs(P[0, 0]) > numpy.abs(P[1, 0]):
+      LH = numpy.matrix([[0, 1]])
+    else:
+      LH = numpy.matrix([[1, 0]])
+    wh = LH * P
+    standard = numpy.concatenate((L45, LH))
+    #print "Standard", standard
+    W = numpy.concatenate((w45, wh))
+    #print "W is", W
+    intersection = numpy.linalg.inv(standard) * W
+    print "intersection point %s" % intersection
+    print "Intersection power is ", velocity_K * velocity_error + position_K * intersection
+    adjusted_pos_error_h, is_inside_h = polydrivetrain.DoCoerceGoal(pos_poly,
+        LH, wh, position_error)
+    adjusted_pos_error_45, is_inside_45 = polydrivetrain.DoCoerceGoal(pos_poly,
+        L45, w45, intersection)
+    if pos_poly.IsInside(intersection):
+      adjusted_pos_error = adjusted_pos_error_h
+      print "horizontal"
+    else:
+      if is_inside_h:
+        if numpy.linalg.norm(adjusted_pos_error_h) > numpy.linalg.norm(adjusted_pos_error_45):
+          adjusted_pos_error = adjusted_pos_error_h
+        else:
+          adjusted_pos_error = adjusted_pos_error_45
+      else:
+        adjusted_pos_error = adjusted_pos_error_45
+      print "45"
+      print velocity_K * velocity_error + position_K * adjusted_pos_error
+      print "45"
+    print adjusted_pos_error
+
+    print "Actual power is ", velocity_K * velocity_error + position_K * adjusted_pos_error
     return velocity_K * velocity_error + position_K * adjusted_pos_error
 
     #U = Kpos * poserror + Kvel * velerror
@@ -423,7 +463,7 @@
 
     The tests themselves are not terribly sophisticated; I just test for 
     whether the goal has been reached and whether the separation goes
-    outside of the initial and goal values by more then max_separation_error.
+    outside of the initial and goal values by more than max_separation_error.
     Prints out something for a failure of either condition and returns
     False if tests fail.
     Args:
@@ -498,11 +538,16 @@
   R = numpy.matrix([[0.0], [1.0], [0.0], [0.0]])
   run_test(claw, initial_X, R)
 
-  # Test changing both separation and position at once..
+  # Test changing both separation and position at once.
   initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
   R = numpy.matrix([[1.0], [1.0], [0.0], [0.0]])
   run_test(claw, initial_X, R)
 
+  # Test a small separation error and a large position one.
+  initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
+  R = numpy.matrix([[2.0], [0.05], [0.0], [0.0]])
+  run_test(claw, initial_X, R)
+
   # Write the generated constants out to a file.
   if len(argv) != 3:
     print "Expected .h file name and .cc file name for the claw."
diff --git a/frc971/control_loops/python/polydrivetrain.py b/frc971/control_loops/python/polydrivetrain.py
index f2dfdbe..2b24396 100755
--- a/frc971/control_loops/python/polydrivetrain.py
+++ b/frc971/control_loops/python/polydrivetrain.py
@@ -28,9 +28,11 @@
   Returns:
     numpy.matrix (2 x 1), the point.
   """
+  return DoCoerceGoal(region, K, w, R)[0]
 
+def DoCoerceGoal(region, K, w, R):
   if region.IsInside(R):
-    return R
+    return (R, True)
 
   perpendicular_vector = K.T / numpy.linalg.norm(K)
   parallel_vector = numpy.matrix([[perpendicular_vector[1, 0]],
@@ -79,7 +81,7 @@
         min_distance = length
         closest_point = point
 
-    return closest_point
+    return (closest_point, True)
   else:
     # Find the vertex of the space that is closest to the line.
     region_vertices = region.Vertices()
@@ -92,7 +94,7 @@
         min_distance = length
         closest_point = point
 
-    return closest_point
+    return (closest_point, False)
 
 
 class VelocityDrivetrainModel(control_loop.ControlLoop):