diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index ee1efdd..73fb9a5 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -37,6 +37,13 @@
 )
 
 cc_library(
+  name = 'gear',
+  hdrs = [
+    'gear.h',
+  ],
+)
+
+cc_library(
   name = 'ssdrivetrain',
   srcs = [
     'ssdrivetrain.cc',
@@ -47,15 +54,18 @@
   deps = [
     ':drivetrain_queue',
     ':drivetrain_config',
+    ':gear',
+    '//aos/common/controls:control_loop',
     '//aos/common/controls:polytope',
-    '//aos/common:math',
-    '//aos/common/messages:robot_state',
-    '//frc971/control_loops:state_feedback_loop',
-    '//frc971/control_loops:coerce_goal',
-    '//frc971:shifter_hall_effect',
-    '//aos/common/util:log_interval',
-    '//aos/common/logging:queue_logging',
     '//aos/common/logging:matrix_logging',
+    '//aos/common/logging:queue_logging',
+    '//aos/common/messages:robot_state',
+    '//aos/common/util:log_interval',
+    '//aos/common/util:trapezoid_profile',
+    '//aos/common:math',
+    '//frc971/control_loops:coerce_goal',
+    '//frc971/control_loops:state_feedback_loop',
+    '//frc971:shifter_hall_effect',
   ],
 )
 
@@ -70,6 +80,7 @@
   deps = [
     ':drivetrain_queue',
     ':drivetrain_config',
+    ':gear',
     '//aos/common/controls:polytope',
     '//aos/common:math',
     '//aos/common/messages:robot_state',
@@ -91,6 +102,7 @@
   ],
   deps = [
     ':drivetrain_queue',
+    ':gear',
     ':polydrivetrain',
     ':ssdrivetrain',
     '//aos/common/controls:control_loop',
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 12d2cbc..b4c04a3 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -31,30 +31,92 @@
       dt_config_(dt_config),
       kf_(dt_config_.make_kf_drivetrain_loop()),
       dt_openloop_(dt_config_, &kf_),
-      dt_closedloop_(dt_config_) {
+      dt_closedloop_(dt_config_, &kf_, &integrated_kf_heading_),
+      left_gear_(dt_config_.default_high_gear ? Gear::HIGH : Gear::LOW),
+      right_gear_(dt_config_.default_high_gear ? Gear::HIGH : Gear::LOW),
+      left_high_requested_(dt_config_.default_high_gear),
+      right_high_requested_(dt_config_.default_high_gear) {
   ::aos::controls::HPolytope<0>::Init();
 }
 
+int DrivetrainLoop::ControllerIndexFromGears() {
+  if (MaybeHigh(left_gear_)) {
+    if (MaybeHigh(right_gear_)) {
+      return 3;
+    } else {
+      return 2;
+    }
+  } else {
+    if (MaybeHigh(right_gear_)) {
+      return 1;
+    } else {
+      return 0;
+    }
+  }
+}
+
+Gear ComputeGear(double shifter_position,
+                 const constants::ShifterHallEffect &shifter_config,
+                 bool high_requested) {
+  if (shifter_position < shifter_config.clear_low) {
+    return Gear::LOW;
+  } else if (shifter_position > shifter_config.clear_high) {
+    return Gear::HIGH;
+  } else {
+    if (high_requested) {
+      return Gear::SHIFTING_UP;
+    } else {
+      return Gear::SHIFTING_DOWN;
+    }
+  }
+}
+
 void DrivetrainLoop::RunIteration(
     const ::frc971::control_loops::DrivetrainQueue::Goal *goal,
     const ::frc971::control_loops::DrivetrainQueue::Position *position,
     ::frc971::control_loops::DrivetrainQueue::Output *output,
     ::frc971::control_loops::DrivetrainQueue::Status *status) {
-  bool bad_pos = false;
-  if (position == nullptr) {
-    LOG_INTERVAL(no_position_);
-    bad_pos = true;
+  // TODO(austin): Put gear detection logic here.
+  switch (dt_config_.shifter_type) {
+    case ShifterType::SIMPLE_SHIFTER:
+      // Force the right controller for simple shifters since we assume that
+      // gear switching is instantaneous.
+      if (left_high_requested_) {
+        left_gear_ = Gear::HIGH;
+      } else {
+        left_gear_ = Gear::LOW;
+      }
+      if (right_high_requested_) {
+        right_gear_ = Gear::HIGH;
+      } else {
+        right_gear_ = Gear::LOW;
+      }
+      break;
+    case ShifterType::HALL_EFFECT_SHIFTER:
+      left_gear_ = ComputeGear(position->left_shifter_position,
+                               dt_config_.left_drive, left_high_requested_);
+      right_gear_ = ComputeGear(position->right_shifter_position,
+                                dt_config_.right_drive, right_high_requested_);
+      break;
   }
-  no_position_.Print();
 
-  kf_.set_controller_index(dt_openloop_.controller_index());
+  kf_.set_controller_index(ControllerIndexFromGears());
+  {
+    GearLogging gear_logging;
+    gear_logging.left_state = static_cast<uint32_t>(left_gear_);
+    gear_logging.right_state = static_cast<uint32_t>(right_gear_);
+    gear_logging.left_loop_high = MaybeHigh(left_gear_);
+    gear_logging.right_loop_high = MaybeHigh(right_gear_);
+    gear_logging.controller_index = kf_.controller_index();
+    LOG_STRUCT(DEBUG, "state", gear_logging);
+  }
 
-  bool gyro_valid = false;
+  // TODO(austin): Signal the current gear to both loops.
+
   if (gyro_reading.FetchLatest()) {
     LOG_STRUCT(DEBUG, "using", *gyro_reading.get());
     last_gyro_heading_ = gyro_reading->angle;
     last_gyro_rate_ = gyro_reading->velocity;
-    gyro_valid = true;
   }
 
   {
@@ -64,62 +126,69 @@
     integrated_kf_heading_ += dt_config_.dt *
                               (kf_.X_hat(3, 0) - kf_.X_hat(1, 0)) /
                               (dt_config_.robot_radius * 2.0);
+
+    // gyro_heading = (real_right - real_left) / width
+    // wheel_heading = (wheel_right - wheel_left) / width
+    // gyro_heading + offset = wheel_heading
+    // gyro_goal + offset = wheel_goal
+    // offset = wheel_heading - gyro_heading
+
+    // gyro_goal + wheel_heading - gyro_heading = wheel_goal
   }
 
+  dt_openloop_.SetPosition(position, left_gear_, right_gear_);
+
   bool control_loop_driving = false;
   if (goal) {
-    double wheel = goal->steering;
-    double throttle = goal->throttle;
-    bool quickturn = goal->quickturn;
-    bool highgear = goal->highgear;
-
     control_loop_driving = goal->control_loop_driving;
-    double left_goal = goal->left_goal;
-    double right_goal = goal->right_goal;
 
-    dt_closedloop_.SetGoal(left_goal, goal->left_velocity_goal, right_goal,
-                           goal->right_velocity_goal);
-    dt_openloop_.SetGoal(wheel, throttle, quickturn, highgear);
+    dt_closedloop_.SetGoal(*goal);
+    dt_openloop_.SetGoal(*goal);
   }
 
-  if (!bad_pos) {
-    const double left_encoder = position->left_encoder;
-    const double right_encoder = position->right_encoder;
-    if (gyro_valid) {
-      dt_closedloop_.SetPosition(left_encoder, right_encoder,
-                                 gyro_reading->angle);
-    } else {
-      dt_closedloop_.SetRawPosition(left_encoder, right_encoder);
-    }
-  }
-  dt_openloop_.SetPosition(position);
   dt_openloop_.Update();
 
   if (control_loop_driving) {
-    dt_closedloop_.Update(output == NULL, true);
-    dt_closedloop_.SendMotors(output);
+    dt_closedloop_.Update(output != NULL);
+    dt_closedloop_.SetOutput(output);
   } else {
-    dt_openloop_.SendMotors(output);
-    if (output) {
-      dt_closedloop_.SetExternalMotors(output->left_voltage,
-                                       output->right_voltage);
-    }
-    dt_closedloop_.Update(output == NULL, false);
+    dt_openloop_.SetOutput(output);
+    // TODO(austin): Set profile to current spot.
+    dt_closedloop_.Update(false);
   }
 
+  // The output should now contain the shift request.
+
   // set the output status of the control loop state
   if (status) {
-    status->robot_speed = dt_closedloop_.GetEstimatedRobotSpeed();
-    status->filtered_left_position = dt_closedloop_.GetEstimatedLeftEncoder();
-    status->filtered_right_position = dt_closedloop_.GetEstimatedRightEncoder();
+    status->robot_speed = (kf_.X_hat(1, 0) + kf_.X_hat(3, 0)) / 2.0;
 
-    status->filtered_left_velocity = dt_closedloop_.loop().X_hat(1, 0);
-    status->filtered_right_velocity = dt_closedloop_.loop().X_hat(3, 0);
-    status->output_was_capped = dt_closedloop_.OutputWasCapped();
-    status->uncapped_left_voltage = dt_closedloop_.loop().U_uncapped(0, 0);
-    status->uncapped_right_voltage = dt_closedloop_.loop().U_uncapped(1, 0);
+    Eigen::Matrix<double, 2, 1> linear =
+        dt_closedloop_.LeftRightToLinear(kf_.X_hat());
+    Eigen::Matrix<double, 2, 1> angular =
+        dt_closedloop_.LeftRightToAngular(kf_.X_hat());
+
+    angular(0, 0) = integrated_kf_heading_;
+
+    Eigen::Matrix<double, 4, 1> gyro_left_right =
+        dt_closedloop_.AngularLinearToLeftRight(linear, angular);
+
+    status->estimated_left_position = gyro_left_right(0, 0);
+    status->estimated_right_position = gyro_left_right(2, 0);
+
+    status->estimated_left_velocity = gyro_left_right(1, 0);
+    status->estimated_right_velocity = gyro_left_right(3, 0);
+    status->output_was_capped = dt_closedloop_.output_was_capped();
+    status->uncapped_left_voltage = kf_.U_uncapped(0, 0);
+    status->uncapped_right_voltage = kf_.U_uncapped(1, 0);
+
+    status->left_voltage_error = kf_.X_hat(4, 0);
+    status->right_voltage_error = kf_.X_hat(5, 0);
+    status->estimated_angular_velocity_error = kf_.X_hat(6, 0);
+    status->estimated_heading = integrated_kf_heading_;
 
     dt_openloop_.PopulateStatus(status);
+    dt_closedloop_.PopulateStatus(status);
   }
 
   double left_voltage = 0.0;
@@ -127,6 +196,8 @@
   if (output) {
     left_voltage = output->left_voltage;
     right_voltage = output->right_voltage;
+    left_high_requested_ = output->left_high;
+    right_high_requested_ = output->right_high;
   }
 
   const double scalar = ::aos::robot_state->voltage_battery / 12.0;
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index 30548be..a9541b7 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -3,14 +3,15 @@
 
 #include "Eigen/Dense"
 
-#include "aos/common/controls/polytope.h"
 #include "aos/common/controls/control_loop.h"
 #include "aos/common/controls/polytope.h"
+#include "aos/common/controls/polytope.h"
+#include "aos/common/util/log_interval.h"
 #include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/gear.h"
 #include "frc971/control_loops/drivetrain/polydrivetrain.h"
 #include "frc971/control_loops/drivetrain/ssdrivetrain.h"
-#include "frc971/control_loops/drivetrain/drivetrain_config.h"
-#include "aos/common/util/log_interval.h"
 
 namespace frc971 {
 namespace control_loops {
@@ -26,6 +27,8 @@
       ::frc971::control_loops::DrivetrainQueue *my_drivetrain =
           &::frc971::control_loops::drivetrain_queue);
 
+  int ControllerIndexFromGears();
+
  protected:
   // Executes one cycle of the control loop.
   void RunIteration(
@@ -36,9 +39,6 @@
 
   void Zero(::frc971::control_loops::DrivetrainQueue::Output *output) override;
 
-  typedef ::aos::util::SimpleLogInterval SimpleLogInterval;
-  SimpleLogInterval no_position_ = SimpleLogInterval(
-      ::aos::time::Time::InSeconds(0.25), WARNING, "no position");
   double last_gyro_heading_ = 0.0;
   double last_gyro_rate_ = 0.0;
 
@@ -48,10 +48,17 @@
   PolyDrivetrain dt_openloop_;
   DrivetrainMotorsSS dt_closedloop_;
 
+  // Current gears for each drive side.
+  Gear left_gear_;
+  Gear right_gear_;
+
   double last_left_voltage_ = 0;
   double last_right_voltage_ = 0;
 
   double integrated_kf_heading_ = 0;
+
+  bool left_high_requested_;
+  bool right_high_requested_;
 };
 
 }  // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index 24b1d38..3ef64bf 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -2,6 +2,14 @@
 
 import "aos/common/controls/control_loops.q";
 
+// Parameters for the motion profiles.
+struct ProfileParameters {
+  // Maximum velocity for the profile.
+  float max_velocity;
+  // Maximum acceleration for the profile.
+  float max_acceleration;
+};
+
 // For logging information about what the code is doing with the shifters.
 struct GearLogging {
   // Which controller is being used.
@@ -62,6 +70,11 @@
     // controller is active.
     double left_velocity_goal;
     double right_velocity_goal;
+
+    // Motion profile parameters.
+    // The control loop will profile if these are all non-zero.
+    ProfileParameters linear;
+    ProfileParameters angular;
   };
 
   message Position {
@@ -101,12 +114,12 @@
     double robot_speed;
 
     // Estimated relative position of each drivetrain side (in meters).
-    double filtered_left_position;
-    double filtered_right_position;
+    double estimated_left_position;
+    double estimated_right_position;
 
     // Estimated velocity of each drivetrain side (in m/s).
-    double filtered_left_velocity;
-    double filtered_right_velocity;
+    double estimated_left_velocity;
+    double estimated_right_velocity;
 
     // The voltage we wanted to send to each drivetrain side last cycle.
     double uncapped_left_voltage;
@@ -116,6 +129,23 @@
     double left_velocity_goal;
     double right_velocity_goal;
 
+    // The voltage error for the left and right sides.
+    double left_voltage_error;
+    double right_voltage_error;
+
+    // The profiled goal states.
+    double profiled_left_position_goal;
+    double profiled_right_position_goal;
+    double profiled_left_velocity_goal;
+    double profiled_right_velocity_goal;
+
+    // The KF offset
+    double estimated_angular_velocity_error;
+    // The KF estimated heading.
+    double estimated_heading;
+    // The KF wheel estimated heading.
+    //double estimated_wheel_heading;
+
     // True if the output voltage was capped last cycle.
     bool output_was_capped;
   };
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 4ceda53..f2c4638 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -2,28 +2,30 @@
 
 #include <memory>
 
-#include "gtest/gtest.h"
-#include "aos/common/network/team_number.h"
-#include "aos/common/controls/polytope.h"
 #include "aos/common/controls/control_loop_test.h"
+#include "aos/common/controls/polytope.h"
+#include "aos/common/network/team_number.h"
+#include "aos/common/time.h"
+#include "gtest/gtest.h"
 
-#include "y2016/constants.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/coerce_goal.h"
 #include "frc971/control_loops/drivetrain/drivetrain.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
 #include "frc971/control_loops/drivetrain/drivetrain_config.h"
 #include "frc971/control_loops/state_feedback_loop.h"
-#include "frc971/control_loops/coerce_goal.h"
-#include "y2016/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "y2016/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
-#include "y2016/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
-#include "y2016/constants.h"
 #include "frc971/queues/gyro.q.h"
+#include "y2016/constants.h"
+#include "y2016/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2016/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
+#include "y2016/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
 
 namespace frc971 {
 namespace control_loops {
 namespace drivetrain {
 namespace testing {
 
+using ::aos::time::Time;
+
 using ::y2016::control_loops::drivetrain::MakeDrivetrainPlant;
 
 // TODO(Comran): Make one that doesn't depend on the actual values for a
@@ -47,13 +49,38 @@
 
       ::y2016::control_loops::drivetrain::kHighGearRatio,
       ::y2016::control_loops::drivetrain::kLowGearRatio,
-      kThreeStateDriveShifter,
-      kThreeStateDriveShifter,
-      false};
+      kThreeStateDriveShifter, kThreeStateDriveShifter, false};
 
   return kDrivetrainConfig;
 };
 
+class DrivetrainPlant : public StateFeedbackPlant<4, 2, 2> {
+ public:
+  explicit DrivetrainPlant(StateFeedbackPlant<4, 2, 2> &&other)
+      : StateFeedbackPlant<4, 2, 2>(::std::move(other)) {}
+
+  void CheckU() override {
+    assert(U(0, 0) <= U_max(0, 0) + 0.00001 + left_voltage_offset_);
+    assert(U(0, 0) >= U_min(0, 0) - 0.00001 + left_voltage_offset_);
+    assert(U(1, 0) <= U_max(1, 0) + 0.00001 + right_voltage_offset_);
+    assert(U(1, 0) >= U_min(1, 0) - 0.00001 + right_voltage_offset_);
+  }
+
+  double left_voltage_offset() const { return left_voltage_offset_; }
+  void set_left_voltage_offset(double left_voltage_offset) {
+    left_voltage_offset_ = left_voltage_offset;
+  }
+
+  double right_voltage_offset() const { return right_voltage_offset_; }
+  void set_right_voltage_offset(double right_voltage_offset) {
+    right_voltage_offset_ = right_voltage_offset;
+  }
+
+ private:
+  double left_voltage_offset_ = 0.0;
+  double right_voltage_offset_ = 0.0;
+};
+
 // Class which simulates the drivetrain and sends out queue messages containing
 // the position.
 class DrivetrainSimulation {
@@ -62,13 +89,15 @@
   // TODO(aschuh) Do we want to test the clutch one too?
   DrivetrainSimulation()
       : drivetrain_plant_(
-            new StateFeedbackPlant<4, 2, 2>(MakeDrivetrainPlant())),
+            new DrivetrainPlant(MakeDrivetrainPlant())),
         my_drivetrain_queue_(".frc971.control_loops.drivetrain", 0x8a8dde77,
                              ".frc971.control_loops.drivetrain.goal",
                              ".frc971.control_loops.drivetrain.position",
                              ".frc971.control_loops.drivetrain.output",
-                             ".frc971.control_loops.drivetrain.status") {
+                             ".frc971.control_loops.drivetrain.status"),
+        gyro_reading_(::frc971::sensors::gyro_reading.name()) {
     Reinitialize();
+    last_U_.setZero();
   }
 
   // Resets the plant.
@@ -90,11 +119,27 @@
     const double left_encoder = GetLeftPosition();
     const double right_encoder = GetRightPosition();
 
-    ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Position>
-        position = my_drivetrain_queue_.position.MakeMessage();
-    position->left_encoder = left_encoder;
-    position->right_encoder = right_encoder;
-    position.Send();
+    {
+      ::aos::ScopedMessagePtr<
+          ::frc971::control_loops::DrivetrainQueue::Position> position =
+          my_drivetrain_queue_.position.MakeMessage();
+      position->left_encoder = left_encoder;
+      position->right_encoder = right_encoder;
+      position->left_shifter_position = left_gear_high_ ? 1.0 : 0.0;
+      position->right_shifter_position = right_gear_high_ ? 1.0 : 0.0;
+      position.Send();
+    }
+
+    {
+      ::aos::ScopedMessagePtr<::frc971::sensors::GyroReading> gyro =
+          gyro_reading_.MakeMessage();
+      gyro->angle = (right_encoder - left_encoder) /
+                    (::y2016::control_loops::drivetrain::kRobotRadius * 2.0);
+      gyro->velocity =
+          (drivetrain_plant_->X(3, 0) - drivetrain_plant_->X(1, 0)) /
+          (::y2016::control_loops::drivetrain::kRobotRadius * 2.0);
+      gyro.Send();
+    }
   }
 
   // Simulates the drivetrain moving for one timestep.
@@ -102,17 +147,57 @@
     last_left_position_ = drivetrain_plant_->Y(0, 0);
     last_right_position_ = drivetrain_plant_->Y(1, 0);
     EXPECT_TRUE(my_drivetrain_queue_.output.FetchLatest());
-    drivetrain_plant_->mutable_U() << my_drivetrain_queue_.output->left_voltage,
+    drivetrain_plant_->mutable_U() = last_U_;
+    last_U_ << my_drivetrain_queue_.output->left_voltage,
         my_drivetrain_queue_.output->right_voltage;
+    {
+      const double scalar = ::aos::robot_state->voltage_battery / 12.0;
+      last_U_ *= scalar;
+    }
+    left_gear_high_ = my_drivetrain_queue_.output->left_high;
+    right_gear_high_ = my_drivetrain_queue_.output->right_high;
+
+    if (left_gear_high_) {
+      if (right_gear_high_) {
+        drivetrain_plant_->set_plant_index(3);
+      } else {
+        drivetrain_plant_->set_plant_index(2);
+      }
+    } else {
+      if (right_gear_high_) {
+        drivetrain_plant_->set_plant_index(1);
+      } else {
+        drivetrain_plant_->set_plant_index(0);
+      }
+    }
+
+    drivetrain_plant_->mutable_U(0, 0) +=
+        drivetrain_plant_->left_voltage_offset();
+    drivetrain_plant_->mutable_U(1, 0) +=
+        drivetrain_plant_->right_voltage_offset();
     drivetrain_plant_->Update();
   }
 
-  ::std::unique_ptr<StateFeedbackPlant<4, 2, 2>> drivetrain_plant_;
+  void set_left_voltage_offset(double left_voltage_offset) {
+    drivetrain_plant_->set_left_voltage_offset(left_voltage_offset);
+  }
+  void set_right_voltage_offset(double right_voltage_offset) {
+    drivetrain_plant_->set_right_voltage_offset(right_voltage_offset);
+  }
+
+  ::std::unique_ptr<DrivetrainPlant> drivetrain_plant_;
 
  private:
   ::frc971::control_loops::DrivetrainQueue my_drivetrain_queue_;
+  ::aos::Queue<::frc971::sensors::GyroReading> gyro_reading_;
+
   double last_left_position_;
   double last_right_position_;
+
+  Eigen::Matrix<double, 2, 1> last_U_;
+
+  bool left_gear_high_ = false;
+  bool right_gear_high_ = false;
 };
 
 class DrivetrainTest : public ::aos::testing::ControlLoopTest {
@@ -135,15 +220,30 @@
         drivetrain_motor_(GetDrivetrainConfig(), &my_drivetrain_queue_),
         drivetrain_motor_plant_() {
     ::frc971::sensors::gyro_reading.Clear();
+    set_battery_voltage(12.0);
+  }
+
+  void RunIteration() {
+    drivetrain_motor_plant_.SendPositionMessage();
+    drivetrain_motor_.Iterate();
+    drivetrain_motor_plant_.Simulate();
+    SimulateTimestep(true);
+  }
+
+  void RunForTime(const Time run_for) {
+    const auto end_time = Time::Now() + run_for;
+    while (Time::Now() < end_time) {
+      RunIteration();
+    }
   }
 
   void VerifyNearGoal() {
     my_drivetrain_queue_.goal.FetchLatest();
     my_drivetrain_queue_.position.FetchLatest();
     EXPECT_NEAR(my_drivetrain_queue_.goal->left_goal,
-                drivetrain_motor_plant_.GetLeftPosition(), 1e-2);
+                drivetrain_motor_plant_.GetLeftPosition(), 1e-3);
     EXPECT_NEAR(my_drivetrain_queue_.goal->right_goal,
-                drivetrain_motor_plant_.GetRightPosition(), 1e-2);
+                drivetrain_motor_plant_.GetRightPosition(), 1e-3);
   }
 
   virtual ~DrivetrainTest() { ::frc971::sensors::gyro_reading.Clear(); }
@@ -156,12 +256,21 @@
       .left_goal(-1.0)
       .right_goal(1.0)
       .Send();
-  for (int i = 0; i < 200; ++i) {
-    drivetrain_motor_plant_.SendPositionMessage();
-    drivetrain_motor_.Iterate();
-    drivetrain_motor_plant_.Simulate();
-    SimulateTimestep(true);
-  }
+  RunForTime(Time::InSeconds(1.0));
+  VerifyNearGoal();
+}
+
+// Tests that the drivetrain converges on a goal when under the effect of a
+// voltage offset/disturbance.
+TEST_F(DrivetrainTest, ConvergesWithVoltageError) {
+  my_drivetrain_queue_.goal.MakeWithBuilder()
+      .control_loop_driving(true)
+      .left_goal(-1.0)
+      .right_goal(1.0)
+      .Send();
+  drivetrain_motor_plant_.set_left_voltage_offset(1.0);
+  drivetrain_motor_plant_.set_right_voltage_offset(1.0);
+  RunForTime(Time::InSeconds(1.5));
   VerifyNearGoal();
 }
 
@@ -197,12 +306,7 @@
 // Tests that never having a goal, but having driver's station messages, doesn't
 // break.
 TEST_F(DrivetrainTest, NoGoalWithRobotState) {
-  for (int i = 0; i < 20; ++i) {
-    drivetrain_motor_plant_.SendPositionMessage();
-    drivetrain_motor_.Iterate();
-    drivetrain_motor_plant_.Simulate();
-    SimulateTimestep(true);
-  }
+  RunForTime(Time::InSeconds(0.1));
 }
 
 // Tests that the robot successfully drives straight forward.
@@ -214,15 +318,12 @@
       .right_goal(4.0)
       .Send();
   for (int i = 0; i < 500; ++i) {
-    drivetrain_motor_plant_.SendPositionMessage();
-    drivetrain_motor_.Iterate();
-    drivetrain_motor_plant_.Simulate();
-    SimulateTimestep(true);
+    RunIteration();
     ASSERT_TRUE(my_drivetrain_queue_.output.FetchLatest());
-    EXPECT_FLOAT_EQ(my_drivetrain_queue_.output->left_voltage,
-                    my_drivetrain_queue_.output->right_voltage);
-    EXPECT_GT(my_drivetrain_queue_.output->left_voltage, -3);
-    EXPECT_GT(my_drivetrain_queue_.output->right_voltage, -3);
+    EXPECT_NEAR(my_drivetrain_queue_.output->left_voltage,
+                my_drivetrain_queue_.output->right_voltage, 1e-4);
+    EXPECT_GT(my_drivetrain_queue_.output->left_voltage, -11);
+    EXPECT_GT(my_drivetrain_queue_.output->right_voltage, -11);
   }
   VerifyNearGoal();
 }
