diff --git a/frc971/control_loops/fridge/arm_motor_plant.cc b/frc971/control_loops/fridge/arm_motor_plant.cc
index c7d09ec..556b812 100644
--- a/frc971/control_loops/fridge/arm_motor_plant.cc
+++ b/frc971/control_loops/fridge/arm_motor_plant.cc
@@ -9,9 +9,9 @@
 
 StateFeedbackPlantCoefficients<4, 2, 2> MakeArmPlantCoefficients() {
   Eigen::Matrix<double, 4, 4> A;
-  A << 1.0, 0.00385609864291, 0.0, 0.0, 0.0, 0.580316842139, 0.0, 0.0, 0.0, 0.0, 0.997415079306, 0.00385216571322, 0.0, 0.0, -0.94787542156, 0.578159966602;
+  A << 1.0, 0.00479642025454, 0.0, 0.0, 0.0, 0.919688585028, 0.0, 0.0, 0.0, 0.0, 0.999539771613, 0.00479566382645, 0.0, 0.0, -0.18154390621, 0.919241022297;
   Eigen::Matrix<double, 4, 2> B;
-  B << 0.000138504938452, 0.000138504938452, 0.050815736504, 0.050815736504, 0.000138436627927, -0.000138436627927, 0.0507639082867, -0.0507639082867;
+  B << 2.46496779984e-05, 2.46496779984e-05, 0.00972420175808, 0.00972420175808, 2.46477449538e-05, -2.46477449538e-05, 0.00972266818532, -0.00972266818532;
   Eigen::Matrix<double, 2, 4> C;
   C << 1, 0, 1, 0, 1, 0, -1, 0;
   Eigen::Matrix<double, 2, 2> D;
@@ -25,11 +25,11 @@
 
 StateFeedbackController<4, 2, 2> MakeArmController() {
   Eigen::Matrix<double, 4, 2> L;
-  L << 0.59015842107, 0.59015842107, 19.0789855292, 19.0789855292, 0.587787522954, -0.587787522954, 18.4121865078, -18.4121865078;
+  L << 0.759844292514, 0.759844292514, 54.2541762188, 54.2541762188, 0.759390396955, -0.759390396955, 54.1048167043, -54.1048167043;
   Eigen::Matrix<double, 2, 4> K;
-  K << 132.284914985, 5.45575813704, 227.613831364, 6.12234622766, 132.284914985, 5.45575813704, -227.613831364, -6.12234622766;
+  K << 320.979606093, 21.0129517955, 884.233784759, 36.3637782119, 320.979606095, 21.0129517956, -884.233784749, -36.3637782119;
   Eigen::Matrix<double, 4, 4> A_inv;
-  A_inv << 1.0, -0.00664481600894, 0.0, 0.0, 0.0, 1.72319658398, 0.0, 0.0, 0.0, 0.0, 0.996283279443, -0.00663803879795, 0.0, 0.0, 1.63337568847, 1.71874225747;
+  A_inv << 1.0, -0.00521526561559, 0.0, 0.0, 0.0, 1.08732457517, 0.0, 0.0, 0.0, 0.0, 0.999513354044, -0.00521444313273, 0.0, 0.0, 0.197397150694, 1.08682415753;
   return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeArmPlantCoefficients());
 }
 
diff --git a/frc971/control_loops/fridge/fridge.cc b/frc971/control_loops/fridge/fridge.cc
index 9452637..46d4f28 100644
--- a/frc971/control_loops/fridge/fridge.cc
+++ b/frc971/control_loops/fridge/fridge.cc
@@ -6,7 +6,7 @@
 #include "aos/common/logging/logging.h"
 
 #include "frc971/control_loops/fridge/elevator_motor_plant.h"
-#include "frc971/control_loops/fridge/arm_motor_plant.h"
+#include "frc971/control_loops/fridge/integral_arm_plant.h"
 #include "frc971/control_loops/voltage_cap/voltage_cap.h"
 #include "frc971/zeroing/zeroing.h"
 
@@ -24,32 +24,35 @@
 }  // namespace
 
 
