diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index 360097d..5998afc 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -1,6 +1,7 @@
 package(default_visibility = ["//visibility:public"])
 
 load("//aos/build:queues.bzl", "queue_library")
+load("//tools:environments.bzl", "mcu_cpus")
 
 cc_library(
     name = "team_number_test_environment",
@@ -89,6 +90,21 @@
 )
 
 cc_library(
+    name = "coerce_goal_uc",
+    srcs = [
+        "coerce_goal.cc",
+    ],
+    hdrs = [
+        "coerce_goal.h",
+    ],
+    restricted_to = mcu_cpus,
+    deps = [
+        "//aos/common/controls:polytope_uc",
+        "//third_party/eigen",
+    ],
+)
+
+cc_library(
     name = "coerce_goal",
     srcs = [
         "coerce_goal.cc",
diff --git a/frc971/control_loops/coerce_goal.cc b/frc971/control_loops/coerce_goal.cc
index 0c98560..d98df09 100644
--- a/frc971/control_loops/coerce_goal.cc
+++ b/frc971/control_loops/coerce_goal.cc
@@ -7,71 +7,5 @@
 namespace frc971 {
 namespace control_loops {
 
-Eigen::Matrix<double, 2, 1> DoCoerceGoal(
-    const aos::controls::HVPolytope<2, 4, 4> &region,
-    const Eigen::Matrix<double, 1, 2> &K, double w,
-    const Eigen::Matrix<double, 2, 1> &R, bool *is_inside) {
-  if (region.IsInside(R)) {
-    if (is_inside) *is_inside = true;
-    return R;
-  }
-  Eigen::Matrix<double, 2, 1> parallel_vector;
-  Eigen::Matrix<double, 2, 1> perpendicular_vector;
-  perpendicular_vector = K.transpose().normalized();
-  parallel_vector << perpendicular_vector(1, 0), -perpendicular_vector(0, 0);
-
-  Eigen::Matrix<double, 4, 1> projectedh = region.static_H() * parallel_vector;
-  Eigen::Matrix<double, 4, 1> projectedk =
-      region.static_k() - region.static_H() * perpendicular_vector * w;
-
-  double min_boundary = ::std::numeric_limits<double>::lowest();
-  double max_boundary = ::std::numeric_limits<double>::max();
-  for (int i = 0; i < 4; ++i) {
-    if (projectedh(i, 0) > 0) {
-      max_boundary =
-          ::std::min(max_boundary, projectedk(i, 0) / projectedh(i, 0));
-    } else {
-      min_boundary =
-          ::std::max(min_boundary, projectedk(i, 0) / projectedh(i, 0));
-    }
-  }
-
-  Eigen::Matrix<double, 1, 2> vertices;
-  vertices << max_boundary, min_boundary;
-
-  if (max_boundary > min_boundary) {
-    double min_distance_sqr = 0;
-    Eigen::Matrix<double, 2, 1> closest_point;
-    for (int i = 0; i < vertices.innerSize(); i++) {
-      Eigen::Matrix<double, 2, 1> point;
-      point = parallel_vector * vertices(0, i) + perpendicular_vector * w;
-      const double length = (R - point).squaredNorm();
-      if (i == 0 || length < min_distance_sqr) {
-        closest_point = point;
-        min_distance_sqr = length;
-      }
-    }
-    if (is_inside) *is_inside = true;
-    return closest_point;
-  } else {
-    Eigen::Matrix<double, 2, 4> region_vertices =
-        region.StaticVertices();
-    CHECK_GT(region_vertices.outerSize(), 0);
-    double min_distance = INFINITY;
-    int closest_i = 0;
-    for (int i = 0; i < region_vertices.outerSize(); i++) {
-      const double length = ::std::abs(
-          (perpendicular_vector.transpose() * (region_vertices.col(i)))(0, 0));
-      if (i == 0 || length < min_distance) {
-        closest_i = i;
-        min_distance = length;
-      }
-    }
-    if (is_inside) *is_inside = false;
-    return (Eigen::Matrix<double, 2, 1>() << region_vertices(0, closest_i),
-            region_vertices(1, closest_i)).finished();
-  }
-}
-
 }  // namespace control_loops
 }  // namespace frc971
diff --git a/frc971/control_loops/coerce_goal.h b/frc971/control_loops/coerce_goal.h
index 83c401d..b1d5c3c 100644
--- a/frc971/control_loops/coerce_goal.h
+++ b/frc971/control_loops/coerce_goal.h
@@ -8,22 +8,95 @@
 namespace frc971 {
 namespace control_loops {
 
-Eigen::Matrix<double, 2, 1> DoCoerceGoal(
-    const aos::controls::HVPolytope<2, 4, 4> &region,
-    const Eigen::Matrix<double, 1, 2> &K, double w,
-    const Eigen::Matrix<double, 2, 1> &R, bool *is_inside);
+template <typename Scalar = double>
+Eigen::Matrix<Scalar, 2, 1> DoCoerceGoal(
+    const aos::controls::HVPolytope<2, 4, 4, Scalar> &region,
+    const Eigen::Matrix<Scalar, 1, 2> &K, Scalar w,
+    const Eigen::Matrix<Scalar, 2, 1> &R, bool *is_inside);
 
 // Intersects a line with a region, and finds the closest point to R.
 // Finds a point that is closest to R inside the region, and on the line
 // defined by K X = w.  If it is not possible to find a point on the line,
 // finds a point that is inside the region and closest to the line.
-static inline Eigen::Matrix<double, 2, 1> CoerceGoal(
-    const aos::controls::HVPolytope<2, 4, 4> &region,
-    const Eigen::Matrix<double, 1, 2> &K, double w,
-    const Eigen::Matrix<double, 2, 1> &R) {
+template <typename Scalar = double>
+static inline Eigen::Matrix<Scalar, 2, 1> CoerceGoal(
+    const aos::controls::HVPolytope<2, 4, 4, Scalar> &region,
+    const Eigen::Matrix<Scalar, 1, 2> &K, Scalar w,
+    const Eigen::Matrix<Scalar, 2, 1> &R) {
   return DoCoerceGoal(region, K, w, R, nullptr);
 }
 
+template <typename Scalar>
+Eigen::Matrix<Scalar, 2, 1> DoCoerceGoal(
+    const aos::controls::HVPolytope<2, 4, 4, Scalar> &region,
+    const Eigen::Matrix<Scalar, 1, 2> &K, Scalar w,
+    const Eigen::Matrix<Scalar, 2, 1> &R, bool *is_inside) {
+  if (region.IsInside(R)) {
+    if (is_inside) *is_inside = true;
+    return R;
+  }
+  Eigen::Matrix<Scalar, 2, 1> parallel_vector;
+  Eigen::Matrix<Scalar, 2, 1> perpendicular_vector;
+  perpendicular_vector = K.transpose().normalized();
+  parallel_vector << perpendicular_vector(1, 0), -perpendicular_vector(0, 0);
+
+  Eigen::Matrix<Scalar, 4, 1> projectedh = region.static_H() * parallel_vector;
+  Eigen::Matrix<Scalar, 4, 1> projectedk =
+      region.static_k() - region.static_H() * perpendicular_vector * w;
+
+  Scalar min_boundary = ::std::numeric_limits<Scalar>::lowest();
+  Scalar max_boundary = ::std::numeric_limits<Scalar>::max();
+  for (int i = 0; i < 4; ++i) {
+    if (projectedh(i, 0) > 0) {
+      max_boundary =
+          ::std::min(max_boundary, projectedk(i, 0) / projectedh(i, 0));
+    } else {
+      min_boundary =
+          ::std::max(min_boundary, projectedk(i, 0) / projectedh(i, 0));
+    }
+  }
+
+  Eigen::Matrix<Scalar, 1, 2> vertices;
+  vertices << max_boundary, min_boundary;
+
+  if (max_boundary > min_boundary) {
+    Scalar min_distance_sqr = 0;
+    Eigen::Matrix<Scalar, 2, 1> closest_point;
+    for (int i = 0; i < vertices.innerSize(); i++) {
+      Eigen::Matrix<Scalar, 2, 1> point;
+      point = parallel_vector * vertices(0, i) + perpendicular_vector * w;
+      const Scalar length = (R - point).squaredNorm();
+      if (i == 0 || length < min_distance_sqr) {
+        closest_point = point;
+        min_distance_sqr = length;
+      }
+    }
+    if (is_inside) *is_inside = true;
+    return closest_point;
+  } else {
+    Eigen::Matrix<Scalar, 2, 4> region_vertices = region.StaticVertices();
+#ifdef __linux__
+    CHECK_GT(region_vertices.outerSize(), 0);
+#else
+    assert(region_vertices.outerSize() > 0);
+#endif
+    Scalar min_distance = INFINITY;
+    int closest_i = 0;
+    for (int i = 0; i < region_vertices.outerSize(); i++) {
+      const Scalar length = ::std::abs(
+          (perpendicular_vector.transpose() * (region_vertices.col(i)))(0, 0));
+      if (i == 0 || length < min_distance) {
+        closest_i = i;
+        min_distance = length;
+      }
+    }
+    if (is_inside) *is_inside = false;
+    return (Eigen::Matrix<Scalar, 2, 1>() << region_vertices(0, closest_i),
+            region_vertices(1, closest_i))
+        .finished();
+  }
+}
+
 }  // namespace control_loops
 }  // namespace frc971
 
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_;
 
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index e62bff9..2ef2ece 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -10,9 +10,13 @@
     self.formatToType = {}
     self.formatToType['%f'] = "double"
     self.formatToType['%d'] = "int"
-  def __str__ (self):
+
+  def Render(self, loop_type):
+    typestring = self.formatToType[self.formatt]
+    if loop_type == 'float' and typestring == 'double':
+      typestring = loop_type
     return str("\nstatic constexpr %s %s = "+ self.formatt +";\n") % \
-        (self.formatToType[self.formatt], self.name, self.value)
+        (typestring, self.name, self.value)
 
 
 class ControlLoopWriter(object):
@@ -138,7 +142,7 @@
       fd.write(self._namespace_start)
 
       for const in self._constant_list:
-          fd.write(str(const))
+          fd.write(const.Render(self._scalar_type))
 
       fd.write('\n\n')
       for loop in self._loops:
@@ -325,6 +329,8 @@
       for y in xrange(matrix.shape[1]):
         write_type =  repr(matrix[x, y])
         if scalar_type == 'float':
+          if '.' not in write_type:
+            write_type += '.0'
           write_type += 'f'
         ans.append('  %s(%d, %d) = %s;\n' % (matrix_name, x, y, write_type))
 
diff --git a/frc971/control_loops/python/drivetrain.py b/frc971/control_loops/python/drivetrain.py
index 2380549..4c89fdc 100644
--- a/frc971/control_loops/python/drivetrain.py
+++ b/frc971/control_loops/python/drivetrain.py
@@ -15,10 +15,10 @@
                  wheel_radius,
                  G_high,
                  G_low,
-                 q_pos_low,
-                 q_pos_high,
-                 q_vel_low,
-                 q_vel_high,
+                 q_pos_low=0.12,
+                 q_pos_high=0.14,
+                 q_vel_low=1.0,
+                 q_vel_high=0.95,
                  efficiency=0.60,
                  has_imu=False,
                  force=False,
@@ -349,7 +349,7 @@
 
 
 def WriteDrivetrain(drivetrain_files, kf_drivetrain_files, year_namespace,
-                    drivetrain_params):
+                    drivetrain_params, scalar_type='double'):
 
     # Write the generated constants out to a file.
     drivetrain_low_low = Drivetrain(
@@ -394,13 +394,17 @@
         right_low=False,
         drivetrain_params=drivetrain_params)
 
-    namespaces = [year_namespace, 'control_loops', 'drivetrain']
+    if isinstance(year_namespace, list):
+      namespaces = year_namespace
+    else:
+      namespaces = [year_namespace, 'control_loops', 'drivetrain']
     dog_loop_writer = control_loop.ControlLoopWriter(
         "Drivetrain", [
             drivetrain_low_low, drivetrain_low_high, drivetrain_high_low,
             drivetrain_high_high
         ],
-        namespaces=namespaces)
+        namespaces=namespaces,
+        scalar_type=scalar_type)
     dog_loop_writer.AddConstant(
         control_loop.Constant("kDt", "%f", drivetrain_low_low.dt))
     dog_loop_writer.AddConstant(
@@ -447,7 +451,8 @@
             kf_drivetrain_low_low, kf_drivetrain_low_high,
             kf_drivetrain_high_low, kf_drivetrain_high_high
         ],
-        namespaces=namespaces)
+        namespaces=namespaces,
+        scalar_type=scalar_type)
     kf_loop_writer.Write(kf_drivetrain_files[0], kf_drivetrain_files[1])
 
 