@@ -236,13 +337,187 @@
       .right_goal(3.9)
       .Send();
   for (int i = 0; i < 500; ++i) {
+    RunIteration();
+    ASSERT_TRUE(my_drivetrain_queue_.output.FetchLatest());
+    EXPECT_GT(my_drivetrain_queue_.output->left_voltage, -11);
+    EXPECT_GT(my_drivetrain_queue_.output->right_voltage, -11);
+  }
+  VerifyNearGoal();
+}
+
+// Tests that converting from a left, right position to a distance, angle
+// coordinate system and back returns the same answer.
+TEST_F(DrivetrainTest, LinearToAngularAndBack) {
+  StateFeedbackLoop<7, 2, 3> kf(
+      GetDrivetrainConfig().make_kf_drivetrain_loop());
+  double kf_heading = 0;
+  DrivetrainMotorsSS drivetrain_ss(GetDrivetrainConfig(), &kf, &kf_heading);
+
+  const double width = GetDrivetrainConfig().robot_radius * 2.0;
+
+  Eigen::Matrix<double, 7, 1> state;
+  state << 2, 3, 4, 5, 0, 0, 0;
+  Eigen::Matrix<double, 2, 1> linear = drivetrain_ss.LeftRightToLinear(state);
+
+  EXPECT_NEAR(3.0, linear(0, 0), 1e-6);
+  EXPECT_NEAR(4.0, linear(1, 0), 1e-6);
+
+  Eigen::Matrix<double, 2, 1> angular = drivetrain_ss.LeftRightToAngular(state);
+
+  EXPECT_NEAR(2.0 / width, angular(0, 0), 1e-6);
+  EXPECT_NEAR(2.0 / width, angular(1, 0), 1e-6);
+
+  Eigen::Matrix<double, 4, 1> back_state =
+      drivetrain_ss.AngularLinearToLeftRight(linear, angular);
+
+  for (int i = 0; i < 4; ++i) {
+    EXPECT_NEAR(state(i, 0), back_state(i, 0), 1e-8);
+  }
+}
+
+// Tests that a linear motion profile succeeds.
+TEST_F(DrivetrainTest, ProfileStraightForward) {
+  {
+    ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
+        goal = my_drivetrain_queue_.goal.MakeMessage();
+    goal->control_loop_driving = true;
+    goal->left_goal = 4.0;
+    goal->right_goal = 4.0;
+    goal->left_velocity_goal = 0.0;
+    goal->right_velocity_goal = 0.0;
+    goal->linear.max_velocity = 1.0;
+    goal->linear.max_acceleration = 3.0;
+    goal->angular.max_velocity = 1.0;
+    goal->angular.max_acceleration = 3.0;
+    goal.Send();
+  }
+
+  while (Time::Now() < Time::InSeconds(6)) {
+    RunIteration();
+    ASSERT_TRUE(my_drivetrain_queue_.output.FetchLatest());
+    EXPECT_NEAR(my_drivetrain_queue_.output->left_voltage,
+                my_drivetrain_queue_.output->right_voltage, 1e-4);
+    EXPECT_GT(my_drivetrain_queue_.output->left_voltage, -6);
+    EXPECT_GT(my_drivetrain_queue_.output->right_voltage, -6);
+    EXPECT_LT(my_drivetrain_queue_.output->left_voltage, 6);
+    EXPECT_LT(my_drivetrain_queue_.output->right_voltage, 6);
+  }
+  VerifyNearGoal();
+}
+
+// Tests that an angular motion profile succeeds.
+TEST_F(DrivetrainTest, ProfileTurn) {
+  {
+    ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
+        goal = my_drivetrain_queue_.goal.MakeMessage();
+    goal->control_loop_driving = true;
+    goal->left_goal = -1.0;
+    goal->right_goal = 1.0;
+    goal->left_velocity_goal = 0.0;
+    goal->right_velocity_goal = 0.0;
+    goal->linear.max_velocity = 1.0;
+    goal->linear.max_acceleration = 3.0;
+    goal->angular.max_velocity = 1.0;
+    goal->angular.max_acceleration = 3.0;
+    goal.Send();
+  }
+
+  while (Time::Now() < Time::InSeconds(6)) {
+    RunIteration();
+    ASSERT_TRUE(my_drivetrain_queue_.output.FetchLatest());
+    EXPECT_NEAR(my_drivetrain_queue_.output->left_voltage,
+                -my_drivetrain_queue_.output->right_voltage, 1e-4);
+    EXPECT_GT(my_drivetrain_queue_.output->left_voltage, -6);
+    EXPECT_GT(my_drivetrain_queue_.output->right_voltage, -6);
+    EXPECT_LT(my_drivetrain_queue_.output->left_voltage, 6);
+    EXPECT_LT(my_drivetrain_queue_.output->right_voltage, 6);
+  }
+  VerifyNearGoal();
+}
+
+// Tests that a mixed turn drive saturated profile succeeds.
+TEST_F(DrivetrainTest, SaturatedTurnDrive) {
+  {
+    ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
+        goal = my_drivetrain_queue_.goal.MakeMessage();
+    goal->control_loop_driving = true;
+    goal->left_goal = 5.0;
+    goal->right_goal = 4.0;
+    goal->left_velocity_goal = 0.0;
+    goal->right_velocity_goal = 0.0;
+    goal->linear.max_velocity = 6.0;
+    goal->linear.max_acceleration = 4.0;
+    goal->angular.max_velocity = 2.0;
+    goal->angular.max_acceleration = 4.0;
+    goal.Send();
+  }
+
+  while (Time::Now() < Time::InSeconds(3)) {
+    RunIteration();
+    ASSERT_TRUE(my_drivetrain_queue_.output.FetchLatest());
+  }
+  VerifyNearGoal();
+}
+
+// Tests that being in teleop drive for a bit and then transitioning to closed
+// drive profiles nicely.
+TEST_F(DrivetrainTest, OpenLoopThenClosed) {
+  my_drivetrain_queue_.goal.MakeWithBuilder()
+      .control_loop_driving(false)
+      .steering(0.0)
+      .throttle(1.0)
+      .highgear(true)
+      .quickturn(false)
+      .Send();
+
+  RunForTime(Time::InSeconds(1.0));
+
+  my_drivetrain_queue_.goal.MakeWithBuilder()
+      .control_loop_driving(false)
+      .steering(0.0)
+      .throttle(-0.3)
+      .highgear(true)
+      .quickturn(false)
+      .Send();
+
+  RunForTime(Time::InSeconds(1.0));
+
+  my_drivetrain_queue_.goal.MakeWithBuilder()
+      .control_loop_driving(false)
+      .steering(0.0)
+      .throttle(0.0)
+      .highgear(true)
+      .quickturn(false)
+      .Send();
+
+  RunForTime(Time::InSeconds(10.0));
+
+  {
+    ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
+        goal = my_drivetrain_queue_.goal.MakeMessage();
+    goal->control_loop_driving = true;
+    goal->left_goal = 5.0;
+    goal->right_goal = 4.0;
+    goal->left_velocity_goal = 0.0;
+    goal->right_velocity_goal = 0.0;
+    goal->linear.max_velocity = 1.0;
+    goal->linear.max_acceleration = 2.0;
+    goal->angular.max_velocity = 1.0;
+    goal->angular.max_acceleration = 2.0;
+    goal.Send();
+  }
+
+  const auto end_time = Time::Now() + Time::InSeconds(4);
+  while (Time::Now() < end_time) {
     drivetrain_motor_plant_.SendPositionMessage();
     drivetrain_motor_.Iterate();
     drivetrain_motor_plant_.Simulate();
     SimulateTimestep(true);
     ASSERT_TRUE(my_drivetrain_queue_.output.FetchLatest());
-    EXPECT_GT(my_drivetrain_queue_.output->left_voltage, -3);
-    EXPECT_GT(my_drivetrain_queue_.output->right_voltage, -3);
+    EXPECT_GT(my_drivetrain_queue_.output->left_voltage, -6);
+    EXPECT_GT(my_drivetrain_queue_.output->right_voltage, -6);
+    EXPECT_LT(my_drivetrain_queue_.output->left_voltage, 6);
+    EXPECT_LT(my_drivetrain_queue_.output->right_voltage, 6);
   }
   VerifyNearGoal();
 }
