diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 915f63d..90385e0 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -1,221 +1,256 @@
-package(default_visibility = ['//visibility:public'])
+package(default_visibility = ["//visibility:public"])
 
-load('//aos/build:queues.bzl', 'queue_library')
+load("//aos/build:queues.bzl", "queue_library")
+load("//tools:environments.bzl", "mcu_cpus")
 
 cc_binary(
-  name = 'replay_drivetrain',
-  srcs = [
-    'replay_drivetrain.cc',
-  ],
-  deps = [
-    ':drivetrain_queue',
-    '//aos/common/controls:replay_control_loop',
-    '//aos/linux_code:init',
-    '//frc971/queues:gyro',
-  ],
+    name = "replay_drivetrain",
+    srcs = [
+        "replay_drivetrain.cc",
+    ],
+    deps = [
+        ":drivetrain_queue",
+        "//aos/common/controls:replay_control_loop",
+        "//aos/linux_code:init",
+        "//frc971/queues:gyro",
+    ],
 )
 
 queue_library(
-  name = 'drivetrain_queue',
-  srcs = [
-    'drivetrain.q',
-  ],
-  deps = [
-    '//aos/common/controls:control_loop_queues',
-    '//frc971/control_loops:queues',
-  ],
+    name = "drivetrain_queue",
+    srcs = [
+        "drivetrain.q",
+    ],
+    deps = [
+        "//aos/common/controls:control_loop_queues",
+        "//frc971/control_loops:queues",
+    ],
 )
 
 cc_library(
-  name = 'drivetrain_config',
-  hdrs = [
-    'drivetrain_config.h',
-  ],
-  deps = [
-    '//frc971/control_loops:state_feedback_loop',
-    '//frc971:shifter_hall_effect',
-  ],
+    name = "drivetrain_config",
+    hdrs = [
+        "drivetrain_config.h",
+    ],
+    deps = [
+        "//frc971:shifter_hall_effect",
+        "//frc971/control_loops:state_feedback_loop",
+    ],
 )
 
 cc_library(
-  name = 'gear',
-  hdrs = [
-    'gear.h',
-  ],
+    name = "gear",
+    hdrs = [
+        "gear.h",
+    ],
+    compatible_with = mcu_cpus,
 )
 
 cc_library(
-  name = 'ssdrivetrain',
-  srcs = [
-    'ssdrivetrain.cc',
-  ],
-  hdrs = [
-    'ssdrivetrain.h',
-  ],
-  deps = [
-    ':drivetrain_queue',
-    ':drivetrain_config',
-    ':gear',
-    '//aos/common/controls:control_loop',
-    '//aos/common/controls:polytope',
-    '//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',
-  ],
+    name = "ssdrivetrain",
+    srcs = [
+        "ssdrivetrain.cc",
+    ],
+    hdrs = [
+        "ssdrivetrain.h",
+    ],
+    deps = [
+        ":drivetrain_config",
+        ":drivetrain_queue",
+        ":gear",
+        "//aos/common:math",
+        "//aos/common/controls:control_loop",
+        "//aos/common/controls:polytope",
+        "//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",
+        "//frc971:shifter_hall_effect",
+        "//frc971/control_loops:coerce_goal",
+        "//frc971/control_loops:state_feedback_loop",
+    ],
 )
 
 cc_library(
-  name = 'polydrivetrain',
-  srcs = [
-    'polydrivetrain.cc',
-  ],
-  hdrs = [
-    'polydrivetrain.h',
-  ],
-  deps = [
-    ':drivetrain_queue',
-    ':drivetrain_config',
-    ':gear',
-    '//aos/common/controls:polytope',
-    '//aos/common:math',
-    '//aos/common/messages:robot_state',
-    '//frc971/control_loops:state_feedback_loop',
-    '//frc971/control_loops:coerce_goal',
-    '//aos/common/util:log_interval',
-    '//aos/common/logging:queue_logging',
-    '//aos/common/logging:matrix_logging',
-  ],
+    name = "polydrivetrain",
+    srcs = [
+        "polydrivetrain.cc",
+    ],
+    hdrs = [
+        "polydrivetrain.h",
+    ],
+    deps = [
+        ":drivetrain_config",
+        ":drivetrain_queue",
+        ":gear",
+        "//aos/common:math",
+        "//aos/common/controls:polytope",
+        "//aos/common/logging:matrix_logging",
+        "//aos/common/logging:queue_logging",
+        "//aos/common/messages:robot_state",
+        "//aos/common/util:log_interval",
+        "//frc971/control_loops:coerce_goal",
+        "//frc971/control_loops:state_feedback_loop",
+    ],
+)
+
+cc_library(
+    name = "drivetrain_config_uc",
+    hdrs = [
+        "drivetrain_config.h",
+    ],
+    restricted_to = mcu_cpus,
+    deps = [
+        "//frc971:shifter_hall_effect",
+        "//frc971/control_loops:state_feedback_loop_uc",
+    ],
+)
+
+cc_library(
+    name = "polydrivetrain_uc",
+    srcs = [
+        "drivetrain_uc.q.cc",
+        "polydrivetrain.cc",
+    ],
+    hdrs = [
+        "drivetrain_uc.q.h",
+        "polydrivetrain.h",
+    ],
+    restricted_to = mcu_cpus,
+    deps = [
+        ":drivetrain_config_uc",
+        ":gear",
+        "//aos/common:math",
+        "//aos/common/controls:polytope_uc",
+        "//frc971/control_loops:coerce_goal_uc",
+        "//frc971/control_loops:state_feedback_loop_uc",
+    ],
 )
 
 genrule(
-  name = 'genrule_down_estimator',
-  visibility = ['//visibility:private'],
-  cmd = '$(location //frc971/control_loops/python:down_estimator) $(OUTS)',
-  tools = [
-    '//frc971/control_loops/python:down_estimator',
-  ],
-  outs = [
-    'down_estimator.h',
-    'down_estimator.cc',
-  ],
+    name = "genrule_down_estimator",
+    outs = [
+        "down_estimator.h",
+        "down_estimator.cc",
+    ],
+    cmd = "$(location //frc971/control_loops/python:down_estimator) $(OUTS)",
+    tools = [
+        "//frc971/control_loops/python:down_estimator",
+    ],
+    visibility = ["//visibility:private"],
 )
 
 cc_library(
-  name = 'down_estimator',
-  hdrs = [
-    'down_estimator.h',
-  ],
-  srcs = [
-    'down_estimator.cc',
-  ],
-  deps = [
-    '//frc971/control_loops:state_feedback_loop',
-  ],
+    name = "down_estimator",
+    srcs = [
+        "down_estimator.cc",
+    ],
+    hdrs = [
+        "down_estimator.h",
+    ],
+    deps = [
+        "//frc971/control_loops:state_feedback_loop",
+    ],
 )
 
 cc_library(
-  name = 'drivetrain_lib',
-  srcs = [
-    'drivetrain.cc',
-  ],
-  hdrs = [
-    'drivetrain.h',
-  ],
-  deps = [
-    ':down_estimator',
-    ':drivetrain_queue',
-    ':gear',
-    ':polydrivetrain',
-    ':ssdrivetrain',
-    '//aos/common/controls:control_loop',
-    '//frc971/queues:gyro',
-    '//frc971/wpilib:imu_queue',
-    '//aos/common/util:log_interval',
-    '//aos/common/logging:queue_logging',
-    '//aos/common/logging:matrix_logging',
-  ],
+    name = "drivetrain_lib",
+    srcs = [
+        "drivetrain.cc",
+    ],
+    hdrs = [
+        "drivetrain.h",
+    ],
+    deps = [
+        ":down_estimator",
+        ":drivetrain_queue",
+        ":gear",
+        ":polydrivetrain",
+        ":ssdrivetrain",
+        "//aos/common/controls:control_loop",
+        "//aos/common/logging:matrix_logging",
+        "//aos/common/logging:queue_logging",
+        "//aos/common/util:log_interval",
+        "//frc971/queues:gyro",
+        "//frc971/wpilib:imu_queue",
+    ],
 )
 
 cc_test(
-  name = 'drivetrain_lib_test',
-  srcs = [
-    'drivetrain_lib_test.cc',
-  ],
-  deps = [
-    '//aos/testing:googletest',
-    ':drivetrain_queue',
-    ':drivetrain_lib',
-    ':drivetrain_config',
-    '//aos/common/controls:control_loop_test',
-    '//frc971/control_loops:state_feedback_loop',
-    '//frc971/queues:gyro',
-    '//aos/common:queues',
-    '//y2016:constants',
-    '//y2016/control_loops/drivetrain:polydrivetrain_plants',
-  ],
+    name = "drivetrain_lib_test",
+    srcs = [
+        "drivetrain_lib_test.cc",
+    ],
+    deps = [
+        ":drivetrain_config",
+        ":drivetrain_lib",
+        ":drivetrain_queue",
+        "//aos/common:queues",
+        "//aos/common/controls:control_loop_test",
+        "//aos/testing:googletest",
+        "//frc971/control_loops:state_feedback_loop",
+        "//frc971/queues:gyro",
+        "//y2016:constants",
+        "//y2016/control_loops/drivetrain:polydrivetrain_plants",
+    ],
 )
 
 genrule(
-  name = 'genrule_haptic_wheel',
-  visibility = ['//visibility:private'],
-  cmd = '$(location //frc971/control_loops/python:haptic_wheel) $(OUTS)',
-  tools = [
-    '//frc971/control_loops/python:haptic_wheel',
-  ],
-  outs = [
-    'haptic_wheel.h',
-    'haptic_wheel.cc',
-    'integral_haptic_wheel.h',
-    'integral_haptic_wheel.cc',
-    'haptic_trigger.h',
-    'haptic_trigger.cc',
-    'integral_haptic_trigger.h',
-    'integral_haptic_trigger.cc',
-  ],
-  restricted_to = ["//tools:k8", "//tools:roborio", "//tools:armhf-debian", "//tools:cortex-m4f"],
+    name = "genrule_haptic_wheel",
+    outs = [
+        "haptic_wheel.h",
+        "haptic_wheel.cc",
+        "integral_haptic_wheel.h",
+        "integral_haptic_wheel.cc",
+        "haptic_trigger.h",
+        "haptic_trigger.cc",
+        "integral_haptic_trigger.h",
+        "integral_haptic_trigger.cc",
+    ],
+    cmd = "$(location //frc971/control_loops/python:haptic_wheel) $(OUTS)",
+    compatible_with = mcu_cpus,
+    tools = [
+        "//frc971/control_loops/python:haptic_wheel",
+    ],
+    visibility = ["//visibility:private"],
 )
 
 cc_library(
-  name = 'haptic_input_uc',
-  hdrs = [
-    'haptic_wheel.h',
-    'integral_haptic_wheel.h',
-    'haptic_trigger.h',
-    'integral_haptic_trigger.h',
-  ],
-  srcs = [
-    'haptic_wheel.cc',
-    'integral_haptic_wheel.cc',
-    'haptic_trigger.cc',
-    'integral_haptic_trigger.cc',
-  ],
-  deps = [
-    '//frc971/control_loops:state_feedback_loop_uc',
-  ],
-  restricted_to = ["//tools:cortex-m4f"],
+    name = "haptic_input_uc",
+    srcs = [
+        "haptic_trigger.cc",
+        "haptic_wheel.cc",
+        "integral_haptic_trigger.cc",
+        "integral_haptic_wheel.cc",
+    ],
+    hdrs = [
+        "haptic_trigger.h",
+        "haptic_wheel.h",
+        "integral_haptic_trigger.h",
+        "integral_haptic_wheel.h",
+    ],
+    restricted_to = mcu_cpus,
+    deps = [
+        "//frc971/control_loops:state_feedback_loop_uc",
+    ],
 )
 
 cc_library(
-  name = 'haptic_wheel',
-  hdrs = [
-    'haptic_wheel.h',
-    'integral_haptic_wheel.h',
-    'haptic_trigger.h',
-    'integral_haptic_trigger.h',
-  ],
-  srcs = [
-    'haptic_wheel.cc',
-    'integral_haptic_wheel.cc',
-    'haptic_trigger.cc',
-    'integral_haptic_trigger.cc',
-  ],
-  deps = [
-    '//frc971/control_loops:state_feedback_loop',
-  ],
+    name = "haptic_wheel",
+    srcs = [
+        "haptic_trigger.cc",
+        "haptic_wheel.cc",
+        "integral_haptic_trigger.cc",
+        "integral_haptic_wheel.cc",
+    ],
+    hdrs = [
+        "haptic_trigger.h",
+        "haptic_wheel.h",
+        "integral_haptic_trigger.h",
+        "integral_haptic_wheel.h",
+    ],
+    deps = [
+        "//frc971/control_loops:state_feedback_loop",
+    ],
 )
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index b4a23c3..14c1a42 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -29,7 +29,7 @@
 namespace drivetrain {
 
 DrivetrainLoop::DrivetrainLoop(
-    const DrivetrainConfig &dt_config,
+    const DrivetrainConfig<double> &dt_config,
     ::frc971::control_loops::DrivetrainQueue *my_drivetrain)
     : aos::controls::ControlLoop<::frc971::control_loops::DrivetrainQueue>(
           my_drivetrain),
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index fd3f076..83e6c52 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -22,7 +22,7 @@
   // Constructs a control loop which can take a Drivetrain or defaults to the
   // drivetrain at frc971::control_loops::drivetrain
   explicit DrivetrainLoop(
-      const DrivetrainConfig &dt_config,
+      const DrivetrainConfig<double> &dt_config,
       ::frc971::control_loops::DrivetrainQueue *my_drivetrain =
           &::frc971::control_loops::drivetrain_queue);
 
@@ -40,10 +40,10 @@
 
   double last_gyro_rate_ = 0.0;
 
-  const DrivetrainConfig dt_config_;
+  const DrivetrainConfig<double> dt_config_;
 
   StateFeedbackLoop<7, 2, 4> kf_;
-  PolyDrivetrain dt_openloop_;
+  PolyDrivetrain<double> dt_openloop_;
   DrivetrainMotorsSS dt_closedloop_;
   ::aos::monotonic_clock::time_point last_gyro_time_ =
       ::aos::monotonic_clock::min_time;
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.h b/frc971/control_loops/drivetrain/drivetrain_config.h
index c2c876e..6e546a3 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.h
+++ b/frc971/control_loops/drivetrain/drivetrain_config.h
@@ -34,6 +34,7 @@
   IMU_Y = 1, // Use the y-axis of the IMU.
 };
 
