diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index 11c93dc..3341402 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -4,6 +4,110 @@
 load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
 
 cc_library(
+    name = "control_loop_test",
+    testonly = True,
+    srcs = [
+        "control_loop_test.cc",
+    ],
+    hdrs = [
+        "control_loop_test.h",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+    deps = [
+        "//aos:flatbuffers",
+        "//aos:json_to_flatbuffer",
+        "//aos/events:simulated_event_loop",
+        "//aos/testing:googletest",
+        "//aos/testing:test_logging",
+        "//aos/time",
+        "//frc971/input:joystick_state_fbs",
+        "//frc971/input:robot_state_fbs",
+    ],
+)
+
+cc_library(
+    name = "polytope",
+    hdrs = [
+        "polytope.h",
+    ],
+    deps = [
+        "@org_tuxfamily_eigen//:eigen",
+    ] + select({
+        "@platforms//os:linux": [
+            "//aos/logging",
+            "//third_party/cddlib",
+            "@com_github_google_glog//:glog",
+        ],
+        "//conditions:default": [],
+    }),
+)
+
+cc_test(
+    name = "polytope_test",
+    srcs = [
+        "polytope_test.cc",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+    deps = [
+        ":polytope",
+        "//aos/testing:googletest",
+        "//aos/testing:test_logging",
+        "@org_tuxfamily_eigen//:eigen",
+    ],
+)
+
+cc_library(
+    name = "control_loop",
+    srcs = [
+        "control_loop.cc",
+        "control_loop-tmpl.h",
+    ],
+    hdrs = [
+        "control_loop.h",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+    deps = [
+        "//aos/events:event_loop",
+        "//aos/events:shm_event_loop",
+        "//aos/logging",
+        "//aos/time",
+        "//aos/util:log_interval",
+        "//frc971/input:joystick_state_fbs",
+        "//frc971/input:robot_state_fbs",
+    ],
+)
+
+cc_library(
+    name = "quaternion_utils",
+    srcs = [
+        "quaternion_utils.cc",
+    ],
+    hdrs = [
+        "quaternion_utils.h",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+    deps = [
+        "@com_github_google_glog//:glog",
+        "@org_tuxfamily_eigen//:eigen",
+    ],
+)
+
+cc_test(
+    name = "quarternion_utils_test",
+    srcs = [
+        "quaternion_utils_test.cc",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+    deps = [
+        ":quaternion_utils",
+        "//aos/testing:googletest",
+        "//aos/testing:random_seed",
+        "@com_github_google_glog//:glog",
+        "@org_tuxfamily_eigen//:eigen",
+    ],
+)
+
+cc_library(
     name = "team_number_test_environment",
     testonly = True,
     srcs = [
@@ -130,7 +234,7 @@
         "//conditions:default": [],
     }),
     deps = [
-        "//aos/controls:polytope",
+        "//frc971/control_loops:polytope",
         "@org_tuxfamily_eigen//:eigen",
     ],
 )
@@ -146,7 +250,7 @@
     target_compatible_with = ["@platforms//os:linux"],
     deps = [
         ":coerce_goal",
-        "//aos/controls:polytope",
+        "//frc971/control_loops:polytope",
         "//aos/testing:googletest",
         "@org_tuxfamily_eigen//:eigen",
     ],
@@ -176,7 +280,7 @@
         ":c2d",
         ":state_feedback_loop",
         "//aos:macros",
-        "//aos/controls:control_loop",
+        "//frc971/control_loops:control_loop",
         "//aos/logging",
         "@org_tuxfamily_eigen//:eigen",
     ],
@@ -263,7 +367,7 @@
         ":profiled_subsystem_fbs",
         ":simple_capped_state_feedback_loop",
         ":state_feedback_loop",
-        "//aos/controls:control_loop",
+        "//frc971/control_loops:control_loop",
         "//aos/util:trapezoid_profile",
         "//frc971/zeroing",
     ],
@@ -509,7 +613,7 @@
         ":static_zeroing_single_dof_profiled_subsystem_test_pot_and_absolute_position_fbs",
         ":static_zeroing_single_dof_profiled_subsystem_test_subsystem_goal_fbs",
         ":static_zeroing_single_dof_profiled_subsystem_test_subsystem_output_fbs",
-        "//aos/controls:control_loop_test",
+        "//frc971/control_loops:control_loop_test",
         "//aos/testing:googletest",
     ],
 )
diff --git a/frc971/control_loops/coerce_goal.cc b/frc971/control_loops/coerce_goal.cc
index 6fdc70f..6e1dcf4 100644
--- a/frc971/control_loops/coerce_goal.cc
+++ b/frc971/control_loops/coerce_goal.cc
@@ -1,7 +1,7 @@
 #include "frc971/control_loops/coerce_goal.h"
 
 #include "Eigen/Dense"