@@ -349,6 +624,8 @@
   EXPECT_EQ(1.0, output(1, 0));
 }
 
+// TODO(austin): Make sure the profile reset code when we disable works.
+
 }  // namespace testing
 }  // namespace drivetrain
 }  // namespace control_loops
diff --git a/frc971/control_loops/drivetrain/gear.h b/frc971/control_loops/drivetrain/gear.h
new file mode 100644
index 0000000..8fabb69
--- /dev/null
+++ b/frc971/control_loops/drivetrain/gear.h
@@ -0,0 +1,25 @@
+#ifndef FRC971_CONTROL_LOOPS_GEAR_H_
+#define FRC971_CONTROL_LOOPS_GEAR_H_
+
+namespace frc971 {
+namespace control_loops {
+namespace drivetrain {
+
+// The state of the shift.
+enum class Gear { HIGH, LOW, SHIFTING_UP, SHIFTING_DOWN };
+
+// True if the the robot might or is trying to be in high gear.
+inline bool MaybeHigh(Gear g) {
+  return g == Gear::HIGH || g == Gear::SHIFTING_UP;
+}
+
+// True if the shifter is engaged and ready for action.
+inline bool IsInGear(Gear gear) {
+  return gear == Gear::LOW || gear == Gear::HIGH;
+}
+
+}  // namespace drivetrain
+}  // namespace control_loops
+}  // namespace frc971
+
+#endif  // FRC971_CONTROL_LOOPS_GEAR_H_
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.cc b/frc971/control_loops/drivetrain/polydrivetrain.cc
index f088bff..8925a40 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.cc
+++ b/frc971/control_loops/drivetrain/polydrivetrain.cc
@@ -36,10 +36,8 @@
       wheel_(0.0),
       throttle_(0.0),
       quickturn_(false),