+template <typename Scalar = double>
 struct DrivetrainConfig {
   // Shifting method we are using.
   ShifterType shifter_type;
@@ -48,18 +49,18 @@
   IMUType imu_type;
 
   // Polydrivetrain functions returning various controller loops with plants.
-  ::std::function<StateFeedbackLoop<4, 2, 2>()> make_drivetrain_loop;
-  ::std::function<StateFeedbackLoop<2, 2, 2>()> make_v_drivetrain_loop;
-  ::std::function<StateFeedbackLoop<7, 2, 4>()> make_kf_drivetrain_loop;
+  ::std::function<StateFeedbackLoop<4, 2, 2, Scalar>()> make_drivetrain_loop;
+  ::std::function<StateFeedbackLoop<2, 2, 2, Scalar>()> make_v_drivetrain_loop;
+  ::std::function<StateFeedbackLoop<7, 2, 4, Scalar>()> make_kf_drivetrain_loop;
 
-  double dt;            // Control loop time step.
-  double robot_radius;  // Robot radius, in meters.
-  double wheel_radius;  // Wheel radius, in meters.
-  double v;             // Motor velocity constant.
+  Scalar dt;            // Control loop time step.
+  Scalar robot_radius;  // Robot radius, in meters.
+  Scalar wheel_radius;  // Wheel radius, in meters.
+  Scalar v;             // Motor velocity constant.
 
   // Gear ratios, from wheel to motor shaft.
-  double high_gear_ratio;
-  double low_gear_ratio;
+  Scalar high_gear_ratio;
+  Scalar low_gear_ratio;
 
   // Hall effect constants. Unused if not applicable to shifter type.
   constants::ShifterHallEffect left_drive;
@@ -69,26 +70,26 @@
   // (ie. true means high gear is default).
   bool default_high_gear;
 
-  double down_offset;
+  Scalar down_offset;
 
-  double wheel_non_linearity;
+  Scalar wheel_non_linearity;
 
-  double quickturn_wheel_multiplier;
+  Scalar quickturn_wheel_multiplier;
 
-  double wheel_multiplier;
+  Scalar wheel_multiplier;
 
   // Converts the robot state to a linear distance position, velocity.
-  static Eigen::Matrix<double, 2, 1> LeftRightToLinear(
-      const Eigen::Matrix<double, 7, 1> &left_right) {
-    Eigen::Matrix<double, 2, 1> linear;
+  static Eigen::Matrix<Scalar, 2, 1> LeftRightToLinear(
+      const Eigen::Matrix<Scalar, 7, 1> &left_right) {
+    Eigen::Matrix<Scalar, 2, 1> linear;
     linear(0, 0) = (left_right(0, 0) + left_right(2, 0)) / 2.0;
     linear(1, 0) = (left_right(1, 0) + left_right(3, 0)) / 2.0;
     return linear;
   }
   // Converts the robot state to an anglular distance, velocity.
-  Eigen::Matrix<double, 2, 1> LeftRightToAngular(
-      const Eigen::Matrix<double, 7, 1> &left_right) const {
-    Eigen::Matrix<double, 2, 1> angular;
+  Eigen::Matrix<Scalar, 2, 1> LeftRightToAngular(
+      const Eigen::Matrix<Scalar, 7, 1> &left_right) const {
+    Eigen::Matrix<Scalar, 2, 1> angular;
     angular(0, 0) =
         (left_right(2, 0) - left_right(0, 0)) / (this->robot_radius * 2.0);
     angular(1, 0) =
@@ -98,12 +99,12 @@
 
   // 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) const {
-    Eigen::Matrix<double, 2, 1> scaled_angle =
+  Eigen::Matrix<Scalar, 4, 1> AngularLinearToLeftRight(
+      const Eigen::Matrix<Scalar, 2, 1> &linear,
+      const Eigen::Matrix<Scalar, 2, 1> &angular) const {
+    Eigen::Matrix<Scalar, 2, 1> scaled_angle =
         angular * this->robot_radius;
-    Eigen::Matrix<double, 4, 1> state;
+    Eigen::Matrix<Scalar, 4, 1> state;
     state(0, 0) = linear(0, 0) - scaled_angle(0, 0);
     state(1, 0) = linear(1, 0) - scaled_angle(1, 0);
     state(2, 0) = linear(0, 0) + scaled_angle(0, 0);
@@ -111,7 +112,6 @@
     return state;
   }
 };
-
 }  // namespace drivetrain
 }  // namespace control_loops
 }  // namespace frc971
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index a791be2..06b4f21 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -34,8 +34,8 @@
 const constants::ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.25,
                                                            0.75};
 