-void CappedStateFeedbackLoop::CapU() {
-  VoltageCap(max_voltage_, U(0, 0), U(1, 0), &mutable_U(0, 0),
-             &mutable_U(1, 0));
+template <int S>
+void CappedStateFeedbackLoop<S>::CapU() {
+  VoltageCap(max_voltage_, this->U(0, 0), this->U(1, 0), &this->mutable_U(0, 0),
+             &this->mutable_U(1, 0));
 }
 
+template <int S>
 Eigen::Matrix<double, 2, 1>
-CappedStateFeedbackLoop::UnsaturateOutputGoalChange() {
+CappedStateFeedbackLoop<S>::UnsaturateOutputGoalChange() {
   // Compute the K matrix used to compensate for position errors.
   Eigen::Matrix<double, 2, 2> Kp;
   Kp.setZero();
-  Kp.col(0) = K().col(0);
-  Kp.col(1) = K().col(2);
+  Kp.col(0) = this->K().col(0);
+  Kp.col(1) = this->K().col(2);
 
   Eigen::Matrix<double, 2, 2> Kp_inv = Kp.inverse();
 
   // Compute how much we need to change R in order to achieve the change in U
   // that was observed.
-  Eigen::Matrix<double, 2, 1> deltaR = -Kp_inv * (U_uncapped() - U());
+  Eigen::Matrix<double, 2, 1> deltaR =
+      -Kp_inv * (this->U_uncapped() - this->U());
   return deltaR;
 }
 
 Fridge::Fridge(control_loops::FridgeQueue *fridge)
     : aos::controls::ControlLoop<control_loops::FridgeQueue>(fridge),
-      arm_loop_(new CappedStateFeedbackLoop(
-          StateFeedbackLoop<4, 2, 2>(MakeArmLoop()))),
-      elevator_loop_(new CappedStateFeedbackLoop(
+      arm_loop_(new CappedStateFeedbackLoop<5>(
+          StateFeedbackLoop<5, 2, 2>(MakeIntegralArmLoop()))),
+      elevator_loop_(new CappedStateFeedbackLoop<4>(
           StateFeedbackLoop<4, 2, 2>(MakeElevatorLoop()))),
       left_arm_estimator_(constants::GetValues().fridge.left_arm_zeroing),
       right_arm_estimator_(constants::GetValues().fridge.right_arm_zeroing),
@@ -461,7 +464,7 @@
   }
 
   // Set the goals.
-  arm_loop_->mutable_R() << arm_goal_, arm_goal_velocity, 0.0, 0.0;
+  arm_loop_->mutable_R() << arm_goal_, arm_goal_velocity, 0.0, 0.0, 0.0;
   elevator_loop_->mutable_R() << elevator_goal_, elevator_goal_velocity, 0.0,
       0.0;
 
diff --git a/frc971/control_loops/fridge/fridge.gyp b/frc971/control_loops/fridge/fridge.gyp
index 1226692..dc25b63 100644
--- a/frc971/control_loops/fridge/fridge.gyp
+++ b/frc971/control_loops/fridge/fridge.gyp
@@ -24,7 +24,7 @@
       'type': 'static_library',
       'sources': [
         'fridge.cc',
-        'arm_motor_plant.cc',
+        'integral_arm_plant.cc',
         'elevator_motor_plant.cc',
       ],
       'dependencies': [
@@ -45,6 +45,7 @@
       'type': 'executable',
       'sources': [
         'fridge_lib_test.cc',
+        'arm_motor_plant.cc',
       ],
       'dependencies': [
         '<(EXTERNALS):gtest',
diff --git a/frc971/control_loops/fridge/fridge.h b/frc971/control_loops/fridge/fridge.h
index fc10594..e9cbaa5 100644
--- a/frc971/control_loops/fridge/fridge.h
+++ b/frc971/control_loops/fridge/fridge.h
@@ -6,8 +6,6 @@
 #include "aos/common/controls/control_loop.h"
 #include "frc971/control_loops/state_feedback_loop.h"
 #include "frc971/control_loops/fridge/fridge.q.h"
-#include "frc971/control_loops/fridge/arm_motor_plant.h"
-#include "frc971/control_loops/fridge/elevator_motor_plant.h"
 #include "frc971/zeroing/zeroing.h"
 
 namespace frc971 {
@@ -19,15 +17,16 @@
 class FridgeTest_ArmGoalNegativeWindupTest_Test;
 class FridgeTest_ElevatorGoalNegativeWindupTest_Test;
 class FridgeTest_SafeArmZeroing_Test;
-}
+}  // namespace testing
 
-class CappedStateFeedbackLoop : public StateFeedbackLoop<4, 2, 2> {
+template<int S>
+class CappedStateFeedbackLoop : public StateFeedbackLoop<S, 2, 2> {
  public:
-  CappedStateFeedbackLoop(StateFeedbackLoop<4, 2, 2> &&loop)
-      : StateFeedbackLoop<4, 2, 2>(::std::move(loop)), max_voltage_(12.0) {}
+  CappedStateFeedbackLoop(StateFeedbackLoop<S, 2, 2> &&loop)
+      : StateFeedbackLoop<S, 2, 2>(::std::move(loop)), max_voltage_(12.0) {}
 
   void set_max_voltage(double max_voltage) {
-    max_voltage_ = ::std::max(-12.0, ::std::min(12.0, max_voltage));
+    max_voltage_ = ::std::max(0.0, ::std::min(12.0, max_voltage));
   }
 
   void CapU() override;
@@ -114,8 +113,8 @@
   void Correct();
 
   // The state feedback control loop or loops to talk to.
-  ::std::unique_ptr<CappedStateFeedbackLoop> arm_loop_;
-  ::std::unique_ptr<CappedStateFeedbackLoop> elevator_loop_;
+  ::std::unique_ptr<CappedStateFeedbackLoop<5>> arm_loop_;
+  ::std::unique_ptr<CappedStateFeedbackLoop<4>> elevator_loop_;
 
   zeroing::ZeroingEstimator left_arm_estimator_;
   zeroing::ZeroingEstimator right_arm_estimator_;
diff --git a/frc971/control_loops/fridge/fridge_lib_test.cc b/frc971/control_loops/fridge/fridge_lib_test.cc
index 1dff2ef..db25b08 100644
--- a/frc971/control_loops/fridge/fridge_lib_test.cc
+++ b/frc971/control_loops/fridge/fridge_lib_test.cc
@@ -8,8 +8,11 @@
 #include "gtest/gtest.h"
 #include "aos/common/queue.h"
 #include "aos/common/time.h"
+#include "aos/common/commonmath.h"
 #include "aos/common/controls/control_loop_test.h"
 #include "frc971/control_loops/position_sensor_sim.h"
+#include "frc971/control_loops/fridge/arm_motor_plant.h"
+#include "frc971/control_loops/fridge/elevator_motor_plant.h"
 #include "frc971/control_loops/fridge/fridge.q.h"
 #include "frc971/constants.h"
 #include "frc971/control_loops/team_number_test_environment.h"
@@ -116,13 +119,25 @@
     position.Send();
   }
 
+  // Sets the difference between the commanded and applied power for the arm.
+  // This lets us test that the integrator for the arm works.
+  void set_arm_power_error(double arm_power_error) {
+    arm_power_error_ = arm_power_error;
+  }
   // Simulates for a single timestep.
   void Simulate() {
     EXPECT_TRUE(fridge_queue_.output.FetchLatest());
 
     // Feed voltages into physics simulation.
-    arm_plant_->mutable_U() << fridge_queue_.output->left_arm,
-        fridge_queue_.output->right_arm;
+    if (arm_power_error_ != 0.0) {
+      arm_plant_->mutable_U() << ::aos::Clip(
+          fridge_queue_.output->left_arm + arm_power_error_, -12, 12),
+          ::aos::Clip(fridge_queue_.output->right_arm + arm_power_error_, -12,
+                      12);
+    } else {
+      arm_plant_->mutable_U() << fridge_queue_.output->left_arm,
+          fridge_queue_.output->right_arm;
+    }
     elevator_plant_->mutable_U() << fridge_queue_.output->left_elevator,
         fridge_queue_.output->right_elevator;
 
@@ -189,6 +204,7 @@
 
   double elevator_initial_difference_ = 0.0;
   double arm_initial_difference_ = 0.0;
+  double arm_power_error_ = 0.0;
 };
 
 class FridgeTest : public ::aos::testing::ControlLoopTest {
@@ -517,8 +533,11 @@
 TEST_F(FridgeTest, ArmGoalPositiveWindupTest) {
   fridge_queue_.goal.MakeWithBuilder().angle(0.03).height(0.45).Send();
 
+  int i = 0;
   while (fridge_.state() != Fridge::ZEROING_ARM) {
     RunIteration();
+    ++i;
+    ASSERT_LE(i, 10000);
   }
 
   // Kick it.
@@ -611,6 +630,20 @@
   }
 }
 
+// Tests that the arm integrator works.
+TEST_F(FridgeTest, ArmIntegratorTest) {
+  fridge_plant_.InitializeArmPosition(
+      (constants::GetValues().fridge.arm.lower_hard_limit +
+       constants::GetValues().fridge.arm.lower_hard_limit) /
+      2.0);
+  fridge_plant_.set_arm_power_error(1.0);
+  fridge_queue_.goal.MakeWithBuilder().angle(0.0).height(0.4).Send();
+
+  RunForTime(Time::InMS(8000));
+
+  VerifyNearGoal();
+}
+
 // Phil:
 // TODO(austin): Check that we e-stop if encoder index pulse is not n revolutions away from last one. (got extra counts from noise, etc).
 // TODO(austin): Check that we e-stop if pot disagrees too much with encoder after we are zeroed.
diff --git a/frc971/control_loops/fridge/integral_arm_plant.cc b/frc971/control_loops/fridge/integral_arm_plant.cc
new file mode 100644
index 0000000..a101044
--- /dev/null
+++ b/frc971/control_loops/fridge/integral_arm_plant.cc
@@ -0,0 +1,49 @@
+#include "frc971/control_loops/fridge/integral_arm_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<5, 2, 2> MakeIntegralArmPlantCoefficients() {
+  Eigen::Matrix<double, 5, 5> A;
+  A << 1.0, 0.00479642025454, 0.0, 0.0, 4.92993559969e-05, 0.0, 0.919688585028, 0.0, 0.0, 0.0194484035162, 0.0, 0.0, 0.999539771613, 0.00479566382645, 0.0, 0.0, 0.0, -0.18154390621, 0.919241022297, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0;
+  Eigen::Matrix<double, 5, 2> B;
+  B << 2.46496779984e-05, 2.46496779984e-05, 0.00972420175808, 0.00972420175808, 2.46477449538e-05, -2.46477449538e-05, 0.00972266818532, -0.00972266818532, 0.0, 0.0;
+  Eigen::Matrix<double, 2, 5> C;
+  C << 1.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, -1.0, 0.0, 0.0;
+  Eigen::Matrix<double, 2, 2> D;
+  D << 0, 0, 0, 0;
+  Eigen::Matrix<double, 2, 1> U_max;
+  U_max << 12.0, 12.0;
+  Eigen::Matrix<double, 2, 1> U_min;
+  U_min << -12.0, -12.0;
+  return StateFeedbackPlantCoefficients<5, 2, 2>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<5, 2, 2> MakeIntegralArmController() {
+  Eigen::Matrix<double, 5, 2> L;
+  L << 0.461805946837, 0.461805946837, 5.83483983392, 5.83483983392, 0.429725467802, -0.429725467802, 0.18044816586, -0.18044816586, 31.0623964848, 31.0623964848;
+  Eigen::Matrix<double, 2, 5> K;
+  K << 320.979606093, 21.0129517955, 884.233784759, 36.3637782119, 1.0, 320.979606095, 21.0129517956, -884.233784749, -36.3637782119, 1.0;
+  Eigen::Matrix<double, 5, 5> A_inv;
+  A_inv << 1.0, -0.00521526561559, 0.0, 0.0, 5.21292341391e-05, 0.0, 1.08732457517, 0.0, 0.0, -0.0211467270909, 0.0, 0.0, 0.999513354044, -0.00521444313273, 0.0, 0.0, 0.0, 0.197397150694, 1.08682415753, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0;
+  return StateFeedbackController<5, 2, 2>(L, K, A_inv, MakeIntegralArmPlantCoefficients());
+}
+
+StateFeedbackPlant<5, 2, 2> MakeIntegralArmPlant() {
+  ::std::vector< ::std::unique_ptr<StateFeedbackPlantCoefficients<5, 2, 2>>> plants(1);
+  plants[0] = ::std::unique_ptr<StateFeedbackPlantCoefficients<5, 2, 2>>(new StateFeedbackPlantCoefficients<5, 2, 2>(MakeIntegralArmPlantCoefficients()));
+  return StateFeedbackPlant<5, 2, 2>(&plants);
+}
+
+StateFeedbackLoop<5, 2, 2> MakeIntegralArmLoop() {
+  ::std::vector< ::std::unique_ptr<StateFeedbackController<5, 2, 2>>> controllers(1);
+  controllers[0] = ::std::unique_ptr<StateFeedbackController<5, 2, 2>>(new StateFeedbackController<5, 2, 2>(MakeIntegralArmController()));
+  return StateFeedbackLoop<5, 2, 2>(&controllers);
+}
+
+}  // namespace control_loops
+}  // namespace frc971
diff --git a/frc971/control_loops/fridge/integral_arm_plant.h b/frc971/control_loops/fridge/integral_arm_plant.h
new file mode 100644
index 0000000..f43f915
--- /dev/null
+++ b/frc971/control_loops/fridge/integral_arm_plant.h
@@ -0,0 +1,20 @@
+#ifndef FRC971_CONTROL_LOOPS_FRIDGE_INTEGRAL_ARM_PLANT_H_
+#define FRC971_CONTROL_LOOPS_FRIDGE_INTEGRAL_ARM_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<5, 2, 2> MakeIntegralArmPlantCoefficients();
+
+StateFeedbackController<5, 2, 2> MakeIntegralArmController();
+
+StateFeedbackPlant<5, 2, 2> MakeIntegralArmPlant();
+
+StateFeedbackLoop<5, 2, 2> MakeIntegralArmLoop();
+
+}  // namespace control_loops
+}  // namespace frc971
+
+#endif  // FRC971_CONTROL_LOOPS_FRIDGE_INTEGRAL_ARM_PLANT_H_
diff --git a/frc971/control_loops/python/arm.py b/frc971/control_loops/python/arm.py
index 812a0c3..e17990a 100755
--- a/frc971/control_loops/python/arm.py
+++ b/frc971/control_loops/python/arm.py
@@ -10,6 +10,7 @@
 import matplotlib
 from matplotlib import pylab
 
+
 class Arm(control_loop.ControlLoop):
   def __init__(self, name="Arm", mass=None):
     super(Arm, self).__init__(name)
@@ -52,19 +53,19 @@
     # Position difference is 1 - 2
     # Input is [Voltage 1, Voltage 2]
 
-    C1 = self.spring / (self.J * 0.5)
-    C2 = self.Kt * self.G / (self.J * 0.5 * self.R)
-    C3 = self.G * self.G * self.Kt / (self.R  * self.J * 0.5 * self.Kv)
+    self.C1 = self.spring / (self.J * 0.5)
+    self.C2 = self.Kt * self.G / (self.J * 0.5 * self.R)
+    self.C3 = self.G * self.G * self.Kt / (self.R  * self.J * 0.5 * self.Kv)
 
     self.A_continuous = numpy.matrix(
         [[0, 1, 0, 0],
-         [0, -C3, 0, 0],
+         [0, -self.C3, 0, 0],
          [0, 0, 0, 1],
-         [0, 0, -C1 * 2.0, -C3]])
+         [0, 0, -self.C1 * 2.0, -self.C3]])
 