diff --git a/frc971/control_loops/python/polydrivetrain.py b/frc971/control_loops/python/polydrivetrain.py
index c9c9efe..29fdeef 100644
--- a/frc971/control_loops/python/polydrivetrain.py
+++ b/frc971/control_loops/python/polydrivetrain.py
@@ -407,19 +407,23 @@
                self.right_gear, self.right_shifter_position)
 
 def WritePolyDrivetrain(drivetrain_files, motor_files, year_namespace,
-                        drivetrain_params):
+                        drivetrain_params, scalar_type='double'):
   vdrivetrain = VelocityDrivetrain(drivetrain_params)
-  namespaces = [year_namespace, 'control_loops', 'drivetrain']
+  if isinstance(year_namespace, list):
+    namespaces = year_namespace
+  else:
+    namespaces = [year_namespace, 'control_loops', 'drivetrain']
   dog_loop_writer = control_loop.ControlLoopWriter(
       "VelocityDrivetrain", [vdrivetrain.drivetrain_low_low,
                      vdrivetrain.drivetrain_low_high,
                      vdrivetrain.drivetrain_high_low,
                      vdrivetrain.drivetrain_high_high],
-                     namespaces=namespaces)
+                     namespaces=namespaces,
+                     scalar_type=scalar_type)
 
   dog_loop_writer.Write(drivetrain_files[0], drivetrain_files[1])
 
-  cim_writer = control_loop.ControlLoopWriter("CIM", [CIM()])
+  cim_writer = control_loop.ControlLoopWriter("CIM", [CIM()], scalar_type=scalar_type)
 
   cim_writer.Write(motor_files[0], motor_files[1])