-const DrivetrainConfig &GetDrivetrainConfig() {
-  static DrivetrainConfig kDrivetrainConfig{
+const DrivetrainConfig<double> &GetDrivetrainConfig() {
+  static DrivetrainConfig<double> kDrivetrainConfig{
       ::frc971::control_loops::drivetrain::ShifterType::HALL_EFFECT_SHIFTER,
       ::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
       ::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO,
@@ -354,7 +354,7 @@
 // Tests that converting from a left, right position to a distance, angle
 // coordinate system and back returns the same answer.
 TEST_F(DrivetrainTest, LinearToAngularAndBack) {
-  const DrivetrainConfig dt_config = GetDrivetrainConfig();
+  const DrivetrainConfig<double> &dt_config = GetDrivetrainConfig();
   const double width = dt_config.robot_radius * 2.0;
 
   Eigen::Matrix<double, 7, 1> state;
@@ -560,7 +560,7 @@
   R << /*[[*/ 1.5, 1.5 /*]]*/;
 
   Eigen::Matrix<double, 2, 1> output =
-      ::frc971::control_loops::CoerceGoal(box, K, 0, R);
+      ::frc971::control_loops::CoerceGoal<double>(box, K, 0, R);
 
   EXPECT_EQ(R(0, 0), output(0, 0));
   EXPECT_EQ(R(1, 0), output(1, 0));
@@ -576,7 +576,7 @@
   R << 5, 5;
 
   Eigen::Matrix<double, 2, 1> output =
-      ::frc971::control_loops::CoerceGoal(box, K, 0, R);
+      ::frc971::control_loops::CoerceGoal<double>(box, K, 0, R);
 
   EXPECT_EQ(2.0, output(0, 0));
   EXPECT_EQ(2.0, output(1, 0));
@@ -592,7 +592,7 @@
   R << 5, 5;
 
   Eigen::Matrix<double, 2, 1> output =
-      ::frc971::control_loops::CoerceGoal(box, K, 0, R);
+      ::frc971::control_loops::CoerceGoal<double>(box, K, 0, R);
 
   EXPECT_EQ(3.0, output(0, 0));
   EXPECT_EQ(2.0, output(1, 0));
@@ -608,7 +608,7 @@
   R << 5, 5;
 
   Eigen::Matrix<double, 2, 1> output =
-      ::frc971::control_loops::CoerceGoal(box, K, 0, R);
+      ::frc971::control_loops::CoerceGoal<double>(box, K, 0, R);
 
   EXPECT_EQ(2.0, output(0, 0));
   EXPECT_EQ(2.0, output(1, 0));
@@ -624,7 +624,7 @@
   R << 5, 5;
 
   Eigen::Matrix<double, 2, 1> output =
-      ::frc971::control_loops::CoerceGoal(box, K, 0, R);
+      ::frc971::control_loops::CoerceGoal<double>(box, K, 0, R);
 
   EXPECT_EQ(1.0, output(0, 0));
   EXPECT_EQ(1.0, output(1, 0));
diff --git a/frc971/control_loops/drivetrain/drivetrain_uc.q.cc b/frc971/control_loops/drivetrain/drivetrain_uc.q.cc
new file mode 100644
index 0000000..aed0773
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain_uc.q.cc
@@ -0,0 +1,111 @@
+#include "frc971/control_loops/drivetrain/drivetrain_uc.q.h"
+#include <inttypes.h>
+
+namespace frc971 {
+namespace control_loops {
+
+GearLogging::GearLogging() { Zero(); }
+
+void GearLogging::Zero() {
+  controller_index = 0;
+  left_loop_high = false;
+  right_loop_high = false;
+  left_state = 0;
+  right_state = 0;
+}
+
+CIMLogging::CIMLogging() { Zero(); }
+
+void CIMLogging::Zero() {
+  left_in_gear = false;
+  right_in_gear = false;
+  left_motor_speed = 0.0f;
+  right_motor_speed = 0.0f;
+  left_velocity = 0.0f;
+  right_velocity = 0.0f;
+}
+
+void DrivetrainQueue_Goal::Zero() {
+  wheel = 0.0f;
+  wheel_velocity = 0.0f;
+  wheel_torque = 0.0f;
+  throttle = 0.0f;
+  throttle_velocity = 0.0f;
+  throttle_torque = 0.0f;
+  highgear = false;
+  quickturn = false;
+  control_loop_driving = false;
+  left_goal = 0.0f;
+  right_goal = 0.0f;
+  left_velocity_goal = 0.0f;
+  right_velocity_goal = 0.0f;
+  max_ss_voltage = 0.0f;
+  //linear.max_velocity = 0.0f;
+  //linear.max_acceleration = 0.0f;
+  //angular.max_velocity = 0.0f;
+  //angular.max_acceleration = 0.0f;
+}
+
+DrivetrainQueue_Goal::DrivetrainQueue_Goal() { Zero(); }
+
+void DrivetrainQueue_Position::Zero() {
+  left_encoder = 0.0f;
+  right_encoder = 0.0f;
+  left_speed = 0.0f;
+  right_speed = 0.0f;
+  left_shifter_position = 0.0f;
+  right_shifter_position = 0.0f;
+  low_left_hall = 0.0f;
+  high_left_hall = 0.0f;
+  low_right_hall = 0.0f;
+  high_right_hall = 0.0f;
+}
+
+DrivetrainQueue_Position::DrivetrainQueue_Position() { Zero(); }
+
+void DrivetrainQueue_Output::Zero() {
+  left_voltage = 0.0f;
+  right_voltage = 0.0f;
+  left_high = false;
+  right_high = false;
+}
+
+DrivetrainQueue_Output::DrivetrainQueue_Output() { Zero(); }
+
+void DrivetrainQueue_Status::Zero() {
+  robot_speed = 0.0f;
+  estimated_left_position = 0.0f;
+  estimated_right_position = 0.0f;
+  estimated_left_velocity = 0.0f;
+  estimated_right_velocity = 0.0f;
+  uncapped_left_voltage = 0.0f;
+  uncapped_right_voltage = 0.0f;
+  left_velocity_goal = 0.0f;
+  right_velocity_goal = 0.0f;
+  left_voltage_error = 0.0f;
+  right_voltage_error = 0.0f;
+  profiled_left_position_goal = 0.0f;
+  profiled_right_position_goal = 0.0f;
+  profiled_left_velocity_goal = 0.0f;
+  profiled_right_velocity_goal = 0.0f;
+  estimated_angular_velocity_error = 0.0f;
+  estimated_heading = 0.0f;
+  output_was_capped = false;
+  ground_angle = 0.0f;
+  gear_logging.controller_index = 0;
+  gear_logging.left_loop_high = false;
+  gear_logging.right_loop_high = false;
+  gear_logging.left_state = 0;
+  gear_logging.right_state = 0;
+  cim_logging.left_in_gear = false;
+  cim_logging.right_in_gear = false;
+  cim_logging.left_motor_speed = 0.0f;
+  cim_logging.right_motor_speed = 0.0f;
+  cim_logging.left_velocity = 0.0f;
+  cim_logging.right_velocity = 0.0f;
+}
+
+DrivetrainQueue_Status::DrivetrainQueue_Status() { Zero(); }
+
+}  // namespace control_loops
+}  // namespace frc971
diff --git a/frc971/control_loops/drivetrain/drivetrain_uc.q.h b/frc971/control_loops/drivetrain/drivetrain_uc.q.h
new file mode 100644
index 0000000..e93af26
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain_uc.q.h
@@ -0,0 +1,120 @@
+#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_Q_H_
+#define FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_Q_H_
+#include <array>
+#include "aos/common/macros.h"
+
+namespace frc971 {
+namespace control_loops {
+
+struct GearLogging {
+  GearLogging();
+  void Zero();
+
+  int8_t controller_index;
+  bool left_loop_high;
+  bool right_loop_high;
+  int8_t left_state;
+  int8_t right_state;
+};
+
+struct CIMLogging {
+  CIMLogging();
+  void Zero();
+
+  bool left_in_gear;
+  bool right_in_gear;
+  float left_motor_speed;
+  float right_motor_speed;
+  float left_velocity;
+  float right_velocity;
+};
+
+struct DrivetrainQueue_Goal {
+  DrivetrainQueue_Goal();
+  void Zero();
+
+  float wheel;
+  float wheel_velocity;
+  float wheel_torque;
+  float throttle;
+  float throttle_velocity;
+  float throttle_torque;
+  bool highgear;
+  bool quickturn;
+  bool control_loop_driving;
+  float left_goal;
+  float right_goal;
+  float left_velocity_goal;
+  float right_velocity_goal;
+  float max_ss_voltage;
+};
+
+struct DrivetrainQueue_Position {
+  DrivetrainQueue_Position();
+  void Zero();
+
+  float left_encoder;
+  float right_encoder;
+  float left_speed;
+  float right_speed;
+  float left_shifter_position;
+  float right_shifter_position;
+  float low_left_hall;
+  float high_left_hall;
+  float low_right_hall;
+  float high_right_hall;
+};
+
+struct DrivetrainQueue_Output {
+  DrivetrainQueue_Output();
+  void Zero();
+
+  float left_voltage;
+  float right_voltage;
+  bool left_high;
+  bool right_high;
+};
+
+struct DrivetrainQueue_Status {
+  DrivetrainQueue_Status();
+  void Zero();
+
+  float robot_speed;
+  float estimated_left_position;
+  float estimated_right_position;
+  float estimated_left_velocity;
+  float estimated_right_velocity;
+  float uncapped_left_voltage;
+  float uncapped_right_voltage;
+  float left_velocity_goal;
+  float right_velocity_goal;
+  float left_voltage_error;
+  float right_voltage_error;
+  float profiled_left_position_goal;
+  float profiled_right_position_goal;
+  float profiled_left_velocity_goal;
+  float profiled_right_velocity_goal;
+  float estimated_angular_velocity_error;
+  float estimated_heading;
+  bool output_was_capped;
+  float ground_angle;
+  ::frc971::control_loops::GearLogging gear_logging;
+  ::frc971::control_loops::CIMLogging cim_logging;
+};
+
+class DrivetrainQueue {
+ public:
+  typedef DrivetrainQueue_Goal Goal;
+  DrivetrainQueue_Goal goal;
+  typedef DrivetrainQueue_Position Position;
+  DrivetrainQueue_Position position;
+  typedef DrivetrainQueue_Output Output;
+  DrivetrainQueue_Output output;
+  typedef DrivetrainQueue_Status Status;
+  DrivetrainQueue_Status status;
+};
+
+}  // namespace control_loops
+}  // namespace frc971
+
+#endif  // FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_Q_H_
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.cc b/frc971/control_loops/drivetrain/polydrivetrain.cc
index db9f1c5..266acd5 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.cc
+++ b/frc971/control_loops/drivetrain/polydrivetrain.cc
@@ -2,340 +2,17 @@
 
 #include "aos/common/commonmath.h"
 #include "aos/common/controls/polytope.h"
-#include "aos/common/logging/logging.h"
-#include "aos/common/logging/matrix_logging.h"
-#include "aos/common/logging/queue_logging.h"
-
-#include "aos/common/messages/robot_state.q.h"
 #include "frc971/control_loops/coerce_goal.h"
+#ifdef __linux__
 #include "frc971/control_loops/drivetrain/drivetrain.q.h"
 #include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#endif  // __linux__
 #include "frc971/control_loops/state_feedback_loop.h"
 
 namespace frc971 {
 namespace control_loops {
 namespace drivetrain {
 
-PolyDrivetrain::PolyDrivetrain(const DrivetrainConfig &dt_config,
-                               StateFeedbackLoop<7, 2, 4> *kf)
-    : kf_(kf),
-      U_Poly_((Eigen::Matrix<double, 4, 2>() << /*[[*/ 1, 0 /*]*/,
-               /*[*/ -1, 0 /*]*/,
-               /*[*/ 0, 1 /*]*/,
-               /*[*/ 0, -1 /*]]*/)
-                  .finished(),
-              (Eigen::Matrix<double, 4, 1>() << /*[[*/ 12 /*]*/,
-               /*[*/ 12 /*]*/,
-               /*[*/ 12 /*]*/,
-               /*[*/ 12 /*]]*/)
-                  .finished(),
-              (Eigen::Matrix<double, 2, 4>() << /*[[*/ 12, 12, -12, -12 /*]*/,
-               /*[*/ -12, 12, 12, -12 /*]*/)
-                  .finished()),
-      loop_(new StateFeedbackLoop<2, 2, 2>(dt_config.make_v_drivetrain_loop())),
-      ttrust_(1.1),
-      wheel_(0.0),
-      throttle_(0.0),
-      quickturn_(false),
-      left_gear_(dt_config.default_high_gear ? Gear::HIGH : Gear::LOW),
-      right_gear_(dt_config.default_high_gear ? Gear::HIGH : Gear::LOW),
-      counter_(0),
-      dt_config_(dt_config) {
-  last_position_.Zero();
-  position_.Zero();
-}
-
-double PolyDrivetrain::MotorSpeed(
-    const constants::ShifterHallEffect &hall_effect, double shifter_position,
-    double velocity, Gear gear) {
-  const double high_gear_speed =
-      velocity / dt_config_.high_gear_ratio / dt_config_.wheel_radius;
-  const double low_gear_speed =
-      velocity / dt_config_.low_gear_ratio / dt_config_.wheel_radius;
-
-  if (shifter_position < hall_effect.clear_low) {
-    // We're in low gear, so return speed for that gear.
-    return low_gear_speed;
-  } else if (shifter_position > hall_effect.clear_high) {
-    // We're in high gear, so return speed for that gear.
-    return high_gear_speed;
-  }
-
-  // Not in gear, so speed-match to destination gear.
-  switch (gear) {
-    case Gear::HIGH:
-    case Gear::SHIFTING_UP:
-      return high_gear_speed;
-    case Gear::LOW:
-    case Gear::SHIFTING_DOWN:
-    default:
-      return low_gear_speed;
-      break;
-  }
-}
-
-Gear PolyDrivetrain::UpdateSingleGear(Gear requested_gear, Gear current_gear) {
-  const Gear shift_up =
-      (dt_config_.shifter_type == ShifterType::HALL_EFFECT_SHIFTER)
-          ? Gear::SHIFTING_UP
-          : Gear::HIGH;
-  const Gear shift_down =
-      (dt_config_.shifter_type == ShifterType::HALL_EFFECT_SHIFTER)
-          ? Gear::SHIFTING_DOWN
-          : Gear::LOW;
-  if (current_gear != requested_gear) {
-    if (IsInGear(current_gear)) {
-      if (requested_gear == Gear::HIGH) {
-        if (current_gear != Gear::HIGH) {
-          current_gear = shift_up;
-        }
-      } else {
-        if (current_gear != Gear::LOW) {
-          current_gear = shift_down;
-        }
-      }
-    } else {
-      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::SetGoal(
-    const ::frc971::control_loops::DrivetrainQueue::Goal &goal) {
-  const double wheel = goal.wheel;
-  const double throttle = goal.throttle;
-  const bool quickturn = goal.quickturn;
-  const bool highgear = goal.highgear;
-
-  // Apply a sin function that's scaled to make it feel better.
-  const double angular_range = M_PI_2 * dt_config_.wheel_non_linearity;
-
-  wheel_ = sin(angular_range * wheel) / sin(angular_range);
-  wheel_ = sin(angular_range * wheel_) / sin(angular_range);
-  wheel_ = 2.0 * wheel - wheel_;
-  quickturn_ = quickturn;
-
-  if (quickturn_) {
-    wheel_ *= dt_config_.quickturn_wheel_multiplier;
-  } else {
-    wheel_ *= dt_config_.wheel_multiplier;
-  }
-
-  static const double kThrottleDeadband = 0.05;
-  if (::std::abs(throttle) < kThrottleDeadband) {
-    throttle_ = 0;
-  } else {
-    throttle_ = copysign(
-        (::std::abs(throttle) - kThrottleDeadband) / (1.0 - kThrottleDeadband),
-        throttle);
-  }
-
-  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,
-    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 {
-  const Eigen::Matrix<double, 2, 2> FF =
-      loop_->plant().B().inverse() *
-      (Eigen::Matrix<double, 2, 2>::Identity() - loop_->plant().A());
-
-  constexpr int kHighGearController = 3;
-  const Eigen::Matrix<double, 2, 2> FF_high =
-      loop_->plant().coefficients(kHighGearController).B.inverse() *
-      (Eigen::Matrix<double, 2, 2>::Identity() -
-       loop_->plant().coefficients(kHighGearController).A);
-
-  ::Eigen::Matrix<double, 1, 2> FF_sum = FF.colwise().sum();
-  int min_FF_sum_index;
-  const double min_FF_sum = FF_sum.minCoeff(&min_FF_sum_index);
-  const double min_K_sum = loop_->controller().K().col(min_FF_sum_index).sum();
-  const double high_min_FF_sum = FF_high.col(0).sum();
-
-  const double adjusted_ff_voltage =
-      ::aos::Clip(throttle * 12.0 * min_FF_sum / high_min_FF_sum, -12.0, 12.0);
-  return (adjusted_ff_voltage +
-          ttrust_ * min_K_sum * (loop_->X_hat(0, 0) + loop_->X_hat(1, 0)) /
-              2.0) /
-         (ttrust_ * min_K_sum + min_FF_sum);
-}
-
-double PolyDrivetrain::MaxVelocity() {
-  const Eigen::Matrix<double, 2, 2> FF =
-      loop_->plant().B().inverse() *
-      (Eigen::Matrix<double, 2, 2>::Identity() - loop_->plant().A());
-
-  constexpr int kHighGearController = 3;
-  const Eigen::Matrix<double, 2, 2> FF_high =
-      loop_->plant().coefficients(kHighGearController).B.inverse() *
-      (Eigen::Matrix<double, 2, 2>::Identity() -
-       loop_->plant().coefficients(kHighGearController).A);
-
-  ::Eigen::Matrix<double, 1, 2> FF_sum = FF.colwise().sum();
-  int min_FF_sum_index;
-  const double min_FF_sum = FF_sum.minCoeff(&min_FF_sum_index);
-  // const double min_K_sum = loop_->K().col(min_FF_sum_index).sum();
-  const double high_min_FF_sum = FF_high.col(0).sum();
-
-  const double adjusted_ff_voltage =
-      ::aos::Clip(12.0 * min_FF_sum / high_min_FF_sum, -12.0, 12.0);
-  return adjusted_ff_voltage / min_FF_sum;
-}
-
-void PolyDrivetrain::Update() {
-  if (dt_config_.loop_type == LoopType::CLOSED_LOOP) {
-    loop_->mutable_X_hat()(0, 0) = kf_->X_hat()(1, 0);
-    loop_->mutable_X_hat()(1, 0) = kf_->X_hat()(3, 0);
-  }
-
-  // TODO(austin): Observer for the current velocity instead of difference
-  // calculations.
-  ++counter_;
-
-  if (IsInGear(left_gear_) && IsInGear(right_gear_)) {
-    // FF * X = U (steady state)
-    const Eigen::Matrix<double, 2, 2> FF =
-        loop_->plant().B().inverse() *
-        (Eigen::Matrix<double, 2, 2>::Identity() - loop_->plant().A());
-
-    // Invert the plant to figure out how the velocity filter would have to
-    // work
-    // out in order to filter out the forwards negative inertia.
-    // This math assumes that the left and right power and velocity are
-    // equals,
-    // and that the plant is the same on the left and right.
-    const double fvel = FilterVelocity(throttle_);
-
-    const double sign_svel = wheel_ * ((fvel > 0.0) ? 1.0 : -1.0);
-    double steering_velocity;
-    if (quickturn_) {
-      steering_velocity = wheel_ * MaxVelocity();
-    } else {
-      steering_velocity = ::std::abs(fvel) * wheel_;
-    }
-    const double left_velocity = fvel - steering_velocity;
-    const double right_velocity = fvel + steering_velocity;
-    goal_left_velocity_ = left_velocity;
-    goal_right_velocity_ = right_velocity;
-
-    // Integrate velocity to get the position.
-    // This position is used to get integral control.
-    loop_->mutable_R() << left_velocity, right_velocity;
-
-    if (!quickturn_) {
-      // K * R = w
-      Eigen::Matrix<double, 1, 2> equality_k;
-      equality_k << 1 + sign_svel, -(1 - sign_svel);
-      const double equality_w = 0.0;
-
-      // Construct a constraint on R by manipulating the constraint on U
-      ::aos::controls::HVPolytope<2, 4, 4> R_poly_hv(
-          U_Poly_.static_H() * (loop_->controller().K() + FF),
-          U_Poly_.static_k() +
-              U_Poly_.static_H() * loop_->controller().K() * loop_->X_hat(),
-          (loop_->controller().K() + FF).inverse() *
-              ::aos::controls::ShiftPoints<2, 4>(
-                  U_Poly_.StaticVertices(),
-                  loop_->controller().K() * loop_->X_hat()));
-
-      // Limit R back inside the box.
-      loop_->mutable_R() =
-          CoerceGoal(R_poly_hv, equality_k, equality_w, loop_->R());
-    }
-
-    const Eigen::Matrix<double, 2, 1> FF_volts = FF * loop_->R();
-    const Eigen::Matrix<double, 2, 1> U_ideal =
-        loop_->controller().K() * (loop_->R() - loop_->X_hat()) + FF_volts;
-
-    for (int i = 0; i < 2; i++) {
-      loop_->mutable_U()[i] = ::aos::Clip(U_ideal[i], -12, 12);
-    }
-
-    if (dt_config_.loop_type == LoopType::OPEN_LOOP) {
-      loop_->mutable_X_hat() =
-          loop_->plant().A() * loop_->X_hat() + loop_->plant().B() * loop_->U();
-    }
-
-    // Housekeeping: set the shifting logging values to zero, because we're not shifting
-    left_motor_speed_ = 0.0;
-    right_motor_speed_ = 0.0;
-    current_left_velocity_ = 0.0;
-    current_right_velocity_ = 0.0;
-  } else {
-    current_left_velocity_ =
-        (position_.left_encoder - last_position_.left_encoder) / dt_config_.dt;
-    current_right_velocity_ =
-        (position_.right_encoder - last_position_.right_encoder) /
-        dt_config_.dt;
-    left_motor_speed_ =
-        MotorSpeed(dt_config_.left_drive, position_.left_shifter_position,
-                   current_left_velocity_, left_gear_);
-    right_motor_speed_ =
-        MotorSpeed(dt_config_.right_drive, position_.right_shifter_position,
-                   current_right_velocity_, right_gear_);
-
-    goal_left_velocity_ = current_left_velocity_;
-    goal_right_velocity_ = current_right_velocity_;
-
-    // Any motor is not in gear. Speed match.
-    ::Eigen::Matrix<double, 1, 1> R_left;
-    ::Eigen::Matrix<double, 1, 1> R_right;
-    R_left(0, 0) = left_motor_speed_;
-    R_right(0, 0) = right_motor_speed_;
-
-    const double wiggle =
-        (static_cast<double>((counter_ % 30) / 15) - 0.5) * 8.0;
-
-    loop_->mutable_U(0, 0) = ::aos::Clip(
-        (R_left / dt_config_.v)(0, 0) + (IsInGear(left_gear_) ? 0 : wiggle),
-        -12.0, 12.0);
-    loop_->mutable_U(1, 0) = ::aos::Clip(
-        (R_right / dt_config_.v)(0, 0) + (IsInGear(right_gear_) ? 0 : wiggle),
-        -12.0, 12.0);
-    loop_->mutable_U() *= 12.0 / ::aos::robot_state->voltage_battery;
-  }
-}
-
-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 = MaybeHigh(left_gear_);
-    output->right_high = MaybeHigh(right_gear_);
-  }
-}
-
-void PolyDrivetrain::PopulateStatus(
-    ::frc971::control_loops::DrivetrainQueue::Status *status) {
-  status->left_velocity_goal = goal_left_velocity_;
-  status->right_velocity_goal = goal_right_velocity_;
-
-  status->cim_logging.left_in_gear = IsInGear(left_gear_);
-  status->cim_logging.left_motor_speed = left_motor_speed_;
-  status->cim_logging.left_velocity = current_left_velocity_;
-
-  status->cim_logging.right_in_gear = IsInGear(right_gear_);
-  status->cim_logging.right_motor_speed = right_motor_speed_;
-  status->cim_logging.right_velocity = current_right_velocity_;
-}
-
 }  // namespace drivetrain
 }  // namespace control_loops
 }  // namespace frc971
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.h b/frc971/control_loops/drivetrain/polydrivetrain.h
index c9b0714..d16e8a1 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.h
+++ b/frc971/control_loops/drivetrain/polydrivetrain.h
@@ -3,8 +3,18 @@
 
 #include "aos/common/controls/polytope.h"
 
+#include "aos/common/commonmath.h"
+#include "frc971/control_loops/coerce_goal.h"
 #include "frc971/control_loops/drivetrain/gear.h"
+#ifdef __linux__
 #include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/logging/matrix_logging.h"
+#include "aos/common/logging/queue_logging.h"
+#include "aos/common/messages/robot_state.q.h"
+#else
+#include "frc971/control_loops/drivetrain/drivetrain_uc.q.h"
+#endif  // __linux__
 #include "frc971/control_loops/state_feedback_loop.h"
 #include "frc971/control_loops/drivetrain/drivetrain_config.h"
 
@@ -12,17 +22,18 @@
 namespace control_loops {
 namespace drivetrain {
 
+template <typename Scalar = double>
 class PolyDrivetrain {
  public:
-  PolyDrivetrain(const DrivetrainConfig &dt_config,
-                 StateFeedbackLoop<7, 2, 4> *kf);
+  PolyDrivetrain(const DrivetrainConfig<Scalar> &dt_config,
+                 StateFeedbackLoop<7, 2, 4, Scalar> *kf);
 
   int controller_index() const { return loop_->index(); }
 
   // 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);
+  Scalar MotorSpeed(const constants::ShifterHallEffect &hall_effect,
+                    Scalar shifter_position, Scalar velocity, Gear gear);
 
   void SetGoal(const ::frc971::control_loops::DrivetrainQueue::Goal &goal);
 
@@ -30,9 +41,9 @@
       const ::frc971::control_loops::DrivetrainQueue::Position *position,
       Gear left_gear, Gear right_gear);
 
-  double FilterVelocity(double throttle) const;
+  Scalar FilterVelocity(Scalar throttle) const;
 
-  double MaxVelocity();
+  Scalar MaxVelocity();
 
   void Update();
 
@@ -44,16 +55,21 @@
   // requested state.
   Gear UpdateSingleGear(Gear requested_gear, Gear current_gear);
 
+  // Returns the current estimated velocity in m/s.
+  Scalar velocity() const {
+    return (loop_->mutable_X_hat()(0) + loop_->mutable_X_hat()(1)) / 2.0;
+  }
+
  private:
-  StateFeedbackLoop<7, 2, 4> *kf_;
+  StateFeedbackLoop<7, 2, 4, Scalar> *kf_;
 
-  const ::aos::controls::HVPolytope<2, 4, 4> U_Poly_;
+  const ::aos::controls::HVPolytope<2, 4, 4, Scalar> U_Poly_;
 
-  ::std::unique_ptr<StateFeedbackLoop<2, 2, 2>> loop_;
+  ::std::unique_ptr<StateFeedbackLoop<2, 2, 2, Scalar>> loop_;
 
-  const double ttrust_;
-  double wheel_;
-  double throttle_;
+  const Scalar ttrust_;
+  Scalar wheel_;
+  Scalar throttle_;
   bool quickturn_;
 
   Gear left_gear_;
@@ -62,18 +78,354 @@
   ::frc971::control_loops::DrivetrainQueue::Position last_position_;
   ::frc971::control_loops::DrivetrainQueue::Position position_;
   int counter_;
-  DrivetrainConfig dt_config_;
+  DrivetrainConfig<Scalar> dt_config_;
 
-  double goal_left_velocity_ = 0.0;
-  double goal_right_velocity_ = 0.0;
+  Scalar goal_left_velocity_ = 0.0;
+  Scalar goal_right_velocity_ = 0.0;
 
   // Stored from the last iteration, for logging shifting logic.
-  double left_motor_speed_ = 0.0;
-  double right_motor_speed_ = 0.0;
-  double current_left_velocity_ = 0.0;
-  double current_right_velocity_ = 0.0;
+  Scalar left_motor_speed_ = 0.0;
+  Scalar right_motor_speed_ = 0.0;
+  Scalar current_left_velocity_ = 0.0;
+  Scalar current_right_velocity_ = 0.0;
 };
 
+template <typename Scalar>
+PolyDrivetrain<Scalar>::PolyDrivetrain(
+    const DrivetrainConfig<Scalar> &dt_config,
+    StateFeedbackLoop<7, 2, 4, Scalar> *kf)
+    : kf_(kf),
+      U_Poly_((Eigen::Matrix<Scalar, 4, 2>() << /*[[*/ 1, 0 /*]*/,
+               /*[*/ -1, 0 /*]*/,
+               /*[*/ 0, 1 /*]*/,
+               /*[*/ 0, -1 /*]]*/)
+                  .finished(),
+              (Eigen::Matrix<Scalar, 4, 1>() << /*[[*/ 12 /*]*/,
+               /*[*/ 12 /*]*/,
+               /*[*/ 12 /*]*/,
+               /*[*/ 12 /*]]*/)
+                  .finished(),
+              (Eigen::Matrix<Scalar, 2, 4>() << /*[[*/ 12, 12, -12, -12 /*]*/,
+               /*[*/ -12, 12, 12, -12 /*]*/)
+                  .finished()),
+      loop_(new StateFeedbackLoop<2, 2, 2, Scalar>(
+          dt_config.make_v_drivetrain_loop())),
+      ttrust_(1.1),
+      wheel_(0.0),
+      throttle_(0.0),
+      quickturn_(false),
+      left_gear_(dt_config.default_high_gear ? Gear::HIGH : Gear::LOW),
+      right_gear_(dt_config.default_high_gear ? Gear::HIGH : Gear::LOW),
+      counter_(0),
+      dt_config_(dt_config) {
+  last_position_.Zero();
+  position_.Zero();
+}
+
+template <typename Scalar>
+Scalar PolyDrivetrain<Scalar>::MotorSpeed(
+    const constants::ShifterHallEffect &hall_effect, Scalar shifter_position,
+    Scalar velocity, Gear gear) {
+  const Scalar high_gear_speed =
+      velocity / dt_config_.high_gear_ratio / dt_config_.wheel_radius;
+  const Scalar low_gear_speed =
+      velocity / dt_config_.low_gear_ratio / dt_config_.wheel_radius;
+
+  if (shifter_position < hall_effect.clear_low) {
+    // We're in low gear, so return speed for that gear.
+    return low_gear_speed;
+  } else if (shifter_position > hall_effect.clear_high) {
+    // We're in high gear, so return speed for that gear.
+    return high_gear_speed;
+  }
+
+  // Not in gear, so speed-match to destination gear.
+  switch (gear) {
+    case Gear::HIGH:
+    case Gear::SHIFTING_UP:
+      return high_gear_speed;
+    case Gear::LOW:
+    case Gear::SHIFTING_DOWN:
+    default:
+      return low_gear_speed;
+      break;
+  }
+}
+
+template <typename Scalar>
+Gear PolyDrivetrain<Scalar>::UpdateSingleGear(Gear requested_gear,
+                                              Gear current_gear) {
+  const Gear shift_up =
+      (dt_config_.shifter_type == ShifterType::HALL_EFFECT_SHIFTER)
+          ? Gear::SHIFTING_UP
+          : Gear::HIGH;
+  const Gear shift_down =
+      (dt_config_.shifter_type == ShifterType::HALL_EFFECT_SHIFTER)
+          ? Gear::SHIFTING_DOWN
+          : Gear::LOW;
+  if (current_gear != requested_gear) {
+    if (IsInGear(current_gear)) {
+      if (requested_gear == Gear::HIGH) {
+        if (current_gear != Gear::HIGH) {
+          current_gear = shift_up;
+        }
+      } else {
+        if (current_gear != Gear::LOW) {
+          current_gear = shift_down;
+        }
+      }
+    } else {
+      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;
+}
+
+template <typename Scalar>
+void PolyDrivetrain<Scalar>::SetGoal(
+    const ::frc971::control_loops::DrivetrainQueue::Goal &goal) {
+  const Scalar wheel = goal.wheel;
+  const Scalar throttle = goal.throttle;
+  const bool quickturn = goal.quickturn;
+  const bool highgear = goal.highgear;
+
+  // Apply a sin function that's scaled to make it feel better.
+  const Scalar angular_range = M_PI_2 * dt_config_.wheel_non_linearity;
+
+  wheel_ = sin(angular_range * wheel) / sin(angular_range);
+  wheel_ = sin(angular_range * wheel_) / sin(angular_range);
+  wheel_ = 2.0 * wheel - wheel_;
+  quickturn_ = quickturn;
+
+  if (quickturn_) {
+    wheel_ *= dt_config_.quickturn_wheel_multiplier;
+  } else {
+    wheel_ *= dt_config_.wheel_multiplier;
+  }
+
+  static const Scalar kThrottleDeadband = 0.05;
+  if (::std::abs(throttle) < kThrottleDeadband) {
+    throttle_ = 0;
+  } else {
+    throttle_ = copysign(
+        (::std::abs(throttle) - kThrottleDeadband) / (1.0 - kThrottleDeadband),
+        throttle);
+  }
+
+  Gear requested_gear = highgear ? Gear::HIGH : Gear::LOW;
+
+  left_gear_ = UpdateSingleGear(requested_gear, left_gear_);
+  right_gear_ = UpdateSingleGear(requested_gear, right_gear_);
+}
+
+template <typename Scalar>
+void PolyDrivetrain<Scalar>::SetPosition(
+    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;
+}
+
+template <typename Scalar>
+Scalar PolyDrivetrain<Scalar>::FilterVelocity(Scalar throttle) const {
+  const Eigen::Matrix<Scalar, 2, 2> FF =
+      loop_->plant().B().inverse() *
+      (Eigen::Matrix<Scalar, 2, 2>::Identity() - loop_->plant().A());
+
+  constexpr int kHighGearController = 3;
+  const Eigen::Matrix<Scalar, 2, 2> FF_high =
+      loop_->plant().coefficients(kHighGearController).B.inverse() *
+      (Eigen::Matrix<Scalar, 2, 2>::Identity() -
+       loop_->plant().coefficients(kHighGearController).A);
+
+  ::Eigen::Matrix<Scalar, 1, 2> FF_sum = FF.colwise().sum();
+  int min_FF_sum_index;
+  const Scalar min_FF_sum = FF_sum.minCoeff(&min_FF_sum_index);
+  const Scalar min_K_sum = loop_->controller().K().col(min_FF_sum_index).sum();
+  const Scalar high_min_FF_sum = FF_high.col(0).sum();
+
+  const Scalar adjusted_ff_voltage =
+      ::aos::Clip(throttle * 12.0 * min_FF_sum / high_min_FF_sum, -12.0, 12.0);
+  return (adjusted_ff_voltage +
+          ttrust_ * min_K_sum * (loop_->X_hat(0, 0) + loop_->X_hat(1, 0)) /
+              2.0) /
+         (ttrust_ * min_K_sum + min_FF_sum);
+}
+
+template <typename Scalar>
+Scalar PolyDrivetrain<Scalar>::MaxVelocity() {
+  const Eigen::Matrix<Scalar, 2, 2> FF =
+      loop_->plant().B().inverse() *
+      (Eigen::Matrix<Scalar, 2, 2>::Identity() - loop_->plant().A());
+
+  constexpr int kHighGearController = 3;
+  const Eigen::Matrix<Scalar, 2, 2> FF_high =
+      loop_->plant().coefficients(kHighGearController).B.inverse() *
+      (Eigen::Matrix<Scalar, 2, 2>::Identity() -
+       loop_->plant().coefficients(kHighGearController).A);
+
+  ::Eigen::Matrix<Scalar, 1, 2> FF_sum = FF.colwise().sum();
+  int min_FF_sum_index;
+  const Scalar min_FF_sum = FF_sum.minCoeff(&min_FF_sum_index);
+  // const Scalar min_K_sum = loop_->K().col(min_FF_sum_index).sum();
+  const Scalar high_min_FF_sum = FF_high.col(0).sum();
+
+  const Scalar adjusted_ff_voltage =
+      ::aos::Clip(12.0 * min_FF_sum / high_min_FF_sum, -12.0, 12.0);
+  return adjusted_ff_voltage / min_FF_sum;
+}
+
+template <typename Scalar>
+void PolyDrivetrain<Scalar>::Update() {
+  if (dt_config_.loop_type == LoopType::CLOSED_LOOP) {
+    loop_->mutable_X_hat()(0, 0) = kf_->X_hat()(1, 0);
+    loop_->mutable_X_hat()(1, 0) = kf_->X_hat()(3, 0);
+  }
+
+  // TODO(austin): Observer for the current velocity instead of difference
+  // calculations.
+  ++counter_;
+
+  if (IsInGear(left_gear_) && IsInGear(right_gear_)) {
+    // FF * X = U (steady state)
+    const Eigen::Matrix<Scalar, 2, 2> FF =
+        loop_->plant().B().inverse() *
+        (Eigen::Matrix<Scalar, 2, 2>::Identity() - loop_->plant().A());
+
+    // Invert the plant to figure out how the velocity filter would have to
+    // work
+    // out in order to filter out the forwards negative inertia.
+    // This math assumes that the left and right power and velocity are
+    // equals,
+    // and that the plant is the same on the left and right.
+    const Scalar fvel = FilterVelocity(throttle_);
+
+    const Scalar sign_svel = wheel_ * ((fvel > 0.0) ? 1.0 : -1.0);
+    Scalar steering_velocity;
+    if (quickturn_) {
+      steering_velocity = wheel_ * MaxVelocity();
+    } else {
+      steering_velocity = ::std::abs(fvel) * wheel_;
+    }
+    const Scalar left_velocity = fvel - steering_velocity;
+    const Scalar right_velocity = fvel + steering_velocity;
+    goal_left_velocity_ = left_velocity;
+    goal_right_velocity_ = right_velocity;
+
+    // Integrate velocity to get the position.
+    // This position is used to get integral control.
+    loop_->mutable_R() << left_velocity, right_velocity;
+
+    if (!quickturn_) {
+      // K * R = w
+      Eigen::Matrix<Scalar, 1, 2> equality_k;
+      equality_k << 1 + sign_svel, -(1 - sign_svel);
+      const Scalar equality_w = 0.0;
+
+      // Construct a constraint on R by manipulating the constraint on U
+      ::aos::controls::HVPolytope<2, 4, 4, Scalar> R_poly_hv(
+          U_Poly_.static_H() * (loop_->controller().K() + FF),
+          U_Poly_.static_k() +
+              U_Poly_.static_H() * loop_->controller().K() * loop_->X_hat(),
+          (loop_->controller().K() + FF).inverse() *
+              ::aos::controls::ShiftPoints<2, 4, Scalar>(
+                  U_Poly_.StaticVertices(),
+                  loop_->controller().K() * loop_->X_hat()));
+
+      // Limit R back inside the box.
+      loop_->mutable_R() =
+          CoerceGoal<Scalar>(R_poly_hv, equality_k, equality_w, loop_->R());
+    }
+
+    const Eigen::Matrix<Scalar, 2, 1> FF_volts = FF * loop_->R();
+    const Eigen::Matrix<Scalar, 2, 1> U_ideal =
+        loop_->controller().K() * (loop_->R() - loop_->X_hat()) + FF_volts;
+
+    for (int i = 0; i < 2; i++) {
+      loop_->mutable_U()[i] = ::aos::Clip(U_ideal[i], -12, 12);
+    }
+
+    if (dt_config_.loop_type == LoopType::OPEN_LOOP) {
+      loop_->mutable_X_hat() =
+          loop_->plant().A() * loop_->X_hat() + loop_->plant().B() * loop_->U();
+    }
+
+    // Housekeeping: set the shifting logging values to zero, because we're not shifting
+    left_motor_speed_ = 0.0;
+    right_motor_speed_ = 0.0;
+    current_left_velocity_ = 0.0;
+    current_right_velocity_ = 0.0;
+  } else {
+    current_left_velocity_ =
+        (position_.left_encoder - last_position_.left_encoder) / dt_config_.dt;
+    current_right_velocity_ =
+        (position_.right_encoder - last_position_.right_encoder) /
+        dt_config_.dt;
+    left_motor_speed_ =
+        MotorSpeed(dt_config_.left_drive, position_.left_shifter_position,
+                   current_left_velocity_, left_gear_);
+    right_motor_speed_ =
+        MotorSpeed(dt_config_.right_drive, position_.right_shifter_position,
+                   current_right_velocity_, right_gear_);
+
+    goal_left_velocity_ = current_left_velocity_;
+    goal_right_velocity_ = current_right_velocity_;
+
+    // Any motor is not in gear. Speed match.
+    ::Eigen::Matrix<Scalar, 1, 1> R_left;
+    ::Eigen::Matrix<Scalar, 1, 1> R_right;
+    R_left(0, 0) = left_motor_speed_;
+    R_right(0, 0) = right_motor_speed_;
+
+    const Scalar wiggle =
+        (static_cast<Scalar>((counter_ % 30) / 15) - 0.5) * 8.0;
+
+    loop_->mutable_U(0, 0) = ::aos::Clip(
+        (R_left / dt_config_.v)(0, 0) + (IsInGear(left_gear_) ? 0 : wiggle),
+        -12.0, 12.0);
+    loop_->mutable_U(1, 0) = ::aos::Clip(
+        (R_right / dt_config_.v)(0, 0) + (IsInGear(right_gear_) ? 0 : wiggle),
+        -12.0, 12.0);
+#ifdef __linux__
+    loop_->mutable_U() *= 12.0 / ::aos::robot_state->voltage_battery;
+#endif  // __linux__
+  }
+}
+
+template <typename Scalar>
+void PolyDrivetrain<Scalar>::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 = MaybeHigh(left_gear_);
+    output->right_high = MaybeHigh(right_gear_);
+  }
+}
+
+template <typename Scalar>
+void PolyDrivetrain<Scalar>::PopulateStatus(
+    ::frc971::control_loops::DrivetrainQueue::Status *status) {
+  status->left_velocity_goal = goal_left_velocity_;
+  status->right_velocity_goal = goal_right_velocity_;
+
+  status->cim_logging.left_in_gear = IsInGear(left_gear_);
+  status->cim_logging.left_motor_speed = left_motor_speed_;
+  status->cim_logging.left_velocity = current_left_velocity_;
+
+  status->cim_logging.right_in_gear = IsInGear(right_gear_);
+  status->cim_logging.right_motor_speed = right_motor_speed_;
+  status->cim_logging.right_velocity = current_right_velocity_;
+}
+
+
 }  // namespace drivetrain
 }  // namespace control_loops
 }  // namespace frc971
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.cc b/frc971/control_loops/drivetrain/ssdrivetrain.cc
index 39c4ade..ad86b1f 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.cc
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.cc
@@ -59,7 +59,7 @@
               (-velocity_K * velocity_error + U_integral - kf_->ff_U()) +
           (U_poly_.static_k() * max_voltage_),
       (position_K * T_).inverse() *
-          ::aos::controls::ShiftPoints<2, 4>(
+          ::aos::controls::ShiftPoints<2, 4, double>(
               (U_poly_.StaticVertices() * max_voltage_),
               -velocity_K * velocity_error + U_integral - kf_->ff_U()));
 
@@ -87,9 +87,9 @@
 
     bool is_inside_h, is_inside_45;
     const auto adjusted_pos_error_h =
-        DoCoerceGoal(pos_poly_hv, LH, wh, drive_error, &is_inside_h);
+        DoCoerceGoal<double>(pos_poly_hv, LH, wh, drive_error, &is_inside_h);
     const auto adjusted_pos_error_45 =
-        DoCoerceGoal(pos_poly_hv, L45, w45, intersection, &is_inside_45);
+        DoCoerceGoal<double>(pos_poly_hv, L45, w45, intersection, &is_inside_45);
     if (pos_poly_hv.IsInside(intersection)) {
       adjusted_pos_error = adjusted_pos_error_h;
     } else {
@@ -116,21 +116,25 @@
   }
 }
 
-DrivetrainMotorsSS::DrivetrainMotorsSS(const DrivetrainConfig &dt_config,
-                                       StateFeedbackLoop<7, 2, 4> *kf,
-                                       double *integrated_kf_heading)
+DrivetrainMotorsSS::DrivetrainMotorsSS(
+    const DrivetrainConfig<double> &dt_config, StateFeedbackLoop<7, 2, 4> *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>() << /*[[*/ 1.0 /*]*/,
-               /*[*/ 1.0 /*]*/,
-               /*[*/ 1.0 /*]*/,
-               /*[*/ 1.0 /*]]*/).finished(),
-              (Eigen::Matrix<double, 2, 4>() << /*[[*/ 1.0, 1.0, -1.0, -1.0 /*]*/,
-               /*[*/ -1.0, 1.0, 1.0, -1.0 /*]*/).finished()),
+      U_poly_(
+          (Eigen::Matrix<double, 4, 2>() << /*[[*/ 1, 0 /*]*/,
+           /*[*/ -1, 0 /*]*/,
+           /*[*/ 0, 1 /*]*/,
+           /*[*/ 0, -1 /*]]*/)
+              .finished(),
+          (Eigen::Matrix<double, 4, 1>() << /*[[*/ 1.0 /*]*/,
+           /*[*/ 1.0 /*]*/,
+           /*[*/ 1.0 /*]*/,
+           /*[*/ 1.0 /*]]*/)
+              .finished(),
+          (Eigen::Matrix<double, 2, 4>() << /*[[*/ 1.0, 1.0, -1.0, -1.0 /*]*/,
+           /*[*/ -1.0, 1.0, 1.0, -1.0 /*]*/)
+              .finished()),
       linear_profile_(::aos::controls::kLoopFrequency),
       angular_profile_(::aos::controls::kLoopFrequency),
       integrated_kf_heading_(integrated_kf_heading) {
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.h b/frc971/control_loops/drivetrain/ssdrivetrain.h
index 9a9c0d8..5e261e9 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.h
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.h
@@ -18,7 +18,7 @@
 
 class DrivetrainMotorsSS {
  public:
-  DrivetrainMotorsSS(const DrivetrainConfig &dt_config,
+  DrivetrainMotorsSS(const DrivetrainConfig<double> &dt_config,
                      StateFeedbackLoop<7, 2, 4> *kf,
                      double *integrated_kf_heading);
 
@@ -42,7 +42,7 @@
   void PolyCapU(Eigen::Matrix<double, 2, 1> *U);
   void ScaleCapU(Eigen::Matrix<double, 2, 1> *U);
 
-  const DrivetrainConfig dt_config_;
+  const DrivetrainConfig<double> dt_config_;
   StateFeedbackLoop<7, 2, 4> *kf_;
   Eigen::Matrix<double, 7, 1> unprofiled_goal_;