-    print 'Full speed is', C2 / C3 * 12.0
+    print 'Full speed is', self.C2 / self.C3 * 12.0
 
-    print 'Stall arm difference is', 12.0 * C2 / C1
+    print 'Stall arm difference is', 12.0 * self.C2 / self.C1
     print 'Stall arm difference first principles is', self.stall_torque * self.G / self.spring
 
     print '5 degrees of arm error is', self.spring / self.r * (math.pi * 5.0 / 180.0)
@@ -72,9 +73,9 @@
     # Start with the unmodified input
     self.B_continuous = numpy.matrix(
         [[0, 0],
-         [C2 / 2.0, C2 / 2.0],
+         [self.C2 / 2.0, self.C2 / 2.0],
          [0, 0],
-         [C2 / 2.0, -C2 / 2.0]])
+         [self.C2 / 2.0, -self.C2 / 2.0]])
 
     self.C = numpy.matrix([[1, 0, 1, 0],
                            [1, 0, -1, 0]])
@@ -118,6 +119,65 @@
     self.U_max = numpy.matrix([[12.0], [12.0]])
     self.U_min = numpy.matrix([[-12.0], [-12.0]])
 
+    print 'Observer (Converted to a KF)', numpy.linalg.inv(self.A) * self.L
+
+    self.InitializeState()
+
+
+class IntegralArm(Arm):
+  def __init__(self, name="IntegralArm", mass=None):
+    super(IntegralArm, self).__init__(name=name, mass=mass)
+
+    self.A_continuous_unaugmented = self.A_continuous
+    self.A_continuous = numpy.matrix(numpy.zeros((5, 5)))
+    self.A_continuous[0:4, 0:4] = self.A_continuous_unaugmented
+    self.A_continuous[1, 4] = self.C2
+
+    # Start with the unmodified input
+    self.B_continuous_unaugmented = self.B_continuous
+    self.B_continuous = numpy.matrix(numpy.zeros((5, 2)))
+    self.B_continuous[0:4, 0:2] = self.B_continuous_unaugmented
+
+    self.C_unaugmented = self.C
+    self.C = numpy.matrix(numpy.zeros((2, 5)))
+    self.C[0:2, 0:4] = self.C_unaugmented
+
+    self.A, self.B = self.ContinuousToDiscrete(
+        self.A_continuous, self.B_continuous, self.dt)
+    print 'A cont', self.A_continuous
+    print 'B cont', self.B_continuous
+    print 'A discrete', self.A
+
+    q_pos = 0.08
+    q_vel = 0.40
+
+    q_pos_diff = 0.08
+    q_vel_diff = 0.40
+    q_voltage = 6.0
+    self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0, 0.0, 0.0],
+                           [0.0, (q_vel ** 2.0), 0.0, 0.0, 0.0],
+                           [0.0, 0.0, (q_pos_diff ** 2.0), 0.0, 0.0],
+                           [0.0, 0.0, 0.0, (q_vel_diff ** 2.0), 0.0],
+                           [0.0, 0.0, 0.0, 0.0, (q_voltage ** 2.0)]])
+
+    r_volts = 0.05
+    self.R = numpy.matrix([[(r_volts ** 2.0), 0.0],
+                           [0.0, (r_volts ** 2.0)]])
+
+    self.KalmanGain, self.Q_steady = controls.kalman(
+        A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+
+    self.U_max = numpy.matrix([[12.0], [12.0]])
+    self.U_min = numpy.matrix([[-12.0], [-12.0]])
+
+    self.K_unaugmented = self.K
+    self.K = numpy.matrix(numpy.zeros((2, 5)));
+    self.K[0:2, 0:4] = self.K_unaugmented
+    self.K[0, 4] = 1;
+    self.K[1, 4] = 1;
+    print 'Kal', self.KalmanGain
+    self.L = self.A * self.KalmanGain
+
     self.InitializeState()
 
 
@@ -225,31 +285,125 @@
     pylab.show()
 
 
+def run_integral_test(arm, initial_X, goal, observer_arm, disturbance,
+                      max_separation_error=0.01, show_graph=True,
+                      iterations=400):
+  """Runs the integral control arm plant with an initial condition and goal.
+
+    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 than max_separation_error.
+    Prints out something for a failure of either condition and returns
+    False if tests fail.
+    Args:
+      arm: arm object to use.
+      initial_X: starting state.
+      goal: goal state.
+      observer_arm: arm object to use for the observer.
+      show_graph: Whether or not to display a graph showing the changing
+           states and voltages.
+      iterations: Number of timesteps to run the model for.
+      disturbance: Voltage missmatch between controller and model.
+  """
+
+  arm.X = initial_X
+
+  # Various lists for graphing things.
+  t = []
+  x_avg = []
+  x_sep = []
+  x_hat_avg = []
+  x_hat_sep = []
+  v_avg = []
+  v_sep = []
+  u_left = []
+  u_right = []
+  u_error = []
+
+  sep_plot_gain = 100.0
+
+  unaugmented_goal = goal
+  goal = numpy.matrix(numpy.zeros((5, 1)))
+  goal[0:4, 0] = unaugmented_goal
+
+  for i in xrange(iterations):
+    X_hat = observer_arm.X_hat[0:4]
+
+    x_hat_avg.append(observer_arm.X_hat[0, 0])
+    x_hat_sep.append(observer_arm.X_hat[2, 0] * sep_plot_gain)
+
+    U = observer_arm.K * (goal - observer_arm.X_hat)
+    u_error.append(observer_arm.X_hat[4,0])
+    U = CapU(U)
+    x_avg.append(arm.X[0, 0])
+    v_avg.append(arm.X[1, 0])
+    x_sep.append(arm.X[2, 0] * sep_plot_gain)
+    v_sep.append(arm.X[3, 0])
+
+    observer_arm.PredictObserver(U)
+
+    arm.Update(U + disturbance)
+    observer_arm.Y = arm.Y
+    observer_arm.CorrectObserver(U)
+
+    t.append(i * arm.dt)
+    u_left.append(U[0, 0])
+    u_right.append(U[1, 0])
+
+  print 'End is', observer_arm.X_hat[4, 0]
+
+  if show_graph:
+    pylab.subplot(2, 1, 1)
+    pylab.plot(t, x_avg, label='x avg')
+    pylab.plot(t, x_sep, label='x sep')
+    if observer_arm is not None:
+      pylab.plot(t, x_hat_avg, label='x_hat avg')
+      pylab.plot(t, x_hat_sep, label='x_hat sep')
+    pylab.legend()
+
+    pylab.subplot(2, 1, 2)
+    pylab.plot(t, u_left, label='u left')
+    pylab.plot(t, u_right, label='u right')
+    pylab.plot(t, u_error, label='u error')
+    pylab.legend()
+    pylab.show()
+
+
 def main(argv):
   loaded_mass = 25
   #loaded_mass = 0
   arm = Arm(mass=13 + loaded_mass)
-  arm_controller = Arm(mass=13 + 15)
-  observer_arm = Arm(mass=13 + 15)
+  #arm_controller = Arm(mass=13 + 15)
+  #observer_arm = Arm(mass=13 + 15)
   #observer_arm = None
 
+  integral_arm = IntegralArm(mass=13 + loaded_mass)
+  integral_arm.X_hat[0, 0] += 0.02
+  integral_arm.X_hat[2, 0] += 0.02
+  integral_arm.X_hat[4] = 0
+
   # Test moving the arm with constant separation.
-  initial_X = numpy.matrix([[0.0], [0.0], [0.01], [0.0]])
-  #initial_X = numpy.matrix([[0.0], [0.0], [0.00], [0.0]])
-  R = numpy.matrix([[1.0], [0.0], [0.0], [0.0]])
-  run_test(arm, initial_X, R, controller_arm=arm_controller,
-           observer_arm=observer_arm)
+  initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
+  R = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
+  run_integral_test(arm, initial_X, R, integral_arm, disturbance=2)
 
   # Write the generated constants out to a file.
-  if len(argv) != 3:
-    print "Expected .h file name and .cc file name for the arm."
+  if len(argv) != 5:
+    print "Expected .h file name and .cc file name for the arm and augmented arm."
   else:
-    arm = Arm("Arm", 2)
+    arm = Arm("Arm", mass=13)
     loop_writer = control_loop.ControlLoopWriter("Arm", [arm])
     if argv[1][-3:] == '.cc':
       loop_writer.Write(argv[2], argv[1])
     else:
       loop_writer.Write(argv[1], argv[2])
 
+    integral_arm = IntegralArm("IntegralArm", mass=13)
+    loop_writer = control_loop.ControlLoopWriter("IntegralArm", [integral_arm])
+    if argv[3][-3:] == '.cc':
+      loop_writer.Write(argv[4], argv[3])
+    else:
+      loop_writer.Write(argv[3], argv[4])
+
 if __name__ == '__main__':
   sys.exit(main(sys.argv))
diff --git a/frc971/control_loops/update_arm.sh b/frc971/control_loops/update_arm.sh
index 40ef68a..c4c1ab9 100755
--- a/frc971/control_loops/update_arm.sh
+++ b/frc971/control_loops/update_arm.sh
@@ -5,4 +5,6 @@
 cd $(dirname $0)
 
 ./python/arm.py fridge/arm_motor_plant.cc \
-    fridge/arm_motor_plant.h
+    fridge/arm_motor_plant.h \
+    fridge/integral_arm_plant.cc \
+    fridge/integral_arm_plant.h