-      stale_count_(0),
-      position_time_delta_(dt_config.dt),
-      left_gear_(LOW),
-      right_gear_(LOW),
+      left_gear_(Gear::LOW),
+      right_gear_(Gear::LOW),
       counter_(0),
       dt_config_(dt_config) {
   last_position_.Zero();
@@ -64,70 +62,60 @@
 
   // Not in gear, so speed-match to destination gear.
   switch (gear) {
-    case HIGH:
-    case SHIFTING_UP:
+    case Gear::HIGH:
+    case Gear::SHIFTING_UP:
       return high_gear_speed;
-    case LOW:
-    case SHIFTING_DOWN:
+    case Gear::LOW:
+    case Gear::SHIFTING_DOWN:
     default:
       return low_gear_speed;
       break;
   }
 }
 
-PolyDrivetrain::Gear PolyDrivetrain::UpdateSingleGear(Gear requested_gear,
-                                                      Gear current_gear) {
+Gear PolyDrivetrain::UpdateSingleGear(Gear requested_gear, Gear current_gear) {
   const Gear shift_up =
       (dt_config_.shifter_type == ShifterType::HALL_EFFECT_SHIFTER)
-          ? SHIFTING_UP
-          : HIGH;
+          ? Gear::SHIFTING_UP
+          : Gear::HIGH;
   const Gear shift_down =
       (dt_config_.shifter_type == ShifterType::HALL_EFFECT_SHIFTER)
-          ? SHIFTING_DOWN
-          : LOW;
+          ? Gear::SHIFTING_DOWN
+          : Gear::LOW;
   if (current_gear != requested_gear) {
     if (IsInGear(current_gear)) {
-      if (requested_gear == HIGH) {
-        if (current_gear != HIGH) {
+      if (requested_gear == Gear::HIGH) {
+        if (current_gear != Gear::HIGH) {
           current_gear = shift_up;
         }
       } else {
-        if (current_gear != LOW) {
+        if (current_gear != Gear::LOW) {
           current_gear = shift_down;
         }
       }
     } else {
-      if (requested_gear == HIGH && current_gear == SHIFTING_DOWN) {
-        current_gear = SHIFTING_UP;
-      } else if (requested_gear == LOW && current_gear == SHIFTING_UP) {
-        current_gear = SHIFTING_DOWN;
+      if (requested_gear == Gear::HIGH && current_gear == Gear::SHIFTING_DOWN) {
+        current_gear = Gear::SHIFTING_UP;
+      } else if (requested_gear == Gear::LOW &&
+                 current_gear == Gear::SHIFTING_UP) {
+        current_gear = Gear::SHIFTING_DOWN;
       }
     }
   }
   return current_gear;
 }
 
-void PolyDrivetrain::UpdateGears(Gear requested_gear) {
-  left_gear_ = UpdateSingleGear(requested_gear, left_gear_);
-  right_gear_ = UpdateSingleGear(requested_gear, right_gear_);
-}
+void PolyDrivetrain::SetGoal(
+    const ::frc971::control_loops::DrivetrainQueue::Goal &goal) {
+  const double wheel = goal.steering;
+  const double throttle = goal.throttle;
+  const bool quickturn = goal.quickturn;
+  const bool highgear = goal.highgear;
 
-void PolyDrivetrain::SetGoal(double wheel, double throttle, bool quickturn,
-                             bool highgear) {
   const double kWheelNonLinearity = 0.4;
   // Apply a sin function that's scaled to make it feel better.
   const double angular_range = M_PI_2 * kWheelNonLinearity;
 
-  if (dt_config_.shifter_type == ShifterType::SIMPLE_SHIFTER) {
-    // Force the right controller for simple shifters since we assume that
-    // gear switching is instantaneous.
-    if (highgear) {
-      loop_->set_controller_index(3);
-    } else {
-      loop_->set_controller_index(0);
-    }
-  }
-
   wheel_ = sin(angular_range * wheel) / sin(angular_range);
   wheel_ = sin(angular_range * wheel_) / sin(angular_range);
   wheel_ = 2.0 * wheel - wheel_;
@@ -142,76 +130,19 @@
         throttle);
   }
 
-  UpdateGears(highgear ? HIGH : LOW);
+  Gear requested_gear = highgear ? Gear::HIGH : Gear::LOW;
+
+  left_gear_ = PolyDrivetrain::UpdateSingleGear(requested_gear, left_gear_);
+  right_gear_ = PolyDrivetrain::UpdateSingleGear(requested_gear, right_gear_);
 }
 
 void PolyDrivetrain::SetPosition(
-    const ::frc971::control_loops::DrivetrainQueue::Position *position) {
-  if (position == NULL) {
-    ++stale_count_;
-  } else {
-    last_position_ = position_;
-    position_ = *position;
-    position_time_delta_ = (stale_count_ + 1) * dt_config_.dt;
-    stale_count_ = 0;
-  }
-
-  if (dt_config_.shifter_type == ShifterType::HALL_EFFECT_SHIFTER && position) {
-    GearLogging gear_logging;
-    // Switch to the correct controller.
-    const double left_middle_shifter_position =
-        (dt_config_.left_drive.clear_high + dt_config_.left_drive.clear_low) /
-        2.0;
-    const double right_middle_shifter_position =
-        (dt_config_.right_drive.clear_high + dt_config_.right_drive.clear_low) /
-        2.0;
-
-    if (position->left_shifter_position < left_middle_shifter_position ||
-        left_gear_ == LOW) {
-      if (position->right_shifter_position < right_middle_shifter_position ||
-          right_gear_ == LOW) {
-        gear_logging.left_loop_high = false;
-        gear_logging.right_loop_high = false;
-        loop_->set_controller_index(gear_logging.controller_index = 0);
-      } else {
-        gear_logging.left_loop_high = false;
-        gear_logging.right_loop_high = true;
-        loop_->set_controller_index(gear_logging.controller_index = 1);
-      }
-    } else {
-      if (position->right_shifter_position < right_middle_shifter_position ||
-          right_gear_ == LOW) {
-        gear_logging.left_loop_high = true;
-        gear_logging.right_loop_high = false;
-        loop_->set_controller_index(gear_logging.controller_index = 2);
-      } else {
-        gear_logging.left_loop_high = true;
-        gear_logging.right_loop_high = true;
-        loop_->set_controller_index(gear_logging.controller_index = 3);
-      }
-    }
-
-    if (position->left_shifter_position > dt_config_.left_drive.clear_high &&
-        left_gear_ == SHIFTING_UP) {
-      left_gear_ = HIGH;
-    }
-    if (position->left_shifter_position < dt_config_.left_drive.clear_low &&
-        left_gear_ == SHIFTING_DOWN) {
-      left_gear_ = LOW;
-    }
-    if (position->right_shifter_position > dt_config_.right_drive.clear_high &&
-        right_gear_ == SHIFTING_UP) {
-      right_gear_ = HIGH;
-    }
-    if (position->right_shifter_position < dt_config_.right_drive.clear_low &&
-        right_gear_ == SHIFTING_DOWN) {
-      right_gear_ = LOW;
-    }
-
-    gear_logging.left_state = left_gear_;
-    gear_logging.right_state = right_gear_;
-    LOG_STRUCT(DEBUG, "state", gear_logging);
-  }
+    const ::frc971::control_loops::DrivetrainQueue::Position *position,
+    Gear left_gear, Gear right_gear) {
+  left_gear_ = left_gear;
+  right_gear_ = right_gear;
+  last_position_ = position_;
+  position_ = *position;
 }
 
 double PolyDrivetrain::FilterVelocity(double throttle) const {
@@ -331,11 +262,10 @@
     }
   } else {
     const double current_left_velocity =
-        (position_.left_encoder - last_position_.left_encoder) /
-        position_time_delta_;
+        (position_.left_encoder - last_position_.left_encoder) / dt_config_.dt;
     const double current_right_velocity =
         (position_.right_encoder - last_position_.right_encoder) /
-        position_time_delta_;
+        dt_config_.dt;
     const double left_motor_speed =
         MotorSpeed(dt_config_.left_drive, position_.left_shifter_position,
                    current_left_velocity, left_gear_);
@@ -380,13 +310,13 @@
   }
 }
 
-void PolyDrivetrain::SendMotors(
+void PolyDrivetrain::SetOutput(
     ::frc971::control_loops::DrivetrainQueue::Output *output) {
   if (output != NULL) {
     output->left_voltage = loop_->U(0, 0);
     output->right_voltage = loop_->U(1, 0);
-    output->left_high = left_gear_ == HIGH || left_gear_ == SHIFTING_UP;
-    output->right_high = right_gear_ == HIGH || right_gear_ == SHIFTING_UP;
+    output->left_high = MaybeHigh(left_gear_);
+    output->right_high = MaybeHigh(right_gear_);
   }
 }
 
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.h b/frc971/control_loops/drivetrain/polydrivetrain.h
index 689e76f..1ea38a7 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.h
+++ b/frc971/control_loops/drivetrain/polydrivetrain.h
@@ -3,6 +3,7 @@
 
 #include "aos/common/controls/polytope.h"
 
+#include "frc971/control_loops/drivetrain/gear.h"
 #include "frc971/control_loops/drivetrain/drivetrain.q.h"
 #include "frc971/control_loops/state_feedback_loop.h"
 #include "frc971/control_loops/drivetrain/drivetrain_config.h"
@@ -13,32 +14,21 @@
 
 class PolyDrivetrain {
  public:
-  enum Gear { HIGH, LOW, SHIFTING_UP, SHIFTING_DOWN };
-
   PolyDrivetrain(const DrivetrainConfig &dt_config,
                  StateFeedbackLoop<7, 2, 3> *kf);
 
   int controller_index() const { return loop_->controller_index(); }
 
-  bool IsInGear(Gear gear) { return gear == LOW || gear == HIGH; }
-
   // Computes the speed of the motor given the hall effect position and the
   // speed of the robot.
   double MotorSpeed(const constants::ShifterHallEffect &hall_effect,
                     double shifter_position, double velocity, Gear gear);
 
-  // Computes the states of the shifters for the left and right drivetrain sides
-  // given a requested state.
-  void UpdateGears(Gear requested_gear);
-
-  // Computes the next state of a shifter given the current state and the
-  // requested state.
-  Gear UpdateSingleGear(Gear requested_gear, Gear current_gear);
-
-  void SetGoal(double wheel, double throttle, bool quickturn, bool highgear);
+  void SetGoal(const ::frc971::control_loops::DrivetrainQueue::Goal &goal);
 
   void SetPosition(
-      const ::frc971::control_loops::DrivetrainQueue::Position *position);
+      const ::frc971::control_loops::DrivetrainQueue::Position *position,
+      Gear left_gear, Gear right_gear);
 
   double FilterVelocity(double throttle) const;
 
@@ -46,10 +36,14 @@
 
   void Update();
 
-  void SendMotors(::frc971::control_loops::DrivetrainQueue::Output *output);
+  void SetOutput(::frc971::control_loops::DrivetrainQueue::Output *output);
 
   void PopulateStatus(::frc971::control_loops::DrivetrainQueue::Status *status);
 
+  // Computes the next state of a shifter given the current state and the
+  // requested state.
+  Gear UpdateSingleGear(Gear requested_gear, Gear current_gear);
+
  private:
   StateFeedbackLoop<7, 2, 3> *kf_;
 
@@ -61,10 +55,10 @@
   double wheel_;
   double throttle_;
   bool quickturn_;
-  int stale_count_;
-  double position_time_delta_;
+
   Gear left_gear_;
   Gear right_gear_;
+
   ::frc971::control_loops::DrivetrainQueue::Position last_position_;
   ::frc971::control_loops::DrivetrainQueue::Position position_;
   int counter_;
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.cc b/frc971/control_loops/drivetrain/ssdrivetrain.cc
index 19bbef1..c1db075 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.cc
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.cc
@@ -15,35 +15,31 @@
 
 using ::frc971::control_loops::DoCoerceGoal;
 
-DrivetrainMotorsSS::LimitedDrivetrainLoop::LimitedDrivetrainLoop(
-    StateFeedbackLoop<4, 2, 2> &&loop)
-    : StateFeedbackLoop<4, 2, 2>(::std::move(loop)),
-      U_poly_((Eigen::Matrix<double, 4, 2>() << 1, 0, -1, 0, 0, 1, 0, -1)
-                  .finished(),
-              (Eigen::Matrix<double, 4, 1>() << 12.0, 12.0, 12.0, 12.0)
-                  .finished()) {
-  ::aos::controls::HPolytope<0>::Init();
-  T_ << 1, 1, 1, -1;
-  T_inverse_ = T_.inverse();
+void DrivetrainMotorsSS::ScaleCapU(Eigen::Matrix<double, 2, 1> *U) {
+  output_was_capped_ =
+      ::std::abs((*U)(0, 0)) > 12.0 || ::std::abs((*U)(1, 0)) > 12.0;
+
+  if (output_was_capped_) {
+    *U *= 12.0 / kf_->U_uncapped().lpNorm<Eigen::Infinity>();
+  }
 }
 
 // This intentionally runs the U-capping code even when it's unnecessary to help
 // make it more deterministic. Only running it when one or both sides want
 // out-of-range voltages could lead to things like running out of CPU under
 // certain situations, which would be bad.
-void DrivetrainMotorsSS::LimitedDrivetrainLoop::CapU() {
-  output_was_capped_ = ::std::abs(U(0, 0)) > 12.0 || ::std::abs(U(1, 0)) > 12.0;
+void DrivetrainMotorsSS::PolyCapU(Eigen::Matrix<double, 2, 1> *U) {
+  output_was_capped_ =
+      ::std::abs((*U)(0, 0)) > 12.0 || ::std::abs((*U)(1, 0)) > 12.0;
 
-  const Eigen::Matrix<double, 4, 1> error = R() - X_hat();
+  const Eigen::Matrix<double, 7, 1> error = kf_->R() - kf_->X_hat();
 
-  LOG_MATRIX(DEBUG, "U at start", U());
-  LOG_MATRIX(DEBUG, "R at start", R());
-  LOG_MATRIX(DEBUG, "Xhat at start", X_hat());
+  LOG_MATRIX(DEBUG, "U_uncapped", *U);
 
   Eigen::Matrix<double, 2, 2> position_K;
-  position_K << K(0, 0), K(0, 2), K(1, 0), K(1, 2);
+  position_K << kf_->K(0, 0), kf_->K(0, 2), kf_->K(1, 0), kf_->K(1, 2);
   Eigen::Matrix<double, 2, 2> velocity_K;
-  velocity_K << K(0, 1), K(0, 3), K(1, 1), K(1, 3);
+  velocity_K << kf_->K(0, 1), kf_->K(0, 3), kf_->K(1, 1), kf_->K(1, 3);
 
   Eigen::Matrix<double, 2, 1> position_error;
   position_error << error(0, 0), error(2, 0);
@@ -53,8 +49,13 @@
   velocity_error << error(1, 0), error(3, 0);
   LOG_MATRIX(DEBUG, "error", error);
 
-  const ::aos::controls::HPolytope<2> pos_poly(U_poly_, position_K * T_,
-                                               -velocity_K * velocity_error);
+
+  Eigen::Matrix<double, 2, 1> U_integral;
+  U_integral << kf_->X_hat(4, 0), kf_->X_hat(5, 0);
+
+  const ::aos::controls::HPolytope<2> pos_poly(
+      U_poly_, position_K * T_,
+      -velocity_K * velocity_error + U_integral - kf_->ff_U());
 
   Eigen::Matrix<double, 2, 1> adjusted_pos_error;
   {
@@ -99,86 +100,164 @@
     }
   }
 
-  mutable_U() =
-      velocity_K * velocity_error + position_K * T_ * adjusted_pos_error;
-  LOG_MATRIX(DEBUG, "U is now", U());
+  *U = -U_integral + velocity_K *velocity_error +
+       position_K *T_ *adjusted_pos_error + kf_->ff_U();
 
   if (!output_was_capped_) {
-    if ((U() - U_uncapped()).norm() > 0.0001) {
+    if ((*U - kf_->U_uncapped()).norm() > 0.0001) {
       LOG(FATAL, "U unnecessarily capped\n");
     }
   }
 }
 
-DrivetrainMotorsSS::DrivetrainMotorsSS(const DrivetrainConfig &dt_config)
-    : loop_(
-          new LimitedDrivetrainLoop(dt_config.make_drivetrain_loop())),
-      filtered_offset_(0.0),
-      gyro_(0.0),
-      left_goal_(0.0),
-      right_goal_(0.0),
-      raw_left_(0.0),
-      raw_right_(0.0),
-      dt_config_(dt_config) {
-  // High gear on both.
-  loop_->set_controller_index(3);
+DrivetrainMotorsSS::DrivetrainMotorsSS(const DrivetrainConfig &dt_config,
+                                       StateFeedbackLoop<7, 2, 3> *kf,
+                                       double *integrated_kf_heading)
+    : dt_config_(dt_config),
+      kf_(kf),
+      U_poly_(
+          (Eigen::Matrix<double, 4, 2>() << 1, 0, -1, 0, 0, 1, 0, -1)
+              .finished(),
+          (Eigen::Matrix<double, 4, 1>() << 12.0, 12.0, 12.0, 12.0).finished()),
+      linear_profile_(::aos::controls::kLoopFrequency),
+      angular_profile_(::aos::controls::kLoopFrequency),
+      integrated_kf_heading_(integrated_kf_heading) {
+  ::aos::controls::HPolytope<0>::Init();
+  T_ << 1, 1, 1, -1;
+  T_inverse_ = T_.inverse();
+  unprofiled_goal_.setZero();
 }
 
-void DrivetrainMotorsSS::SetGoal(double left, double left_velocity,
-                                 double right, double right_velocity) {
-  left_goal_ = left;
-  right_goal_ = right;
-  loop_->mutable_R() << left, left_velocity, right, right_velocity;
-}
-void DrivetrainMotorsSS::SetRawPosition(double left, double right) {
-  raw_right_ = right;
-  raw_left_ = left;
-  Eigen::Matrix<double, 2, 1> Y;
-  Y << left + filtered_offset_, right - filtered_offset_;
-  loop_->Correct(Y);
-}
-void DrivetrainMotorsSS::SetPosition(double left, double right, double gyro) {
-  // Decay the offset quickly because this gyro is great.
-  const double offset = (right - left) / 2.0 - gyro * dt_config_.robot_radius;
-  filtered_offset_ = 0.25 * offset + 0.75 * filtered_offset_;
-  gyro_ = gyro;
-  SetRawPosition(left, right);
+void DrivetrainMotorsSS::SetGoal(
+    const ::frc971::control_loops::DrivetrainQueue::Goal &goal) {
+  unprofiled_goal_ << goal.left_goal, goal.left_velocity_goal, goal.right_goal,
+      goal.right_velocity_goal, 0.0, 0.0, 0.0;
+
+  use_profile_ =
+      !kf_->Kff().isZero(0) &&
+      (goal.linear.max_velocity != 0.0 && goal.linear.max_acceleration != 0.0 &&
+       goal.angular.max_velocity != 0.0 &&
+       goal.angular.max_acceleration != 0.0);
+  linear_profile_.set_maximum_velocity(goal.linear.max_velocity);
+  linear_profile_.set_maximum_acceleration(goal.linear.max_acceleration);
+  angular_profile_.set_maximum_velocity(goal.angular.max_velocity);
+  angular_profile_.set_maximum_acceleration(goal.angular.max_acceleration);
 }
 
-void DrivetrainMotorsSS::SetExternalMotors(double left_voltage,
-                                           double right_voltage) {
-  loop_->mutable_U() << left_voltage, right_voltage;
+// (left + right) / 2 = linear
+// (right - left) / width = angular
+
+Eigen::Matrix<double, 2, 1> DrivetrainMotorsSS::LeftRightToLinear(
+    const Eigen::Matrix<double, 7, 1> &left_right) {
+  Eigen::Matrix<double, 2, 1> linear;
+  linear << (left_right(0, 0) + left_right(2, 0)) / 2.0,
+      (left_right(1, 0) + left_right(3, 0)) / 2.0;
+  return linear;
 }
 
-void DrivetrainMotorsSS::Update(bool stop_motors, bool enable_control_loop) {
+Eigen::Matrix<double, 2, 1> DrivetrainMotorsSS::LeftRightToAngular(
+    const Eigen::Matrix<double, 7, 1> &left_right) {
+  Eigen::Matrix<double, 2, 1> angular;
+  angular << (left_right(2, 0) - left_right(0, 0)) /
+                 (dt_config_.robot_radius * 2.0),
+      (left_right(3, 0) - left_right(1, 0)) / (dt_config_.robot_radius * 2.0);
+  return angular;
+}
+
+Eigen::Matrix<double, 4, 1> DrivetrainMotorsSS::AngularLinearToLeftRight(
+    const Eigen::Matrix<double, 2, 1> &linear,
+    const Eigen::Matrix<double, 2, 1> &angular) {
+  Eigen::Matrix<double, 2, 1> scaled_angle = angular * dt_config_.robot_radius;
+  Eigen::Matrix<double, 4, 1> state;
+  state << linear(0, 0) - scaled_angle(0, 0), linear(1, 0) - scaled_angle(1, 0),
+      linear(0, 0) + scaled_angle(0, 0), linear(1, 0) + scaled_angle(1, 0);
+  return state;
+}
+
+void DrivetrainMotorsSS::Update(bool enable_control_loop) {
   if (enable_control_loop) {
-    loop_->Update(stop_motors);
-  } else {
-    if (stop_motors) {
-      loop_->mutable_U().setZero();
-      loop_->mutable_U_uncapped().setZero();
+    // Update profiles.
+    Eigen::Matrix<double, 2, 1> unprofiled_linear =
+      LeftRightToLinear(unprofiled_goal_);
+    Eigen::Matrix<double, 2, 1> unprofiled_angular =
+      LeftRightToAngular(unprofiled_goal_);
+
+    Eigen::Matrix<double, 2, 1> wheel_heading =
+        LeftRightToAngular(kf_->X_hat());
+
+    Eigen::Matrix<double, 2, 1> next_linear;
+    Eigen::Matrix<double, 2, 1> next_angular;
+
+    if (use_profile_) {
+      next_linear = linear_profile_.Update(unprofiled_linear(0, 0),
+                                           unprofiled_linear(1, 0));
+      next_angular = angular_profile_.Update(unprofiled_angular(0, 0),
+                                             unprofiled_angular(1, 0));
+
+    } else {
+      next_angular = unprofiled_angular;
+      next_linear = unprofiled_linear;
     }
-    loop_->UpdateObserver(loop_->U());
+
+    next_angular(0, 0) += wheel_heading(0, 0) - *integrated_kf_heading_;
+
+    kf_->mutable_next_R().block<4, 1>(0, 0) =
+        AngularLinearToLeftRight(next_linear, next_angular);
+
+    kf_->mutable_next_R().block<3, 1>(4, 0) =
+        unprofiled_goal_.block<3, 1>(4, 0);
+
+    if (!use_profile_) {
+      kf_->mutable_R() = kf_->next_R();
+    }
+
+    // Run the controller.
+    Eigen::Matrix<double, 2, 1> U = kf_->ControllerOutput();
+
+    kf_->mutable_U_uncapped() = kf_->mutable_U() = U;
+    ScaleCapU(&kf_->mutable_U());
+
+    // Now update the feed forwards.
+    kf_->UpdateFFReference();
+
+    // Now, move the profile if things didn't go perfectly.
+    if (use_profile_ &&
+        (kf_->U() - kf_->U_uncapped()).lpNorm<Eigen::Infinity>() > 1e-4) {
+      linear_profile_.MoveCurrentState(LeftRightToLinear(kf_->R()));
+      angular_profile_.MoveCurrentState(LeftRightToAngular(kf_->R()));
+    }
+  } else {
+    unprofiled_goal_(0, 0) = kf_->X_hat()(0, 0);
+    unprofiled_goal_(1, 0) = 0.0;
+    unprofiled_goal_(2, 0) = kf_->X_hat()(2, 0);
+    unprofiled_goal_(3, 0) = 0.0;
+
+    linear_profile_.MoveCurrentState(LeftRightToLinear(unprofiled_goal_));
+    angular_profile_.MoveCurrentState(LeftRightToAngular(unprofiled_goal_));
+
+    kf_->mutable_next_R() = unprofiled_goal_;
+    kf_->mutable_R() = unprofiled_goal_;
   }
-  ::Eigen::Matrix<double, 4, 1> E = loop_->R() - loop_->X_hat();
-  LOG_MATRIX(DEBUG, "E", E);
 }
 
-double DrivetrainMotorsSS::GetEstimatedRobotSpeed() const {
-  // lets just call the average of left and right velocities close enough
-  return (loop_->X_hat(1, 0) + loop_->X_hat(3, 0)) / 2;
-}
-
-void DrivetrainMotorsSS::SendMotors(
+void DrivetrainMotorsSS::SetOutput(
     ::frc971::control_loops::DrivetrainQueue::Output *output) const {
   if (output) {
-    output->left_voltage = loop_->U(0, 0);
-    output->right_voltage = loop_->U(1, 0);
+    output->left_voltage = kf_->U(0, 0);
+    output->right_voltage = kf_->U(1, 0);
     output->left_high = true;
     output->right_high = true;
   }
 }
 
+void DrivetrainMotorsSS::PopulateStatus(
+    ::frc971::control_loops::DrivetrainQueue::Status *status) const {
+  status->profiled_left_position_goal = kf_->next_R(0, 0);
+  status->profiled_left_velocity_goal = kf_->next_R(1, 0);
+  status->profiled_right_position_goal = kf_->next_R(2, 0);
+  status->profiled_right_velocity_goal = kf_->next_R(3, 0);
+}
+
 }  // namespace drivetrain
 }  // namespace control_loops
 }  // namespace frc971
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.h b/frc971/control_loops/drivetrain/ssdrivetrain.h
index 193dbb2..419b401 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.h
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.h
@@ -1,9 +1,11 @@
 #ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_SSDRIVETRAIN_H_
 #define FRC971_CONTROL_LOOPS_DRIVETRAIN_SSDRIVETRAIN_H_
 
-#include "aos/common/controls/polytope.h"
 #include "aos/common/commonmath.h"
+#include "aos/common/controls/control_loop.h"
+#include "aos/common/controls/polytope.h"
 #include "aos/common/logging/matrix_logging.h"
+#include "aos/common/util/trapezoid_profile.h"
 
 #include "frc971/control_loops/state_feedback_loop.h"
 #include "frc971/control_loops/coerce_goal.h"
@@ -16,74 +18,62 @@
 
 class DrivetrainMotorsSS {
  public:
-  class LimitedDrivetrainLoop : public StateFeedbackLoop<4, 2, 2> {
-   public:
-    LimitedDrivetrainLoop(StateFeedbackLoop<4, 2, 2> &&loop);
+  DrivetrainMotorsSS(const DrivetrainConfig &dt_config,
+                     StateFeedbackLoop<7, 2, 3> *kf,
+                     double *integrated_kf_heading);
 
-    bool output_was_capped() const {
-      return output_was_capped_;
-    }
+  void SetGoal(const ::frc971::control_loops::DrivetrainQueue::Goal &goal);
 
-   private:
-    void CapU() override;
+  // Computes the power to send out as part of the controller.  Should be called
+  // when disabled (with enable_control_loop false) so the profiles get computed
+  // correctly.
+  // enable_control_loop includes the actual enable bit and if the loop will go
+  // out to hw.
+  void Update(bool enable_control_loop);
 
-    // Reprsents +/- full power on each motor in U-space, aka the square from
-    // (-12, -12) to (12, 12).
-    const ::aos::controls::HPolytope<2> U_poly_;
+  bool output_was_capped() const { return output_was_capped_; }
 
-    // multiplying by T converts [left_error, right_error] to
-    // [left_right_error_difference, total_distance_error].
-    Eigen::Matrix<double, 2, 2> T_, T_inverse_;
-
-    bool output_was_capped_ = false;
-  };
-
-  DrivetrainMotorsSS(const DrivetrainConfig &dt_config);
-
-  void SetGoal(double left, double left_velocity, double right,
-               double right_velocity);
-
-  void SetRawPosition(double left, double right);
-
-  void SetPosition(double left, double right, double gyro);
-
-  void SetExternalMotors(double left_voltage, double right_voltage);
-
-  void Update(bool stop_motors, bool enable_control_loop);
-
-  double GetEstimatedRobotSpeed() const;
-
-  double GetEstimatedLeftEncoder() const {
-    return loop_->X_hat(0, 0);
-  }
-
-  double left_velocity() const { return loop_->X_hat(1, 0); }
-  double right_velocity() const { return loop_->X_hat(3, 0); }
-
-  double GetEstimatedRightEncoder() const {
-    return loop_->X_hat(2, 0);
-  }
-
-  bool OutputWasCapped() const {
-    return loop_->output_was_capped();
-  }
-
-  void SendMotors(
+  void SetOutput(
       ::frc971::control_loops::DrivetrainQueue::Output *output) const;
+  void PopulateStatus(
+      ::frc971::control_loops::DrivetrainQueue::Status *status) const;
 
-  const LimitedDrivetrainLoop &loop() const { return *loop_; }
+  // Converts the robot state to a linear distance position, velocity.
+  Eigen::Matrix<double, 2, 1> LeftRightToLinear(
+      const Eigen::Matrix<double, 7, 1> &left_right);
+  // Converts the robot state to an anglular distance, velocity.
+  Eigen::Matrix<double, 2, 1> LeftRightToAngular(
+      const Eigen::Matrix<double, 7, 1> &left_right);
+
+  // Converts the linear and angular position, velocity to the top 4 states of
+  // the robot state.
+  Eigen::Matrix<double, 4, 1> AngularLinearToLeftRight(
+      const Eigen::Matrix<double, 2, 1> &linear,
+      const Eigen::Matrix<double, 2, 1> &angular);
 
  private:
-  ::std::unique_ptr<LimitedDrivetrainLoop> loop_;
-
-  double filtered_offset_;
-  double gyro_;
-  double left_goal_;
-  double right_goal_;
-  double raw_left_;
-  double raw_right_;
+  void PolyCapU(Eigen::Matrix<double, 2, 1> *U);
+  void ScaleCapU(Eigen::Matrix<double, 2, 1> *U);
 
   const DrivetrainConfig dt_config_;
+  StateFeedbackLoop<7, 2, 3> *kf_;
+  Eigen::Matrix<double, 7, 1> unprofiled_goal_;
+
+  // Reprsents +/- full power on each motor in U-space, aka the square from
+  // (-12, -12) to (12, 12).
+  const ::aos::controls::HPolytope<2> U_poly_;
+
+  // multiplying by T converts [left_error, right_error] to
+  // [left_right_error_difference, total_distance_error].
+  Eigen::Matrix<double, 2, 2> T_, T_inverse_;
+
+  aos::util::TrapezoidProfile linear_profile_, angular_profile_;
+
+  bool output_was_capped_ = false;
+
+  bool use_profile_ = false;
+
+  double *integrated_kf_heading_;
 };
 
 }  // namespace drivetrain