-#include "aos/controls/polytope.h"
+#include "frc971/control_loops/polytope.h"
 
 namespace frc971 {
 namespace control_loops {}  // namespace control_loops
diff --git a/frc971/control_loops/coerce_goal.h b/frc971/control_loops/coerce_goal.h
index b682b85..1847682 100644
--- a/frc971/control_loops/coerce_goal.h
+++ b/frc971/control_loops/coerce_goal.h
@@ -2,15 +2,14 @@
 #define FRC971_CONTROL_LOOPS_COERCE_GOAL_H_
 
 #include "Eigen/Dense"
-
-#include "aos/controls/polytope.h"
+#include "frc971/control_loops/polytope.h"
 
 namespace frc971 {
 namespace control_loops {
 
 template <typename Scalar = double>
 Eigen::Matrix<Scalar, 2, 1> DoCoerceGoal(
-    const aos::controls::HVPolytope<2, 4, 4, Scalar> &region,
+    const frc971::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);
 
@@ -20,7 +19,7 @@
 // finds a point that is inside the region and closest to the line.
 template <typename Scalar = double>
 static inline Eigen::Matrix<Scalar, 2, 1> CoerceGoal(
-    const aos::controls::HVPolytope<2, 4, 4, Scalar> &region,
+    const frc971::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);
@@ -28,7 +27,7 @@
 
 template <typename Scalar>
 Eigen::Matrix<Scalar, 2, 1> DoCoerceGoal(
-    const aos::controls::HVPolytope<2, 4, 4, Scalar> &region,
+    const frc971::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)) {
diff --git a/frc971/control_loops/coerce_goal_test.cc b/frc971/control_loops/coerce_goal_test.cc
index ae04d6c..acdee9f 100644
--- a/frc971/control_loops/coerce_goal_test.cc
+++ b/frc971/control_loops/coerce_goal_test.cc
@@ -2,7 +2,7 @@
 
 #include <unistd.h>
 
-#include "aos/controls/polytope.h"
+#include "frc971/control_loops/polytope.h"
 #include "gtest/gtest.h"
 
 namespace frc971 {
@@ -10,8 +10,8 @@
 
 namespace {
 
-aos::controls::HVPolytope<2, 4, 4> MakeBox(double x1_min, double x1_max,
-                                           double x2_min, double x2_max) {
+frc971::controls::HVPolytope<2, 4, 4> MakeBox(double x1_min, double x1_max,
+                                              double x2_min, double x2_max) {
   Eigen::Matrix<double, 4, 2> box_H;
   box_H << /*[[*/ 1.0, 0.0 /*]*/,
       /*[*/ -1.0, 0.0 /*]*/,
@@ -22,21 +22,20 @@
       /*[*/ -x1_min /*]*/,
       /*[*/ x2_max /*]*/,
       /*[*/ -x2_min /*]]*/;
-  aos::controls::HPolytope<2> t_poly(box_H, box_k);
-  return aos::controls::HVPolytope<2, 4, 4>(t_poly.H(), t_poly.k(),
-                                            t_poly.Vertices());
+  frc971::controls::HPolytope<2> t_poly(box_H, box_k);
+  return frc971::controls::HVPolytope<2, 4, 4>(t_poly.H(), t_poly.k(),
+                                               t_poly.Vertices());
 }
 }  // namespace
 
 class CoerceGoalTest : public ::testing::Test {
  public:
-  void SetUp() override { aos::controls::HPolytope<2>::Init(); }
+  void SetUp() override { frc971::controls::HPolytope<2>::Init(); }
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
-
 TEST_F(CoerceGoalTest, Inside) {
-  aos::controls::HVPolytope<2, 4, 4> box = MakeBox(1, 2, 1, 2);
+  frc971::controls::HVPolytope<2, 4, 4> box = MakeBox(1, 2, 1, 2);
 
   Eigen::Matrix<double, 1, 2> K;
   K << /*[[*/ 1, -1 /*]]*/;
@@ -52,7 +51,7 @@
 }
 
 TEST_F(CoerceGoalTest, LineOutside) {
-  aos::controls::HVPolytope<2, 4, 4> box = MakeBox(1, 2, 1, 2);
+  frc971::controls::HVPolytope<2, 4, 4> box = MakeBox(1, 2, 1, 2);
 
   // Make a line equivalent to y = -x, which does not pass through the box and
   // is nearest the box at (1, 1).
@@ -76,7 +75,7 @@
 }
 
 TEST_F(CoerceGoalTest, GoalOutsideLineInsideThroughOrigin) {
-  aos::controls::HVPolytope<2, 4, 4> box = MakeBox(1, 2, 1, 2);
+  frc971::controls::HVPolytope<2, 4, 4> box = MakeBox(1, 2, 1, 2);
 
   Eigen::Matrix<double, 1, 2> K;
   K << 1, -1;
@@ -92,7 +91,7 @@
 }
 
 TEST_F(CoerceGoalTest, GoalOutsideLineNotThroughOrigin) {
-  aos::controls::HVPolytope<2, 4, 4> box = MakeBox(1, 2, 1, 2);
+  frc971::controls::HVPolytope<2, 4, 4> box = MakeBox(1, 2, 1, 2);
 
   Eigen::Matrix<double, 1, 2> K;
   K << 1, 1;
@@ -108,7 +107,7 @@
 }
 
 TEST_F(CoerceGoalTest, GoalOutsideLineThroughVertex) {
-  aos::controls::HVPolytope<2, 4, 4> box = MakeBox(1, 2, 1, 2);
+  frc971::controls::HVPolytope<2, 4, 4> box = MakeBox(1, 2, 1, 2);
 
   Eigen::Matrix<double, 1, 2> K;
   K << 1, -1;
@@ -124,7 +123,7 @@
 }
 
 TEST_F(CoerceGoalTest, LineAndGoalOutside) {
-  aos::controls::HVPolytope<2, 4, 4> box = MakeBox(3, 4, 1, 2);
+  frc971::controls::HVPolytope<2, 4, 4> box = MakeBox(3, 4, 1, 2);
 
   Eigen::Matrix<double, 1, 2> K;
   K << 1, -1;
@@ -140,7 +139,7 @@
 }
 
 TEST_F(CoerceGoalTest, LineThroughEdgeOfBox) {
-  aos::controls::HVPolytope<2, 4, 4> box = MakeBox(0, 4, 1, 2);
+  frc971::controls::HVPolytope<2, 4, 4> box = MakeBox(0, 4, 1, 2);
 
   Eigen::Matrix<double, 1, 2> K;
   K << -1, 1;
@@ -156,7 +155,7 @@
 }
 
 TEST_F(CoerceGoalTest, PerpendicularLine) {
-  aos::controls::HVPolytope<2, 4, 4> box = MakeBox(1, 2, 1, 2);
+  frc971::controls::HVPolytope<2, 4, 4> box = MakeBox(1, 2, 1, 2);
 
   Eigen::Matrix<double, 1, 2> K;
   K << 1, 1;
diff --git a/frc971/control_loops/control_loop-tmpl.h b/frc971/control_loops/control_loop-tmpl.h
new file mode 100644
index 0000000..f6fd0af
--- /dev/null
+++ b/frc971/control_loops/control_loop-tmpl.h
@@ -0,0 +1,94 @@
+#include <stddef.h>
+#include <inttypes.h>
+
+#include "aos/logging/logging.h"
+
+namespace frc971 {
+namespace controls {
+
+// TODO(aschuh): Tests.
+
+template <class GoalType, class PositionType, class StatusType,
+          class OutputType>
+constexpr ::std::chrono::milliseconds ControlLoop<
+    GoalType, PositionType, StatusType, OutputType>::kStaleLogInterval;
+template <class GoalType, class PositionType, class StatusType,
+          class OutputType>
+constexpr ::std::chrono::milliseconds ControlLoop<
+    GoalType, PositionType, StatusType, OutputType>::kPwmDisableTime;
+
+template <class GoalType, class PositionType, class StatusType,
+          class OutputType>
+void ControlLoop<GoalType, PositionType, StatusType, OutputType>::ZeroOutputs() {
+  typename ::aos::Sender<OutputType>::Builder builder =
+      output_sender_.MakeBuilder();
+  builder.Send(Zero(&builder));
+}
+
+template <class GoalType, class PositionType, class StatusType,
+          class OutputType>
+void ControlLoop<GoalType, PositionType, StatusType,
+                 OutputType>::IteratePosition(const PositionType &position) {
+  no_goal_.Print();
+  no_sensor_state_.Print();
+  motors_off_log_.Print();
+
+  // Fetch the latest control loop goal. If there is no new
+  // goal, we will just reuse the old one.
+  goal_fetcher_.Fetch();
+  const GoalType *goal = goal_fetcher_.get();
+
+  const bool new_robot_state = robot_state_fetcher_.Fetch();
+  if (!robot_state_fetcher_.get()) {
+    AOS_LOG_INTERVAL(no_sensor_state_);
+    return;
+  }
+  if (sensor_reader_pid_ != robot_state_fetcher_->reader_pid()) {
+    AOS_LOG(INFO, "new sensor reader PID %" PRId32 ", old was %" PRId32 "\n",
+            robot_state_fetcher_->reader_pid(), sensor_reader_pid_);
+    reset_ = true;
+    sensor_reader_pid_ = robot_state_fetcher_->reader_pid();
+  }
+
+  bool outputs_enabled = robot_state_fetcher_->outputs_enabled();
+
+  // Check to see if we got a driver station packet recently.
+  if (new_robot_state) {
+    if (robot_state_fetcher_->outputs_enabled()) {
+      // If the driver's station reports being disabled, we're probably not
+      // actually going to send motor values regardless of what the FPGA
+      // reports.
+      last_pwm_sent_ = robot_state_fetcher_.context().monotonic_event_time;
+    }
+  }
+
+  const ::aos::monotonic_clock::time_point monotonic_now =
+      event_loop_->monotonic_now();
+  const bool motors_off = monotonic_now >= kPwmDisableTime + last_pwm_sent_;
+  joystick_state_fetcher_.Fetch();
+  if (motors_off) {
+    if (joystick_state_fetcher_.get() && joystick_state_fetcher_->enabled()) {
+      AOS_LOG_INTERVAL(motors_off_log_);
+    }
+    outputs_enabled = false;
+  }
+
+  typename ::aos::Sender<StatusType>::Builder status =
+      status_sender_.MakeBuilder();
+  if (outputs_enabled) {
+    typename ::aos::Sender<OutputType>::Builder output =
+        output_sender_.MakeBuilder();
+    RunIteration(goal, &position, &output, &status);
+
+    output.CheckSent();
+  } else {
+    // The outputs are disabled, so pass nullptr in for the output.
+    RunIteration(goal, &position, nullptr, &status);
+    ZeroOutputs();
+  }
+
+  status.CheckSent();
+}
+
+}  // namespace controls
+}  // namespace frc971
diff --git a/frc971/control_loops/control_loop.cc b/frc971/control_loops/control_loop.cc
new file mode 100644
index 0000000..99c005a
--- /dev/null
+++ b/frc971/control_loops/control_loop.cc
@@ -0,0 +1,7 @@
+#include "frc971/control_loops/control_loop.h"
+
+namespace frc971 {
+namespace controls {
+
+}  // namespace controls
+}  // namespace frc971
diff --git a/frc971/control_loops/control_loop.h b/frc971/control_loops/control_loop.h
new file mode 100644
index 0000000..741b32f
--- /dev/null
+++ b/frc971/control_loops/control_loop.h
@@ -0,0 +1,142 @@
+#ifndef AOS_CONTROL_LOOP_CONTROL_LOOP_H_
+#define AOS_CONTROL_LOOP_CONTROL_LOOP_H_
+
+#include <string.h>
+
+#include <atomic>
+
+#include "aos/events/event_loop.h"
+#include "aos/time/time.h"
+#include "aos/type_traits/type_traits.h"
+#include "aos/util/log_interval.h"
+#include "frc971/input/joystick_state_generated.h"
+#include "frc971/input/robot_state_generated.h"
+
+namespace frc971 {
+namespace controls {
+
+// Control loops run this often, "starting" at time 0.
+constexpr ::std::chrono::nanoseconds kLoopFrequency =
+    ::std::chrono::milliseconds(5);
+
+// Provides helper methods to assist in writing control loops.
+// It will then call the RunIteration method every cycle that it has enough
+// valid data for the control loop to run.
+template <class GoalType, class PositionType, class StatusType,
+          class OutputType>
+class ControlLoop {
+ public:
+  ControlLoop(aos::EventLoop *event_loop, const ::std::string &name)
+      : event_loop_(event_loop), name_(name) {
+    output_sender_ = event_loop_->MakeSender<OutputType>(name_);
+    status_sender_ = event_loop_->MakeSender<StatusType>(name_);
+    goal_fetcher_ = event_loop_->MakeFetcher<GoalType>(name_);
+    robot_state_fetcher_ = event_loop_->MakeFetcher<::aos::RobotState>("/aos");
+    joystick_state_fetcher_ =
+        event_loop_->MakeFetcher<::aos::JoystickState>("/aos");
+
+    event_loop_->MakeWatcher(name_, [this](const PositionType &position) {
+      this->IteratePosition(position);
+    });
+  }
+
+  const ::aos::RobotState &robot_state() const { return *robot_state_fetcher_; }
+  bool has_joystick_state() const { return joystick_state_fetcher_.get(); }
+  const ::aos::JoystickState &joystick_state() const {
+    return *joystick_state_fetcher_;
+  }
+
+  // Returns true if all the counters etc in the sensor data have been reset.
+  // This will return true only a single time per reset.
+  bool WasReset() {
+    if (reset_) {
+      reset_ = false;
+      return true;
+    } else {
+      return false;
+    }
+  }
+
+  // Constructs and sends a message on the output queue which sets everything to
+  // a safe state.  Default is to set everything to zero.  Override Zero below
+  // to change that behavior.
+  void ZeroOutputs();
+
+  // Sets the output to zero.
+  // Override this if a value of zero (or false) is not "off" for this
+  // subsystem.
+  virtual flatbuffers::Offset<OutputType> Zero(
+      typename ::aos::Sender<OutputType>::Builder *builder) {
+    return builder->template MakeBuilder<OutputType>().Finish();
+  }
+
+ protected:
+  // Runs one cycle of the loop.
+  void IteratePosition(const PositionType &position);
+
+  aos::EventLoop *event_loop() { return event_loop_; }
+
+  // Returns the position context.  This is only valid inside the RunIteration
+  // method.
+  const aos::Context &position_context() { return event_loop_->context(); }
+
+  // Runs an iteration of the control loop.
+  // goal is the last goal that was sent.  It might be any number of cycles old
+  // or nullptr if we haven't ever received a goal.
+  // position is the current position, or nullptr if we didn't get a position
+  // this cycle.
+  // output is the values to be sent to the motors.  This is nullptr if the
+  // output is going to be ignored and set to 0.
+  // status is the status of the control loop.
+  // Both output and status should be filled in by the implementation.
+  virtual void RunIteration(
+      const GoalType *goal, const PositionType *position,
+      typename ::aos::Sender<OutputType>::Builder *output,
+      typename ::aos::Sender<StatusType>::Builder *status) = 0;
+
+ private:
+  static constexpr ::std::chrono::milliseconds kStaleLogInterval =
+      ::std::chrono::milliseconds(100);
+  // The amount of time after the last PWM pulse we consider motors enabled for.
+  // 100ms is the result of using an oscilliscope to look at the input and
+  // output of a Talon. The Info Sheet also lists 100ms for Talon SR, Talon SRX,
+  // and Victor SP.
+  static constexpr ::std::chrono::milliseconds kPwmDisableTime =
+      ::std::chrono::milliseconds(100);
+
+  // Pointer to the queue group
+  aos::EventLoop *event_loop_;
+  ::std::string name_;
+
+  ::aos::Sender<OutputType> output_sender_;
+  ::aos::Sender<StatusType> status_sender_;
+  ::aos::Fetcher<GoalType> goal_fetcher_;
+  ::aos::Fetcher<::aos::RobotState> robot_state_fetcher_;
+  ::aos::Fetcher<::aos::JoystickState> joystick_state_fetcher_;
+
+  // Fetcher only to be used for the Iterate method.  If Iterate is called, Run
+  // can't be called.
+  bool has_iterate_fetcher_ = false;
+  ::aos::Fetcher<PositionType> iterate_position_fetcher_;
+
+  bool reset_ = false;
+  int32_t sensor_reader_pid_ = 0;
+
+  ::aos::monotonic_clock::time_point last_pwm_sent_ =
+      ::aos::monotonic_clock::min_time;
+
+  typedef ::aos::util::SimpleLogInterval SimpleLogInterval;
+  SimpleLogInterval no_sensor_state_ =
+      SimpleLogInterval(kStaleLogInterval, ERROR, "no sensor state");
+  SimpleLogInterval motors_off_log_ =
+      SimpleLogInterval(kStaleLogInterval, WARNING, "motors disabled");
+  SimpleLogInterval no_goal_ =
+      SimpleLogInterval(kStaleLogInterval, ERROR, "no goal");
+};
+
+}  // namespace controls
+}  // namespace frc971
+
+#include "frc971/control_loops/control_loop-tmpl.h"  // IWYU pragma: export
+
+#endif
diff --git a/frc971/control_loops/control_loop_test.cc b/frc971/control_loops/control_loop_test.cc
new file mode 100644
index 0000000..e28f595
--- /dev/null
+++ b/frc971/control_loops/control_loop_test.cc
@@ -0,0 +1,10 @@
+#include "frc971/control_loops/control_loop_test.h"
+
+#include <chrono>
+
+namespace frc971 {
+namespace testing {
+
+
+}  // namespace testing
+}  // namespace frc971
diff --git a/frc971/control_loops/control_loop_test.h b/frc971/control_loops/control_loop_test.h
new file mode 100644
index 0000000..de9d4cd
--- /dev/null
+++ b/frc971/control_loops/control_loop_test.h
@@ -0,0 +1,196 @@
+#ifndef AOS_CONTROLS_CONTROL_LOOP_TEST_H_
+#define AOS_CONTROLS_CONTROL_LOOP_TEST_H_
+
+#include <chrono>
+#include <string_view>
+
+#include "aos/events/simulated_event_loop.h"
+#include "aos/flatbuffers.h"
+#include "aos/json_to_flatbuffer.h"
+#include "aos/testing/test_logging.h"
+#include "aos/time/time.h"
+#include "frc971/input/joystick_state_generated.h"
+#include "frc971/input/robot_state_generated.h"
+#include "gtest/gtest.h"
+
+namespace frc971 {
+namespace testing {
+
+// Handles setting up the environment that all control loops need to actually
+// run.
+// This includes sending the queue messages and Clear()ing the queues when
+// appropriate.
+// It also includes dealing with ::aos::time.
+template <typename TestBaseClass>
+class ControlLoopTestTemplated : public TestBaseClass {
+ public:
+  ControlLoopTestTemplated(
+      aos::FlatbufferDetachedBuffer<aos::Configuration> configuration,
+      ::std::chrono::nanoseconds dt = kTimeTick)
+      : configuration_(std::move(configuration)),
+        event_loop_factory_(&configuration_.message()),
+        dt_(dt),
+        robot_status_event_loop_(MakeEventLoop(
+            "robot_status",
+            aos::configuration::MultiNode(event_loop_factory_.configuration())
+                ? aos::configuration::GetNode(
+                      event_loop_factory_.configuration(), "roborio")
+                : nullptr)) {
+    aos::testing::EnableTestLogging();
+    robot_state_sender_ =
+        robot_status_event_loop_->MakeSender<::aos::RobotState>("/aos");
+    joystick_state_sender_ =
+        robot_status_event_loop_->MakeSender<::aos::JoystickState>("/aos");
+
+    // Schedule the robot status send 1 nanosecond before the loop runs.
+    send_robot_state_phased_loop_ = robot_status_event_loop_->AddPhasedLoop(
+        [this](int) { SendRobotState(); }, dt_,
+        dt - ::std::chrono::nanoseconds(1));
+
+    send_joystick_state_timer_ =
+        robot_status_event_loop_->AddTimer([this]() { SendJoystickState(); });
+
+    robot_status_event_loop_->OnRun([this]() {
+      send_joystick_state_timer_->Setup(
+          robot_status_event_loop_->monotonic_now(), dt_);
+    });
+  }
+  virtual ~ControlLoopTestTemplated() {}
+
+  void set_team_id(uint16_t team_id) { team_id_ = team_id; }
+  uint16_t team_id() const { return team_id_; }
+
+  // Sets the enabled/disabled bit and (potentially) rebroadcasts the robot
+  // state messages.
+  void SetEnabled(bool enabled) {
+    if (enabled_ != enabled) {
+      enabled_ = enabled;
+      SendJoystickState();
+      SendRobotState();
+      send_joystick_state_timer_->Setup(
+          robot_status_event_loop_->monotonic_now(), dt_);
+    }
+  }
+
+  // Simulate a reset of the process reading sensors, which tells loops that all
+  // index counts etc will be reset.
+  void SimulateSensorReset() { ++reader_pid_; }
+
+  // Sets the battery voltage in robot_state.
+  void set_battery_voltage(double battery_voltage) {
+    battery_voltage_ = battery_voltage;
+  }
+
+  ::std::unique_ptr<::aos::EventLoop> MakeEventLoop(
+      std::string_view name, const aos::Node *node = nullptr) {
+    return event_loop_factory_.MakeEventLoop(name, node);
+  }
+
+  void set_send_delay(std::chrono::nanoseconds send_delay) {
+    event_loop_factory_.set_send_delay(send_delay);
+  }
+
+  void RunFor(aos::monotonic_clock::duration duration) {
+    event_loop_factory_.RunFor(duration);
+  }
+
+  ::aos::monotonic_clock::time_point monotonic_now() {
+    return robot_status_event_loop_->monotonic_now();
+  }
+
+  ::std::chrono::nanoseconds dt() const { return dt_; }
+
+  const aos::Configuration *configuration() const {
+    return &configuration_.message();
+  }
+
+  aos::SimulatedEventLoopFactory *event_loop_factory() {
+    return &event_loop_factory_;
+  }
+
+ private:
+  // Sends out all of the required queue messages.
+  void SendJoystickState() {
+    if (monotonic_now() >= kDSPacketTime + last_ds_time_ ||
+        last_enabled_ != enabled_) {
+      auto new_state = joystick_state_sender_.MakeBuilder();
+      ::aos::JoystickState::Builder builder =
+          new_state.template MakeBuilder<::aos::JoystickState>();
+
+      builder.add_fake(true);
+
+      builder.add_enabled(enabled_);
+      builder.add_autonomous(false);
+      builder.add_team_id(team_id_);
+
+      new_state.Send(builder.Finish());
+
+      last_ds_time_ = monotonic_now();
+      last_enabled_ = enabled_;
+    }
+  }
+
+  bool last_enabled_ = false;
+
+  void SendRobotState() {
+    auto new_state = robot_state_sender_.MakeBuilder();
+
+    ::aos::RobotState::Builder builder =
+        new_state.template MakeBuilder<::aos::RobotState>();
+
+    builder.add_reader_pid(reader_pid_);
+    builder.add_outputs_enabled(enabled_);
+    builder.add_browned_out(false);
+
+    builder.add_is_3v3_active(true);
+    builder.add_is_5v_active(true);
+    builder.add_voltage_3v3(3.3);
+    builder.add_voltage_5v(5.0);
+
+    builder.add_voltage_roborio_in(battery_voltage_);
+    builder.add_voltage_battery(battery_voltage_);
+
+    new_state.Send(builder.Finish());
+  }
+
+  static constexpr ::std::chrono::microseconds kTimeTick{5000};
+  static constexpr ::std::chrono::milliseconds kDSPacketTime{20};
+
+  aos::FlatbufferDetachedBuffer<aos::Configuration> configuration_;
+
+  aos::SimulatedEventLoopFactory event_loop_factory_;
+
+  const ::std::chrono::nanoseconds dt_;
+
+  uint16_t team_id_ = 971;
+  int32_t reader_pid_ = 1;
+  double battery_voltage_ = 12.4;
+
+  ::aos::monotonic_clock::time_point last_ds_time_ =
+      ::aos::monotonic_clock::min_time;
+
+  bool enabled_ = false;
+
+  ::std::unique_ptr<::aos::EventLoop> robot_status_event_loop_;
+
+  ::aos::Sender<::aos::RobotState> robot_state_sender_;
+  ::aos::Sender<::aos::JoystickState> joystick_state_sender_;
+
+  ::aos::PhasedLoopHandler *send_robot_state_phased_loop_ = nullptr;
+  ::aos::TimerHandler *send_joystick_state_timer_ = nullptr;
+};
+
+typedef ControlLoopTestTemplated<::testing::Test> ControlLoopTest;
+
+template <typename TestBaseClass>
+constexpr ::std::chrono::microseconds
+    ControlLoopTestTemplated<TestBaseClass>::kTimeTick;
+
+template <typename TestBaseClass>
+constexpr ::std::chrono::milliseconds
+    ControlLoopTestTemplated<TestBaseClass>::kDSPacketTime;
+
+}  // namespace testing
+}  // namespace frc971
+
+#endif  // AOS_CONTROLS_CONTROL_LOOP_TEST_H_
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index ec480c0..f44abe0 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -315,8 +315,8 @@
         ":localizer",
         ":spline_goal_fbs",
         "//aos:math",
-        "//aos/controls:control_loop",
-        "//aos/controls:polytope",
+        "//frc971/control_loops:control_loop",
+        "//frc971/control_loops:polytope",
         "//frc971/input:robot_state_fbs",
         "//aos/util:log_interval",
         "//aos/util:trapezoid_profile",
@@ -344,7 +344,7 @@
         ":drivetrain_states",
         ":gear",
         "//aos:math",
-        "//aos/controls:polytope",
+        "//frc971/control_loops:polytope",
         "//frc971/control_loops:coerce_goal",
         "//frc971/control_loops:control_loops_fbs",
         ":spline_goal_fbs",
@@ -425,7 +425,7 @@
         ":spline_goal_fbs",
         ":splinedrivetrain",
         ":ssdrivetrain",
-        "//aos/controls:control_loop",
+        "//frc971/control_loops:control_loop",
         "//aos/util:log_interval",
         "//frc971/control_loops:runge_kutta",
         "//frc971/queues:gyro_fbs",
@@ -497,7 +497,7 @@
         ":drivetrain_position_fbs",
         ":drivetrain_output_fbs",
         ":drivetrain_test_lib",
-        "//aos/controls:control_loop_test",
+        "//frc971/control_loops:control_loop_test",
         "//aos/events/logging:log_writer",
         "//aos/testing:googletest",
         "//frc971/queues:gyro_fbs",
@@ -728,7 +728,7 @@
     deps = [
         ":drivetrain_config",
         ":drivetrain_status_fbs",
-        "//aos/controls:quaternion_utils",
+        "//frc971/control_loops:quaternion_utils",
         "//aos/events:event_loop",
         "//frc971/control_loops:control_loops_fbs",
         "//frc971/control_loops:runge_kutta",
@@ -746,7 +746,7 @@
     target_compatible_with = ["@platforms//os:linux"],
     deps = [
         ":drivetrain_test_lib",
-        "//aos/controls:quaternion_utils",
+        "//frc971/control_loops:quaternion_utils",
         "//aos/testing:googletest",
         "//aos/testing:random_seed",
         "//frc971/control_loops/drivetrain:improved_down_estimator",
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 91f1513..5242550 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -2,10 +2,11 @@
 
 #include <sched.h>
 #include <stdio.h>
+
 #include <cmath>
 #include <memory>
-#include "Eigen/Dense"
 
+#include "Eigen/Dense"
 #include "aos/logging/logging.h"
 #include "frc971/control_loops/drivetrain/down_estimator.h"
 #include "frc971/control_loops/drivetrain/drivetrain_config.h"
@@ -47,7 +48,7 @@
       right_high_requested_(dt_config_.default_high_gear) {
   last_voltage_.setZero();
   last_last_voltage_.setZero();
-  aos::controls::HPolytope<0>::Init();
+  frc971::controls::HPolytope<0>::Init();
   event_loop->OnRun([this]() {
     // On the first fetch, make sure that we are caught all the way up to the
     // present.
@@ -300,8 +301,8 @@
                                ::aos::EventLoop *event_loop,
                                LocalizerInterface *localizer,
                                const ::std::string &name)
-    : aos::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
-                                                                 name),
+    : frc971::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
+                                                                    name),
       dt_config_(dt_config),
       filters_(dt_config, event_loop, localizer),
       dt_openloop_(dt_config_, filters_.kf()),
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index 325715f..efaf53d 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -2,9 +2,8 @@
 #define FRC971_CONTROL_LOOPS_DRIVETRAIN_H_
 
 #include "Eigen/Dense"
-
-#include "aos/controls/control_loop.h"
-#include "aos/controls/polytope.h"
+#include "frc971/control_loops/control_loop.h"
+#include "frc971/control_loops/polytope.h"
 #include "aos/util/log_interval.h"
 #include "frc971/control_loops/control_loops_generated.h"
 #include "frc971/control_loops/drivetrain/drivetrain_config.h"
@@ -142,7 +141,7 @@
 };
 
 class DrivetrainLoop
-    : public aos::controls::ControlLoop<Goal, Position, Status, Output> {
+    : public frc971::controls::ControlLoop<Goal, Position, Status, Output> {
  public:
   // Note that we only actually store N - 1 splines, since we need to keep one
   // fetcher free to check whether there are any new splines.
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 64fb0b8..749b998 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -3,16 +3,11 @@
 #include <chrono>
 #include <memory>
 
-#include "aos/controls/control_loop_test.h"
-#include "aos/controls/polytope.h"
 #include "aos/events/event_loop.h"
 #include "aos/events/logging/log_writer.h"
 #include "aos/time/time.h"
-#include "gflags/gflags.h"
-#include "gtest/gtest.h"
-#include "gmock/gmock.h"
-
 #include "frc971/control_loops/coerce_goal.h"
+#include "frc971/control_loops/control_loop_test.h"
 #include "frc971/control_loops/drivetrain/drivetrain.h"
 #include "frc971/control_loops/drivetrain/drivetrain_config.h"
 #include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
@@ -20,9 +15,13 @@
 #include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
 #include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
 #include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
-#include "frc971/control_loops/drivetrain/trajectory_generator.h"
 #include "frc971/control_loops/drivetrain/localizer_generated.h"
+#include "frc971/control_loops/drivetrain/trajectory_generator.h"
+#include "frc971/control_loops/polytope.h"
 #include "frc971/queues/gyro_generated.h"
+#include "gflags/gflags.h"
+#include "gmock/gmock.h"
+#include "gtest/gtest.h"
 
 DEFINE_string(output_file, "",
               "If set, logs all channels to the provided logfile.");
@@ -35,10 +34,10 @@
 namespace chrono = ::std::chrono;
 using ::aos::monotonic_clock;
 
-class DrivetrainTest : public ::aos::testing::ControlLoopTest {
+class DrivetrainTest : public ::frc971::testing::ControlLoopTest {
  protected:
   DrivetrainTest()
-      : ::aos::testing::ControlLoopTest(
+      : ::frc971::testing::ControlLoopTest(
             aos::configuration::ReadConfig(
                 "frc971/control_loops/drivetrain/simulation_config.json"),
             GetTestDrivetrainConfig().dt),
@@ -581,7 +580,6 @@
   VerifyNearSplineGoal();
 }
 
-
 // Tests that we can drive a spline backwards.
 TEST_F(DrivetrainTest, SplineSimpleBackwards) {
   SetEnabled(true);
@@ -741,7 +739,6 @@
     EXPECT_FALSE(CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())
                      ->has_y());
   }
-
 }
 
 // Tests that a spline can't be restarted.
@@ -869,8 +866,8 @@
 }
 
 INSTANTIATE_TEST_SUITE_P(DriveSplinesForwardsAndBackwards,
-                        DrivetrainBackwardsParamTest,
-                        ::testing::Values(false, true));
+                         DrivetrainBackwardsParamTest,
+                         ::testing::Values(false, true));
 
 // Tests that simple spline converges when it starts to the side of where it
 // thinks.
diff --git a/frc971/control_loops/drivetrain/improved_down_estimator.cc b/frc971/control_loops/drivetrain/improved_down_estimator.cc
index 56e7536..2c129f7 100644
--- a/frc971/control_loops/drivetrain/improved_down_estimator.cc
+++ b/frc971/control_loops/drivetrain/improved_down_estimator.cc
@@ -2,8 +2,7 @@
 
 #include "Eigen/Dense"
 #include "Eigen/Geometry"
-
-#include "aos/controls/quaternion_utils.h"
+#include "frc971/control_loops/quaternion_utils.h"
 
 namespace frc971 {
 namespace control_loops {
@@ -80,7 +79,7 @@
 
   // We now have the sigma points after the model update.
   // Compute the mean of the transformed sigma point
-  X_hat_ = Eigen::Quaternion<double>(aos::controls::QuaternionMean(Y));
+  X_hat_ = Eigen::Quaternion<double>(frc971::controls::QuaternionMean(Y));
 
   // And the covariance.
   Eigen::Matrix<double, 3, 2 * 3 + 1> Wprime;
@@ -159,7 +158,7 @@
 
   // Update X_hat and the covariance P
   X_hat_ = X_hat_ * Eigen::Quaternion<double>(
-                        aos::controls::ToQuaternionFromRotationVector(
+                        frc971::controls::ToQuaternionFromRotationVector(
                             K * (measurement - Z_hat_)));
   P_ = P_prior - K * P_vv * K.transpose();
 }
@@ -215,7 +214,7 @@
   for (int i = 0; i < 7; ++i) {
     // Compute the error vector for each sigma point.
     Eigen::Matrix<double, 3, 1> Wprimei =
-        aos::controls::ToRotationVectorFromQuaternion(
+        frc971::controls::ToRotationVectorFromQuaternion(
             Eigen::Quaternion<double>(mean).conjugate() *
             Eigen::Quaternion<double>(points.col(i)));
     // Now, compute the contribution of this sigma point to P_prior.
@@ -241,7 +240,7 @@
   Eigen::Matrix<double, 4, 3 * 2 + 1> X;
   for (int i = 0; i < 3; ++i) {
     Eigen::Quaternion<double> perturbation(
-        aos::controls::ToQuaternionFromRotationVector(S.col(i), M_PI_2));
+        frc971::controls::ToQuaternionFromRotationVector(S.col(i), M_PI_2));
 
     X.col(i * 2) = (mean * perturbation).coeffs();
     X.col(i * 2 + 1) = (mean * perturbation.conjugate()).coeffs();
diff --git a/frc971/control_loops/drivetrain/improved_down_estimator_test.cc b/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
index 9894f68..8aed6b2 100644
--- a/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
+++ b/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
@@ -3,7 +3,7 @@
 #include <Eigen/Geometry>
 #include <random>
 
-#include "aos/controls/quaternion_utils.h"
+#include "frc971/control_loops/quaternion_utils.h"
 #include "aos/testing/random_seed.h"
 #include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
 #include "frc971/control_loops/runge_kutta.h"
@@ -235,7 +235,7 @@
       drivetrain::GenerateSigmaPoints(mean, covariance);
 
   const Eigen::Matrix<double, 4, 1> calculated_mean =
-      aos::controls::QuaternionMean(vectors);
+      frc971::controls::QuaternionMean(vectors);
 
   VLOG(1) << "actual mean: " << mean.coeffs();
   VLOG(1) << "calculated mean: " << calculated_mean;
@@ -266,7 +266,7 @@
       drivetrain::GenerateSigmaPoints(mean, covariance);
 
   const Eigen::Matrix<double, 4, 1> calculated_mean =
-      aos::controls::QuaternionMean(vectors);
+      frc971::controls::QuaternionMean(vectors);
 
   Eigen::Matrix<double, 3, 3 * 2 + 1> Wprime;
   Eigen::Matrix<double, 3, 3> calculated_covariance =
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.h b/frc971/control_loops/drivetrain/polydrivetrain.h
index fe733bb..aea030e 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.h
+++ b/frc971/control_loops/drivetrain/polydrivetrain.h
@@ -2,7 +2,7 @@
 #define FRC971_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_H_
 
 #include "aos/commonmath.h"
-#include "aos/controls/polytope.h"
+#include "frc971/control_loops/polytope.h"
 #include "frc971/control_loops/coerce_goal.h"
 #include "frc971/control_loops/drivetrain/gear.h"
 #ifdef __linux__
@@ -80,7 +80,7 @@
 
   StateFeedbackLoop<7, 2, 4, Scalar> *kf_;
 
-  const ::aos::controls::HVPolytope<2, 4, 4, Scalar> U_Poly_;
+  const ::frc971::controls::HVPolytope<2, 4, 4, Scalar> U_Poly_;
 
   ::std::unique_ptr<StateFeedbackLoop<2, 2, 2, Scalar>> loop_;
 
@@ -340,12 +340,12 @@
       const Scalar equality_w = kZero;
 
       // Construct a constraint on R by manipulating the constraint on U
-      ::aos::controls::HVPolytope<2, 4, 4, Scalar> R_poly_hv(
+      ::frc971::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>(
+              ::frc971::controls::ShiftPoints<2, 4, Scalar>(
                   U_Poly_.StaticVertices(),
                   loop_->controller().K() * loop_->X_hat()));
 
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.cc b/frc971/control_loops/drivetrain/ssdrivetrain.cc
index 3e58d09..594175c 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.cc
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.cc
@@ -1,8 +1,7 @@
 #include "frc971/control_loops/drivetrain/ssdrivetrain.h"
 
 #include "aos/commonmath.h"
-#include "aos/controls/polytope.h"
-
+#include "frc971/control_loops/polytope.h"
 #include "frc971/control_loops/coerce_goal.h"
 #include "frc971/control_loops/drivetrain/drivetrain_config.h"
 #include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
@@ -33,10 +32,10 @@
           (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),
+      linear_profile_(::frc971::controls::kLoopFrequency),
+      angular_profile_(::frc971::controls::kLoopFrequency),
       localizer_(localizer) {
-  ::aos::controls::HPolytope<0>::Init();
+  ::frc971::controls::HPolytope<0>::Init();
   T_ << 1, 1, 1, -1;
   T_inverse_ = T_.inverse();
   unprofiled_goal_.setZero();
@@ -87,13 +86,13 @@
       error_K * Eigen::Matrix<double, 2, 1>(kf_->X_hat(kLeftError),
                                             kf_->X_hat(kRightError));
 
-  const ::aos::controls::HVPolytope<2, 4, 4> pos_poly_hv(
+  const ::frc971::controls::HVPolytope<2, 4, 4> pos_poly_hv(
       U_poly_.static_H() * position_K * T_,
       U_poly_.static_H() *
               (-velocity_K * velocity_error + U_integral - kf_->ff_U()) +
           (U_poly_.static_k() * max_voltage_),
       (position_K * T_).inverse() *
-          ::aos::controls::ShiftPoints<2, 4, double>(
+          ::frc971::controls::ShiftPoints<2, 4, double>(
               (U_poly_.StaticVertices() * max_voltage_),
               -velocity_K * velocity_error + U_integral - kf_->ff_U()));
 
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.h b/frc971/control_loops/drivetrain/ssdrivetrain.h
index d83473d..be42abf 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.h
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.h
@@ -2,10 +2,9 @@
 #define FRC971_CONTROL_LOOPS_DRIVETRAIN_SSDRIVETRAIN_H_
 
 #include "aos/commonmath.h"
-#include "aos/controls/control_loop.h"
-#include "aos/controls/polytope.h"
+#include "frc971/control_loops/control_loop.h"
+#include "frc971/control_loops/polytope.h"
 #include "aos/util/trapezoid_profile.h"
-
 #include "frc971/control_loops/coerce_goal.h"
 #include "frc971/control_loops/control_loops_generated.h"
 #include "frc971/control_loops/drivetrain/drivetrain_config.h"
@@ -56,7 +55,7 @@
 
   // Reprsents +/- full power on each motor in U-space, aka the square from
   // (-12, -12) to (12, 12).
-  const ::aos::controls::HVPolytope<2, 4, 4> U_poly_;
+  const ::frc971::controls::HVPolytope<2, 4, 4> U_poly_;
 
   // multiplying by T converts [left_error, right_error] to
   // [left_right_error_difference, total_distance_error].
diff --git a/frc971/control_loops/hybrid_state_feedback_loop.h b/frc971/control_loops/hybrid_state_feedback_loop.h
index f033a89..4dab950 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop.h
+++ b/frc971/control_loops/hybrid_state_feedback_loop.h
@@ -11,7 +11,7 @@
 #include "Eigen/Dense"
 #include "unsupported/Eigen/MatrixFunctions"
 
-#include "aos/controls/control_loop.h"
+#include "frc971/control_loops/control_loop.h"
 #include "aos/logging/logging.h"
 #include "aos/macros.h"
 #include "frc971/control_loops/c2d.h"
diff --git a/frc971/control_loops/polytope.h b/frc971/control_loops/polytope.h
new file mode 100644
index 0000000..1d707d4
--- /dev/null
+++ b/frc971/control_loops/polytope.h
@@ -0,0 +1,240 @@
+#ifndef AOS_CONTROLS_POLYTOPE_H_
+#define AOS_CONTROLS_POLYTOPE_H_
+
+#include "Eigen/Dense"
+
+#ifdef __linux__
+#include "third_party/cddlib/lib-src/setoper.h"
+#include "third_party/cddlib/lib-src/cdd.h"
+
+#include "glog/logging.h"
+#endif  // __linux__
+
+namespace frc971 {
+namespace controls {
+
+// A number_of_dimensions dimensional polytope.
+// This represents the region consisting of all points X such that H * X <= k.
+// The vertices are calculated at construction time because we always use those
+// and libcdd is annoying about calculating vertices. In particular, for some
+// random-seeming polytopes it refuses to calculate the vertices completely. To
+// avoid issues with that, using the "shifting" constructor is recommended
+// whenever possible.
+template <int number_of_dimensions, typename Scalar = double>
+class Polytope {
+ public:
+  virtual ~Polytope() {}
+
+  // Returns a reference to H.
+  virtual Eigen::Ref<
+      const Eigen::Matrix<Scalar, Eigen::Dynamic, number_of_dimensions>>
+  H() const = 0;
+
+  // Returns a reference to k.
+  virtual Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>> k()
+      const = 0;
+
+  // Returns the number of dimensions in the polytope.
+  constexpr int ndim() const { return number_of_dimensions; }
+
+  // Returns the number of constraints currently in the polytope.
+  int num_constraints() const { return k().rows(); }
+
+  // Returns true if the point is inside the polytope.
+  bool IsInside(Eigen::Matrix<Scalar, number_of_dimensions, 1> point) const;
+
+  // Returns the list of vertices inside the polytope.
+  virtual Eigen::Matrix<Scalar, number_of_dimensions, Eigen::Dynamic> Vertices()
+      const = 0;
+};
+
+template <int number_of_dimensions, int num_vertices, typename Scalar = double>
+Eigen::Matrix<Scalar, number_of_dimensions, num_vertices> ShiftPoints(
+    const Eigen::Matrix<Scalar, number_of_dimensions, num_vertices> &vertices,
+    const Eigen::Matrix<Scalar, number_of_dimensions, 1> &offset) {
+  Eigen::Matrix<Scalar, number_of_dimensions, num_vertices> ans = vertices;
+  for (int i = 0; i < num_vertices; ++i) {
+    ans.col(i) += offset;
+  }
+  return ans;
+}
+
+template <int number_of_dimensions, int num_constraints, int num_vertices,
+          typename Scalar = double>
+class HVPolytope : public Polytope<number_of_dimensions, Scalar> {
+ public:
+  // Constructs a polytope given the H and k matrices.
+  HVPolytope(Eigen::Ref<const Eigen::Matrix<Scalar, num_constraints,
+                                            number_of_dimensions>> H,
+             Eigen::Ref<const Eigen::Matrix<Scalar, num_constraints, 1>> k,
+             Eigen::Ref<const Eigen::Matrix<Scalar, number_of_dimensions,
+                                            num_vertices>> vertices)
+      : H_(H), k_(k), vertices_(vertices) {}
+
+  Eigen::Ref<const Eigen::Matrix<Scalar, num_constraints, number_of_dimensions>>
+  static_H() const {
+    return H_;
+  }
+
+  Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, number_of_dimensions>>
+  H() const override {
+    return H_;
+  }
+
+  Eigen::Ref<const Eigen::Matrix<Scalar, num_constraints, 1>> static_k()
+      const {
+    return k_;
+  }
+  Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>> k()
+      const override {
+    return k_;
+  }
+
+  Eigen::Matrix<Scalar, number_of_dimensions, Eigen::Dynamic> Vertices()
+      const override {
+    return vertices_;
+  }
+
+  Eigen::Matrix<Scalar, number_of_dimensions, num_vertices>
+  StaticVertices() const {
+    return vertices_;
+  }
+
+ private:
+  const Eigen::Matrix<Scalar, num_constraints, number_of_dimensions> H_;
+  const Eigen::Matrix<Scalar, num_constraints, 1> k_;
+  const Eigen::Matrix<Scalar, number_of_dimensions, num_vertices> vertices_;
+};
+
+
+
+#ifdef __linux__
+
+template <int number_of_dimensions>
+class HPolytope : public Polytope<number_of_dimensions> {
+ public:
+  // Constructs a polytope given the H and k matrices.
+  HPolytope(
+      Eigen::Ref<
+          const Eigen::Matrix<double, Eigen::Dynamic, number_of_dimensions>> H,
+      Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>> k)
+      : H_(H), k_(k), vertices_(CalculateVertices(H, k)) {}
+
+  // This is an initialization function shared across all instantiations of this
+  // template.
+  // This must be called at least once before creating any instances. It is
+  // not thread-safe.
+  static void Init() { dd_set_global_constants(); }
+
+  Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, number_of_dimensions>>
+  H() const override {
+    return H_;
+  }
+  Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>> k()
+      const override {
+    return k_;
+  }
+
+  // NOTE: If you are getting bizarre errors that you are tracing back to the
+  // vertex matrix being funky, please check that you correctly called Init().
+  Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic> Vertices()
+      const override {
+    return vertices_;
+  }
+
+ private:
+  const Eigen::Matrix<double, Eigen::Dynamic, number_of_dimensions> H_;
+  const Eigen::Matrix<double, Eigen::Dynamic, 1> k_;
+  const Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic> vertices_;
+
+  static Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic>
+  CalculateVertices(
+      Eigen::Ref<
+          const Eigen::Matrix<double, Eigen::Dynamic, number_of_dimensions>> &H,
+      Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>> &k);
+};
+
+#endif  // __linux__
+
+template <int number_of_dimensions, typename Scalar>
+bool Polytope<number_of_dimensions, Scalar>::IsInside(
+    Eigen::Matrix<Scalar, number_of_dimensions, 1> point) const {
+  Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>> k_ptr = k();
+  for (int i = 0; i < k_ptr.rows(); ++i) {
+    Scalar ev = (H().row(i) * point)(0, 0);
+    if (ev > k_ptr(i, 0)) {
+      return false;
+    }
+  }
+  return true;
+}
+
+#ifdef __linux__
+template <int number_of_dimensions>
+Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic>
+HPolytope<number_of_dimensions>::CalculateVertices(
+    Eigen::Ref<
+        const Eigen::Matrix<double, Eigen::Dynamic, number_of_dimensions>> &H,
+    Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>> &k) {
+  dd_MatrixPtr matrix = dd_CreateMatrix(k.rows(), number_of_dimensions + 1);
+
+  // Copy the data over. TODO(aschuh): Is there a better way?  I hate copying...
+  for (int i = 0; i < k.rows(); ++i) {
+    dd_set_d(matrix->matrix[i][0], k(i, 0));
+    for (int j = 0; j < number_of_dimensions; ++j) {
+      dd_set_d(matrix->matrix[i][j + 1], -H(i, j));
+    }
+  }
+
+  matrix->representation = dd_Inequality;
+  matrix->numbtype = dd_Real;
+
+  dd_ErrorType error;
+  dd_PolyhedraPtr polyhedra = dd_DDMatrix2Poly(matrix, &error);
+  if (error != dd_NoError || polyhedra == NULL) {
+    dd_WriteErrorMessages(stderr, error);
+    dd_FreeMatrix(matrix);
+    LOG(ERROR) << "bad H" << H;
+    LOG(ERROR) << "bad k_" << k;
+    LOG(FATAL) << "dd_DDMatrix2Poly failed";
+  }
+
+  dd_MatrixPtr vertex_matrix = dd_CopyGenerators(polyhedra);
+
+  int num_vertices = 0;
+  int num_rays = 0;
+  for (int i = 0; i < vertex_matrix->rowsize; ++i) {
+    if (dd_get_d(vertex_matrix->matrix[i][0]) == 0) {
+      num_rays += 1;
+    } else {
+      num_vertices += 1;
+    }
+  }
+
+  // Rays are unsupported right now.  This may change in the future.
+  CHECK_EQ(0, num_rays);
+
+  Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic> vertices(
+      number_of_dimensions, num_vertices);
+
+  int vertex_index = 0;
+  for (int i = 0; i < vertex_matrix->rowsize; ++i) {
+    if (dd_get_d(vertex_matrix->matrix[i][0]) != 0) {
+      for (int j = 0; j < number_of_dimensions; ++j) {
+        vertices(j, vertex_index) = dd_get_d(vertex_matrix->matrix[i][j + 1]);
+      }
+      ++vertex_index;
+    }
+  }
+  dd_FreeMatrix(vertex_matrix);
+  dd_FreePolyhedra(polyhedra);
+  dd_FreeMatrix(matrix);
+
+  return vertices;
+}
+#endif  // __linux__
+
+}  // namespace controls
+}  // namespace frc971
+
+#endif  // AOS_CONTROLS_POLYTOPE_H_
diff --git a/frc971/control_loops/polytope_test.cc b/frc971/control_loops/polytope_test.cc
new file mode 100644
index 0000000..849438d
--- /dev/null
+++ b/frc971/control_loops/polytope_test.cc
@@ -0,0 +1,102 @@
+#include "frc971/control_loops/polytope.h"
+
+#include <vector>
+
+#include "Eigen/Dense"
+#include "gtest/gtest.h"
+#include "gmock/gmock.h"
+
+#include "aos/testing/test_logging.h"
+
+namespace frc971 {
+namespace controls {
+
+class HPolytopeTest : public ::testing::Test {
+ protected:
+  HPolytope<2> Polytope1() {
+    return HPolytope<2>{
+        (Eigen::Matrix<double, 4, 2>() << 1, 0, -1, 0, 0, 1, 0, -1).finished(),
+        (Eigen::Matrix<double, 4, 1>() << 12.0, 12.0, 12.0, 12.0).finished()};
+  }
+  HPolytope<2> Polytope2() {
+    return HPolytope<2>{
+        (Eigen::Matrix<double, 4, 2>() << 1, 1, -1, -1, 0, 1, 0, -1).finished(),
+        (Eigen::Matrix<double, 4, 1>() << 12.0, 12.0, 12.0, 12.0).finished()};
+  }
+  HPolytope<1> Polytope3() {
+    return HPolytope<1>{(Eigen::Matrix<double, 2, 1>() << 1, -0.5).finished(),
+                        (Eigen::Matrix<double, 2, 1>() << 5.0, 2.0).finished()};
+  }
+  HPolytope<2> Polytope4() {
+    return HPolytope<2>{
+        (Eigen::Matrix<double, 4, 2>() << 1, 1, -1, -1, 1, -1, -1, 1)
+            .finished(),
+        (Eigen::Matrix<double, 4, 1>() << 2, -1, 2, -1).finished()};
+  }
+  HPolytope<2> Polytope5() {
+    return HPolytope<2>{
+        (Eigen::Matrix<double, 4, 2>() << 1, 1, -1, -1, 1, -1, -1, 1)
+            .finished(),
+        (Eigen::Matrix<double, 4, 1>() << 1.5, -0.5, 1.5, -0.5).finished()};
+  }
+
+  void SetUp() override {
+    ::aos::testing::EnableTestLogging();
+    ::aos::testing::ForcePrintLogsDuringTests();
+    HPolytope<0>::Init();
+  }
+
+  template <typename T>
+  ::std::vector<::std::vector<double>> MatrixToVectors(const T &matrix) {
+    ::std::vector<::std::vector<double>> r;
+    for (int i = 0; i < matrix.cols(); ++i) {
+      ::std::vector<double> col;
+      for (int j = 0; j < matrix.rows(); ++j) {
+        col.emplace_back(matrix(j, i));
+      }
+      r.emplace_back(col);
+    }
+    return r;
+  }
+
+  template <typename T>
+  ::std::vector<::testing::Matcher<::std::vector<double>>> MatrixToMatchers(
+      const T &matrix) {
+    ::std::vector<::testing::Matcher<::std::vector<double>>> r;
+    for (int i = 0; i < matrix.cols(); ++i) {
+      ::std::vector<::testing::Matcher<double>> col;
+      for (int j = 0; j < matrix.rows(); ++j) {
+        col.emplace_back(::testing::DoubleNear(matrix(j, i), 0.000001));
+      }
+      r.emplace_back(::testing::ElementsAreArray(col));
+    }
+    return r;
+  }
+};
+
+// Tests that the vertices for various polytopes calculated from H and k are
+// correct.
+TEST_F(HPolytopeTest, CalculatedVertices) {
+  EXPECT_THAT(MatrixToVectors(Polytope1().Vertices()),
+              ::testing::UnorderedElementsAreArray(
+                  MatrixToVectors((Eigen::Matrix<double, 2, 4>() << -12, -12,
+                                   12, 12, -12, 12, 12, -12).finished())));
+  EXPECT_THAT(MatrixToVectors(Polytope2().Vertices()),
+              ::testing::UnorderedElementsAreArray(
+                  MatrixToVectors((Eigen::Matrix<double, 2, 4>() << 24, 0, -24,
+                                   0, -12, 12, 12, -12).finished())));
+  EXPECT_THAT(MatrixToVectors(Polytope3().Vertices()),
+              ::testing::UnorderedElementsAreArray(MatrixToVectors(
+                  (Eigen::Matrix<double, 1, 2>() << 5, -4).finished())));
+  EXPECT_THAT(MatrixToVectors(Polytope4().Vertices()),
+              ::testing::UnorderedElementsAreArray(
+                  MatrixToVectors((Eigen::Matrix<double, 2, 4>() << 1, 1.5, 1.5,
+                                   2, 0, -0.5, 0.5, 0).finished())));
+  EXPECT_THAT(MatrixToVectors(Polytope5().Vertices()),
+              ::testing::UnorderedElementsAreArray(
+                  MatrixToVectors((Eigen::Matrix<double, 2, 4>() << 0.5, 1, 1.5,
+                                   1, 0, 0.5, 0, -0.5).finished())));
+}
+
+}  // namespace controls
+}  // namespace frc971
diff --git a/frc971/control_loops/profiled_subsystem.h b/frc971/control_loops/profiled_subsystem.h
index d16c17d..6780e3b 100644
--- a/frc971/control_loops/profiled_subsystem.h
+++ b/frc971/control_loops/profiled_subsystem.h
@@ -8,7 +8,7 @@
 
 #include "Eigen/Dense"
 
-#include "aos/controls/control_loop.h"
+#include "frc971/control_loops/control_loop.h"
 #include "aos/util/trapezoid_profile.h"
 #include "frc971/constants.h"
 #include "frc971/control_loops/control_loops_generated.h"
diff --git a/frc971/control_loops/quaternion_utils.cc b/frc971/control_loops/quaternion_utils.cc
new file mode 100644
index 0000000..0226e6d
--- /dev/null
+++ b/frc971/control_loops/quaternion_utils.cc
@@ -0,0 +1,139 @@
+#include "frc971/control_loops/quaternion_utils.h"
+
+#include "Eigen/Dense"
+#include "Eigen/Geometry"
+
+namespace frc971 {
+namespace controls {
+
+Eigen::Matrix<double, 4, 1> ToQuaternionFromRotationVector(
+    const Eigen::Matrix<double, 3, 1> &X, const double max_angle_cap) {
+  const double unclipped_angle = X.norm();
+  const double angle_scalar =
+      (unclipped_angle > max_angle_cap) ? max_angle_cap / unclipped_angle : 1.0;
+  const double angle = unclipped_angle * angle_scalar;
+  const double half_angle = angle * 0.5;
+
+  const double half_angle_squared = half_angle * half_angle;
+
+  // sin(x)/x = 1
+  double sinx_x = 1.0;
+
+  // - x^2/3!
+  double value = half_angle_squared / 6.0;
+  sinx_x -= value;
+
+  // + x^4/5!
+  value = value * half_angle_squared / 20.0;
+  sinx_x += value;
+
+  // - x^6/7!
+  value = value * half_angle_squared / (6.0 * 7.0);
+  sinx_x -= value;
+
+  // + x^8/9!
+  value = value * half_angle_squared / (8.0 * 9.0);
+  sinx_x += value;
+
+  // - x^10/11!
+  value = value * half_angle_squared / (10.0 * 11.0);
+  sinx_x -= value;
+
+  // + x^12/13!
+  value = value * half_angle_squared / (12.0 * 13.0);
+  sinx_x += value;
+
+  // - x^14/15!
+  value = value * half_angle_squared / (14.0 * 15.0);
+  sinx_x -= value;
+
+  // + x^16/17!
+  value = value * half_angle_squared / (16.0 * 17.0);
+  sinx_x += value;
+
+  // To plot the residual in matplotlib, run:
+  // import numpy
+  // import scipy
+  // from matplotlib import pyplot
+  // x = numpy.arange(-numpy.pi, numpy.pi, 0.01)
+  // pyplot.plot(x, 1 - x**2 / scipy.misc.factorial(3) +
+  //                   x**4 / scipy.misc.factorial(5) -
+  //                   x**6 / scipy.misc.factorial(7) +
+  //                   x**8 / scipy.misc.factorial(9) -
+  //                   x ** 10 / scipy.misc.factorial(11) +
+  //                   x ** 12 / scipy.misc.factorial(13) -
+  //                   x ** 14 / scipy.misc.factorial(15) +
+  //                   x ** 16 / scipy.misc.factorial(17) -
+  //                   numpy.sin(x) / x)
+
+  const double scalar = sinx_x * 0.5;
+
+  Eigen::Matrix<double, 4, 1> result;
+  result.block<3, 1>(0, 0) = X * scalar * angle_scalar;
+  result(3, 0) = std::cos(half_angle);
+  return result;
+}
+
+inline Eigen::Matrix<double, 4, 1> MaybeFlipX(
+    const Eigen::Matrix<double, 4, 1> &X) {
+  if (X(3, 0) < 0.0) {
+    return -X;
+  } else {
+    return X;
+  }
+}
+
+Eigen::Matrix<double, 3, 1> ToRotationVectorFromQuaternion(
+    const Eigen::Matrix<double, 4, 1> &X) {
+  // TODO(austin): Verify we still need it.
+  const Eigen::Matrix<double, 4, 1> corrected_X = MaybeFlipX(X);
+  const double half_angle =
+      std::atan2(corrected_X.block<3, 1>(0, 0).norm(), corrected_X(3, 0));
+
+  const double half_angle_squared = half_angle * half_angle;
+
+  // TODO(austin): We are doing a division at the end of this. Do the taylor
+  // series expansion of x/sin(x) instead to avoid this.
+
+  // sin(x)/x = 1
+  double sinx_x = 1.0;
+
+  // - x^2/3!
+  double value = half_angle_squared / 6.0;
+  sinx_x -= value;
+
+  // + x^4/5!
+  value = value * half_angle_squared / 20.0;
+  sinx_x += value;
+
+  // - x^6/7!
+  value = value * half_angle_squared / (6.0 * 7.0);
+  sinx_x -= value;
+
+  // + x^8/9!
+  value = value * half_angle_squared / (8.0 * 9.0);
+  sinx_x += value;
+
+  // - x^10/11!
+  value = value * half_angle_squared / (10.0 * 11.0);
+  sinx_x -= value;
+
+  // + x^12/13!
+  value = value * half_angle_squared / (12.0 * 13.0);
+  sinx_x += value;
+
+  // - x^14/15!
+  value = value * half_angle_squared / (14.0 * 15.0);
+  sinx_x -= value;
+
+  // + x^16/17!
+  value = value * half_angle_squared / (16.0 * 17.0);
+  sinx_x += value;
+
+  const double scalar = 2.0 / sinx_x;
+
+  return corrected_X.block<3, 1>(0, 0) * scalar;
+}
+
+}  // namespace controls
+}  // namespace frc971
diff --git a/frc971/control_loops/quaternion_utils.h b/frc971/control_loops/quaternion_utils.h
new file mode 100644
index 0000000..25ffa91
--- /dev/null
+++ b/frc971/control_loops/quaternion_utils.h
@@ -0,0 +1,71 @@
+#ifndef AOS_CONTROLS_QUATERNION_UTILS_H_
+#define AOS_CONTROLS_QUATERNION_UTILS_H_
+
+#include "Eigen/Dense"
+#include "Eigen/Geometry"
+#include "glog/logging.h"
+
+namespace frc971 {
+namespace controls {
+
+// Function to compute the quaternion average of a bunch of quaternions. Each
+// column in the input matrix is a quaternion (optionally scaled by it's
+// weight).
+template <int SM>
+inline Eigen::Matrix<double, 4, 1> QuaternionMean(
+    Eigen::Matrix<double, 4, SM> input) {
+  // Algorithm to compute the average of a bunch of quaternions:
+  // http://www.acsu.buffalo.edu/~johnc/ave_quat07.pdf
+
+  Eigen::Matrix<double, 4, 4> m = input * input.transpose();
+
+  Eigen::EigenSolver<Eigen::Matrix<double, 4, 4>> solver;
+  solver.compute(m);
+
+  Eigen::EigenSolver<Eigen::Matrix<double, 4, 4>>::EigenvectorsType
+      eigenvectors = solver.eigenvectors();
+  Eigen::EigenSolver<Eigen::Matrix<double, 4, 4>>::EigenvalueType eigenvalues =
+      solver.eigenvalues();
+
+  int max_index = 0;
+  double max_eigenvalue = 0.0;
+  for (int i = 0; i < 4; ++i) {
+    const double eigenvalue = std::abs(eigenvalues(i, 0));
+    if (eigenvalue > max_eigenvalue) {
+      max_eigenvalue = eigenvalue;
+      max_index = i;
+    }
+  }
+
+  // Assume that there shouldn't be any imaginary components to the eigenvector.
+  // I can't prove this is true, but everyone else seems to assume it...
+  // TODO(james): Handle this more rigorously.
+  for (int i = 0; i < 4; ++i) {
+    CHECK_LT(eigenvectors(i, max_index).imag(), 1e-4)
+        << eigenvectors(i, max_index);
+  }
+  return eigenvectors.col(max_index).real().normalized();
+}
+
+// Converts from a quaternion to a rotation vector, where the rotation vector's
+// direction represents the axis to rotate around and its magnitude represents
+// the number of radians to rotate.
+Eigen::Matrix<double, 3, 1> ToRotationVectorFromQuaternion(
+    const Eigen::Matrix<double, 4, 1> &X);
+
+inline Eigen::Matrix<double, 3, 1> ToRotationVectorFromQuaternion(
+    const Eigen::Quaternion<double> &X) {
+  return ToRotationVectorFromQuaternion(X.coeffs());
+}
+
+// Converts from a rotation vector to a quaternion. If you supply max_angle_cap,
+// then the rotation vector's magnitude will be clipped to be no more than
+// max_angle_cap before being converted to a quaternion.
+Eigen::Matrix<double, 4, 1> ToQuaternionFromRotationVector(
+    const Eigen::Matrix<double, 3, 1> &X,
+    const double max_angle_cap = std::numeric_limits<double>::infinity());
+
+}  // namespace controls
+}  // namespace frc971
+
+#endif  // AOS_CONTROLS_QUATERNION_UTILS_H_
diff --git a/frc971/control_loops/quaternion_utils_test.cc b/frc971/control_loops/quaternion_utils_test.cc
new file mode 100644
index 0000000..f472a46
--- /dev/null
+++ b/frc971/control_loops/quaternion_utils_test.cc
@@ -0,0 +1,152 @@
+#include "Eigen/Dense"
+
+#include <random>
+
+#include "frc971/control_loops/quaternion_utils.h"
+#include "aos/testing/random_seed.h"
+#include "glog/logging.h"
+#include "gtest/gtest.h"
+
+namespace frc971 {
+namespace controls {
+namespace testing {
+
+// Tests that small perturbations around a couple quaternions averaged out
+// return the original quaternion.
+TEST(DownEstimatorTest, QuaternionMean) {
+  Eigen::Matrix<double, 4, 7> vectors;
+  vectors.col(0) << 0, 0, 0, 1;
+  for (int i = 0; i < 3; ++i) {
+    Eigen::Matrix<double, 4, 1> perturbation;
+    perturbation.setZero();
+    perturbation(i, 0) = 0.1;
+
+    vectors.col(i * 2 + 1) = vectors.col(0) + perturbation;
+    vectors.col(i * 2 + 2) = vectors.col(0) - perturbation;
+  }
+
+  for (int i = 0; i < 7; ++i) {
+    vectors.col(i).normalize();
+  }
+
+  Eigen::Matrix<double, 4, 1> mean = QuaternionMean(vectors);
+
+  for (int i = 0; i < 4; ++i) {
+    EXPECT_NEAR(mean(i, 0), vectors(i, 0), 0.001) << ": Failed on index " << i;
+  }
+}
+
+// Tests that ToRotationVectorFromQuaternion works for a 0 rotation.
+TEST(DownEstimatorTest, ToRotationVectorFromQuaternionAtZero) {
+  Eigen::Matrix<double, 3, 1> vector = ToRotationVectorFromQuaternion(
+      Eigen::Quaternion<double>(
+          Eigen::AngleAxis<double>(0.0, Eigen::Vector3d::UnitX()))
+          .coeffs());
+
+  EXPECT_NEAR(0.0, (vector - Eigen::Vector3d::Zero()).norm(), 1e-4);
+}
+
+// Tests that ToRotationVectorFromQuaternion works for a real rotation.
+TEST(DownEstimatorTest, ToRotationVectorFromQuaternion) {
+  Eigen::Matrix<double, 3, 1> vector = ToRotationVectorFromQuaternion(
+      Eigen::Quaternion<double>(
+          Eigen::AngleAxis<double>(M_PI * 0.5, Eigen::Vector3d::UnitX()))
+          .coeffs());
+
+  EXPECT_NEAR(0.0, (vector - Eigen::Vector3d::UnitX() * M_PI * 0.5).norm(),
+              1e-4);
+}
+
+// Tests that ToRotationVectorFromQuaternion works for a solution with negative
+// coefficients.
+TEST(DownEstimatorTest, ToRotationVectorFromQuaternionNegative) {
+  Eigen::Matrix<double, 3, 1> vector = ToRotationVectorFromQuaternion(
+      Eigen::Quaternion<double>(
+          -Eigen::Quaternion<double>(
+               Eigen::AngleAxis<double>(M_PI * 0.5, Eigen::Vector3d::UnitX()))
+               .coeffs())
+          .coeffs());
+
+  EXPECT_NEAR(0.0, (vector - Eigen::Vector3d::UnitX() * M_PI * 0.5).norm(),
+              1e-4);
+}
+
+// Tests that ToQuaternionFromRotationVector works for a 0 rotation.
+TEST(DownEstimatorTest, ToQuaternionFromRotationVectorAtZero) {
+  Eigen::Matrix<double, 4, 1> quaternion =
+      ToQuaternionFromRotationVector(Eigen::Vector3d::Zero());
+
+  EXPECT_NEAR(
+      0.0,
+      (quaternion - Eigen::Quaternion<double>(
+                        Eigen::AngleAxis<double>(0.0, Eigen::Vector3d::UnitX()))
+                        .coeffs())
+          .norm(),
+      1e-4);
+}
+
+// Tests that ToQuaternionFromRotationVector works for a real rotation.
+TEST(DownEstimatorTest, ToQuaternionFromRotationVector) {
+  Eigen::Matrix<double, 4, 1> quaternion =
+      ToQuaternionFromRotationVector(Eigen::Vector3d::UnitX() * M_PI * 0.5);
+
+  EXPECT_NEAR(0.0,
+              (quaternion - Eigen::Quaternion<double>(
+                                Eigen::AngleAxis<double>(
+                                    M_PI * 0.5, Eigen::Vector3d::UnitX()))
+                                .coeffs())
+
+                  .norm(),
+              1e-4);
+}
+
+// Tests that ToQuaternionFromRotationVector correctly clips a rotation vector
+// that is too large in magnitude.
+TEST(DownEstimatorTest, ToQuaternionFromLargeRotationVector) {
+  const double kMaxAngle = 2.0;
+  const Eigen::Vector3d rotation_vector =
+      Eigen::Vector3d::UnitX() * kMaxAngle * 2.0;
+  const Eigen::Matrix<double, 3, 1> clipped_vector =
+      ToRotationVectorFromQuaternion(
+          ToQuaternionFromRotationVector(rotation_vector, kMaxAngle));
+
+  EXPECT_NEAR(0.0, (rotation_vector / 2.0 - clipped_vector).norm(), 1e-4);
+}
+
+// Tests that ToQuaternionFromRotationVector and ToRotationVectorFromQuaternion
+// works for random rotations.
+TEST(DownEstimatorTest, RandomQuaternions) {
+  std::mt19937 generator(aos::testing::RandomSeed());
+  std::uniform_real_distribution<double> random_scalar(-1.0, 1.0);
+
+  for (int i = 0; i < 1000; ++i) {
+    Eigen::Matrix<double, 3, 1> axis;
+    axis << random_scalar(generator), random_scalar(generator),
+        random_scalar(generator);
+    EXPECT_GE(axis.norm(), 1e-6);
+    axis.normalize();
+
+    const double angle = random_scalar(generator) * M_PI;
+
+    Eigen::Matrix<double, 4, 1> quaternion =
+        ToQuaternionFromRotationVector(axis * angle);
+
+    Eigen::Quaternion<double> answer(Eigen::AngleAxis<double>(angle, axis));
+
+    EXPECT_NEAR(quaternion(3, 0), std::cos(angle / 2.0), 1e-8);
+    EXPECT_NEAR(answer.w(), std::cos(angle / 2.0), 1e-8);
+
+    EXPECT_NEAR(1.0, (answer.coeffs() * quaternion.transpose()).norm(), 1e-6);
+
+    const Eigen::Matrix<double, 3, 1> recalculated_axis =
+        ToRotationVectorFromQuaternion(quaternion);
+
+    EXPECT_NEAR(std::abs(angle), recalculated_axis.norm(), 1e-8);
+
+    EXPECT_NEAR(0.0, (axis * angle - recalculated_axis).norm(), 1e-8);
+  }
+}
+
+}  // namespace testing
+}  // namespace controls
+}  // namespace frc971
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
index 1bebbe1..cb0f651 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
@@ -1,11 +1,10 @@
-#include "flatbuffers/flatbuffers.h"
-#include "gtest/gtest.h"
-
-#include "aos/controls/control_loop.h"
-#include "aos/controls/control_loop_test.h"
-#include "frc971/control_loops/capped_test_plant.h"
-#include "frc971/control_loops/position_sensor_sim.h"
 #include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
+
+#include "flatbuffers/flatbuffers.h"
+#include "frc971/control_loops/capped_test_plant.h"
+#include "frc971/control_loops/control_loop.h"
+#include "frc971/control_loops/control_loop_test.h"
+#include "frc971/control_loops/position_sensor_sim.h"
 #include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test_absolute_encoder_status_generated.h"
 #include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test_absolute_position_generated.h"
 #include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test_integral_plant.h"
@@ -15,6 +14,7 @@
 #include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test_subsystem_goal_generated.h"
 #include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test_subsystem_output_generated.h"
 #include "frc971/zeroing/zeroing.h"
+#include "gtest/gtest.h"
 
 using ::frc971::control_loops::PositionSensorSimulator;
 
@@ -163,7 +163,8 @@
     typename ::aos::Sender<PositionType>::Builder position =
         subsystem_position_sender_.MakeBuilder();
 
-    auto real_position_builder = position.template MakeBuilder<RealPositionType>();
+    auto real_position_builder =
+        position.template MakeBuilder<RealPositionType>();
     flatbuffers::Offset<RealPositionType> position_offset =
         this->subsystem_sensor_sim_
             .template GetSensorValues<typename RealPositionType::Builder>(
@@ -262,7 +263,7 @@
 // to wrap it.
 template <typename QueueGroup, typename SZSDPS>
 class Subsystem
-    : public ::aos::controls::ControlLoop<
+    : public ::frc971::controls::ControlLoop<
           typename QueueGroup::Goal, typename QueueGroup::Position,
           typename QueueGroup::Status, typename QueueGroup::Output> {
  public:
@@ -272,7 +273,7 @@
   typedef typename QueueGroup::Output OutputType;
 
   Subsystem(::aos::EventLoop *event_loop, const ::std::string &name)
-      : aos::controls::ControlLoop<
+      : frc971::controls::ControlLoop<
             typename QueueGroup::Goal, typename QueueGroup::Position,
             typename QueueGroup::Status, typename QueueGroup::Output>(
             event_loop, name),
@@ -347,7 +348,7 @@
 };
 
 template <typename TSZSDPS>
-class IntakeSystemTest : public ::aos::testing::ControlLoopTest {
+class IntakeSystemTest : public ::frc971::testing::ControlLoopTest {
  protected:
   using SZSDPS = typename TSZSDPS::first_type;
   using QueueGroup = typename TSZSDPS::second_type;
@@ -360,7 +361,7 @@
   typedef typename QueueGroup::Output OutputType;
 
   IntakeSystemTest()
-      : ::aos::testing::ControlLoopTest(
+      : ::frc971::testing::ControlLoopTest(
             aos::configuration::ReadConfig("frc971/control_loops/"
                                            "static_zeroing_single_dof_profiled_"
                                            "subsystem_test_config.json"),
diff --git a/frc971/control_loops/zeroed_joint.h b/frc971/control_loops/zeroed_joint.h
index 41269eb..449cee1 100644
--- a/frc971/control_loops/zeroed_joint.h
+++ b/frc971/control_loops/zeroed_joint.h
@@ -3,7 +3,7 @@
 
 #include <memory>
 
-#include "aos/controls/control_loop.h"
+#include "frc971/control_loops/control_loop.h"
 #include "frc971/control_loops/state_feedback_loop.h"
 
 namespace frc971 {