diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index 773b6cb..3d6c930 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -243,6 +243,17 @@
     ],
 )
 
+queue_library(
+    name = "static_zeroing_single_dof_profiled_subsystem_test_queue",
+    srcs = [
+        "static_zeroing_single_dof_profiled_subsystem_test.q",
+    ],
+    deps = [
+        ":profiled_subsystem_queue",
+        ":queues",
+    ],
+)
+
 cc_library(
     name = "profiled_subsystem",
     srcs = [
@@ -407,6 +418,7 @@
         ":position_sensor_sim",
         ":static_zeroing_single_dof_profiled_subsystem",
         ":static_zeroing_single_dof_profiled_subsystem_test_plants",
+        ":static_zeroing_single_dof_profiled_subsystem_test_queue",
         "//aos/controls:control_loop_test",
         "//aos/testing:googletest",
     ],
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index b45f016..b9d2bc4 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -93,7 +93,8 @@
     const ::frc971::control_loops::DrivetrainQueue::Position *position,
     ::frc971::control_loops::DrivetrainQueue::Output *output,
     ::frc971::control_loops::DrivetrainQueue::Status *status) {
-  monotonic_clock::time_point monotonic_now = monotonic_clock::now();
+  const monotonic_clock::time_point monotonic_now =
+      event_loop()->monotonic_now();
 
   if (!has_been_enabled_ && output) {
     has_been_enabled_ = true;
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 3f20554..e4de9bb 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -10,9 +10,6 @@
 #include "aos/time/time.h"
 #include "gflags/gflags.h"
 #include "gtest/gtest.h"
-#if defined(SUPPORT_PLOT)
-#include "third_party/matplotlib-cpp/matplotlibcpp.h"
-#endif
 
 #include "frc971/control_loops/coerce_goal.h"
 #include "frc971/control_loops/drivetrain/drivetrain.h"
@@ -22,8 +19,6 @@
 #include "frc971/control_loops/drivetrain/localizer.q.h"
 #include "frc971/queues/gyro.q.h"
 
-DEFINE_bool(plot, false, "If true, plot");
-
 namespace frc971 {
 namespace control_loops {
 namespace drivetrain {
@@ -34,92 +29,59 @@
 
 class DrivetrainTest : public ::aos::testing::ControlLoopTest {
  protected:
-  // Create a new instance of the test queue so that it invalidates the queue
-  // that it points to.  Otherwise, we will have a pointer to shared memory that
-  // is no longer valid.
-  ::frc971::control_loops::DrivetrainQueue my_drivetrain_queue_;
-
-  ::aos::ShmEventLoop event_loop_;
-  const DrivetrainConfig<double> dt_config_;
-  DeadReckonEkf localizer_;
-  // Create a loop and simulation plant.
-  DrivetrainLoop drivetrain_motor_;
-  ::aos::ShmEventLoop simulation_event_loop_;
-  DrivetrainSimulation drivetrain_motor_plant_;
-
   DrivetrainTest()
-      : my_drivetrain_queue_(".frc971.control_loops.drivetrain_queue",
-                             ".frc971.control_loops.drivetrain_queue.goal",
-                             ".frc971.control_loops.drivetrain_queue.position",
-                             ".frc971.control_loops.drivetrain_queue.output",
-                             ".frc971.control_loops.drivetrain_queue.status"),
+      : ::aos::testing::ControlLoopTest(GetTestDrivetrainConfig().dt),
+        test_event_loop_(MakeEventLoop()),
+        drivetrain_goal_sender_(
+            test_event_loop_
+                ->MakeSender<::frc971::control_loops::DrivetrainQueue::Goal>(
+                    ".frc971.control_loops.drivetrain_queue.goal")),
+        drivetrain_goal_fetcher_(
+            test_event_loop_
+                ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Goal>(
+                    ".frc971.control_loops.drivetrain_queue.goal")),
+        drivetrain_status_fetcher_(
+            test_event_loop_
+                ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Status>(
+                    ".frc971.control_loops.drivetrain_queue.status")),
+        drivetrain_output_fetcher_(
+            test_event_loop_
+                ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Output>(
+                    ".frc971.control_loops.drivetrain_queue.output")),
+        localizer_control_sender_(
+            test_event_loop_->MakeSender<LocalizerControl>(
+                ".frc971.control_loops.drivetrain.localizer_control")),
+        drivetrain_event_loop_(MakeEventLoop()),
         dt_config_(GetTestDrivetrainConfig()),
-        localizer_(dt_config_),
-        drivetrain_motor_(dt_config_, &event_loop_, &localizer_),
-        drivetrain_motor_plant_(&simulation_event_loop_, dt_config_) {
+        localizer_(drivetrain_event_loop_.get(), dt_config_),
+        drivetrain_(dt_config_, drivetrain_event_loop_.get(), &localizer_),
+        drivetrain_plant_event_loop_(MakeEventLoop()),
+        drivetrain_plant_(drivetrain_plant_event_loop_.get(), dt_config_) {
     set_battery_voltage(12.0);
   }
+  virtual ~DrivetrainTest() {}
 
-  void RunIteration() {
-    drivetrain_motor_plant_.SendPositionMessage();
-    drivetrain_motor_.Iterate();
-    drivetrain_motor_plant_.Simulate();
-    SimulateTimestep(true, dt_config_.dt);
-    if (FLAGS_plot) {
-      my_drivetrain_queue_.status.FetchLatest();
-
-      ::Eigen::Matrix<double, 2, 1> actual_position =
-          drivetrain_motor_plant_.GetPosition();
-      actual_x_.push_back(actual_position(0));
-      actual_y_.push_back(actual_position(1));
-
-      trajectory_x_.push_back(my_drivetrain_queue_.status->trajectory_logging.x);
-      trajectory_y_.push_back(my_drivetrain_queue_.status->trajectory_logging.y);
-    }
-  }
-
-  void RunForTime(monotonic_clock::duration run_for) {
-    const auto end_time = monotonic_clock::now() + run_for;
-    while (monotonic_clock::now() < end_time) {
-      RunIteration();
-    }
-  }
-
-  void TearDown() {
-#if defined(SUPPORT_PLOT)
-    if (FLAGS_plot) {
-      std::cout << "plotting.\n";
-      matplotlibcpp::figure();
-      matplotlibcpp::plot(actual_x_, actual_y_, {{"label", "actual position"}});
-      matplotlibcpp::plot(trajectory_x_, trajectory_y_,
-                          {{"label", "trajectory position"}});
-      matplotlibcpp::legend();
-      matplotlibcpp::show();
-    }
-#endif
-  }
+  void TearDown() { drivetrain_plant_.MaybePlot(); }
 
   void VerifyNearGoal() {
-    my_drivetrain_queue_.goal.FetchLatest();
-    my_drivetrain_queue_.position.FetchLatest();
-    EXPECT_NEAR(my_drivetrain_queue_.goal->left_goal,
-                drivetrain_motor_plant_.GetLeftPosition(), 1e-3);
-    EXPECT_NEAR(my_drivetrain_queue_.goal->right_goal,
-                drivetrain_motor_plant_.GetRightPosition(), 1e-3);
+    drivetrain_goal_fetcher_.Fetch();
+    EXPECT_NEAR(drivetrain_goal_fetcher_->left_goal,
+                drivetrain_plant_.GetLeftPosition(), 1e-3);
+    EXPECT_NEAR(drivetrain_goal_fetcher_->right_goal,
+                drivetrain_plant_.GetRightPosition(), 1e-3);
   }
 
   void VerifyNearPosition(double x, double y) {
-    my_drivetrain_queue_.status.FetchLatest();
-    auto actual = drivetrain_motor_plant_.GetPosition();
+    auto actual = drivetrain_plant_.GetPosition();
     EXPECT_NEAR(actual(0), x, 1e-2);
     EXPECT_NEAR(actual(1), y, 1e-2);
   }
 
   void VerifyNearSplineGoal() {
-    my_drivetrain_queue_.status.FetchLatest();
-    double expected_x = my_drivetrain_queue_.status->trajectory_logging.x;
-    double expected_y = my_drivetrain_queue_.status->trajectory_logging.y;
-    auto actual = drivetrain_motor_plant_.GetPosition();
+    drivetrain_status_fetcher_.Fetch();
+    const double expected_x = drivetrain_status_fetcher_->trajectory_logging.x;
+    const double expected_y = drivetrain_status_fetcher_->trajectory_logging.y;
+    const ::Eigen::Vector2d actual = drivetrain_plant_.GetPosition();
     EXPECT_NEAR(actual(0), expected_x, 2e-2);
     EXPECT_NEAR(actual(1), expected_y, 2e-2);
   }
@@ -128,104 +90,117 @@
     do {
       // Run for fewer iterations while the worker thread computes.
       ::std::this_thread::sleep_for(::std::chrono::milliseconds(5));
-      RunIteration();
-      my_drivetrain_queue_.status.FetchLatest();
-    } while (my_drivetrain_queue_.status->trajectory_logging.planning_state !=
-        (int8_t)SplineDrivetrain::PlanState::kPlannedTrajectory);
+      RunFor(dt());
+      EXPECT_TRUE(drivetrain_status_fetcher_.Fetch());
+    } while (drivetrain_status_fetcher_->trajectory_logging.planning_state !=
+             (int8_t)SplineDrivetrain::PlanState::kPlannedTrajectory);
   }
 
   void WaitForTrajectoryExecution() {
     do {
-      RunIteration();
-      my_drivetrain_queue_.status.FetchLatest();
-    } while (!my_drivetrain_queue_.status->trajectory_logging.is_executed);
+      RunFor(dt());
+      EXPECT_TRUE(drivetrain_status_fetcher_.Fetch());
+    } while (!drivetrain_status_fetcher_->trajectory_logging.is_executed);
   }
 
-  virtual ~DrivetrainTest() {}
+  ::std::unique_ptr<::aos::EventLoop> test_event_loop_;
+  ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Goal>
+      drivetrain_goal_sender_;
+  ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Goal>
+      drivetrain_goal_fetcher_;
+  ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Status>
+      drivetrain_status_fetcher_;
+  ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Output>
+      drivetrain_output_fetcher_;
+  ::aos::Sender<LocalizerControl> localizer_control_sender_;
 
- private:
-  ::std::vector<double> actual_x_;
-  ::std::vector<double> actual_y_;
-  ::std::vector<double> trajectory_x_;
-  ::std::vector<double> trajectory_y_;
+  ::std::unique_ptr<::aos::EventLoop> drivetrain_event_loop_;
+  const DrivetrainConfig<double> dt_config_;
+  DeadReckonEkf localizer_;
+  // Create a loop and simulation plant.
+  DrivetrainLoop drivetrain_;
 
+  ::std::unique_ptr<::aos::EventLoop> drivetrain_plant_event_loop_;
+  DrivetrainSimulation drivetrain_plant_;
 };
 
 // Tests that the drivetrain converges on a goal.
 TEST_F(DrivetrainTest, ConvergesCorrectly) {
-  my_drivetrain_queue_.goal.MakeWithBuilder()
-      .controller_type(1)
-      .left_goal(-1.0)
-      .right_goal(1.0)
-      .Send();
-  RunForTime(chrono::seconds(2));
+  SetEnabled(true);
+  {
+    auto message = drivetrain_goal_sender_.MakeMessage();
+    message->controller_type = 1;
+    message->left_goal = -1.0;
+    message->right_goal = 1.0;
+    EXPECT_TRUE(message.Send());
+  }
+  RunFor(chrono::seconds(2));
   VerifyNearGoal();
 }
 
 // Tests that the drivetrain converges on a goal when under the effect of a
 // voltage offset/disturbance.
 TEST_F(DrivetrainTest, ConvergesWithVoltageError) {
-  my_drivetrain_queue_.goal.MakeWithBuilder()
-      .controller_type(1)
-      .left_goal(-1.0)
-      .right_goal(1.0)
-      .Send();
-  drivetrain_motor_plant_.set_left_voltage_offset(1.0);
-  drivetrain_motor_plant_.set_right_voltage_offset(1.0);
-  RunForTime(chrono::milliseconds(1500));
+  SetEnabled(true);
+  {
+    auto message = drivetrain_goal_sender_.MakeMessage();
+    message->controller_type = 1;
+    message->left_goal = -1.0;
+    message->right_goal = 1.0;
+    EXPECT_TRUE(message.Send());
+  }
+  drivetrain_plant_.set_left_voltage_offset(1.0);
+  drivetrain_plant_.set_right_voltage_offset(1.0);
+  RunFor(chrono::milliseconds(1500));
   VerifyNearGoal();
 }
 
 // Tests that it survives disabling.
 TEST_F(DrivetrainTest, SurvivesDisabling) {
-  my_drivetrain_queue_.goal.MakeWithBuilder()
-      .controller_type(1)
-      .left_goal(-1.0)
-      .right_goal(1.0)
-      .Send();
+  {
+    auto message = drivetrain_goal_sender_.MakeMessage();
+    message->controller_type = 1;
+    message->left_goal = -1.0;
+    message->right_goal = 1.0;
+    EXPECT_TRUE(message.Send());
+  }
   for (int i = 0; i < 500; ++i) {
-    drivetrain_motor_plant_.SendPositionMessage();
-    drivetrain_motor_.Iterate();
-    drivetrain_motor_plant_.Simulate();
     if (i > 20 && i < 200) {
-      SimulateTimestep(false, dt_config_.dt);
+      SetEnabled(false);
     } else {
-      SimulateTimestep(true, dt_config_.dt);
+      SetEnabled(true);
     }
+    RunFor(dt());
   }
   VerifyNearGoal();
 }
 
-// Tests that never having a goal doesn't break.
-TEST_F(DrivetrainTest, NoGoalStart) {
-  for (int i = 0; i < 20; ++i) {
-    drivetrain_motor_plant_.SendPositionMessage();
-    drivetrain_motor_.Iterate();
-    drivetrain_motor_plant_.Simulate();
-  }
-}
-
 // Tests that never having a goal, but having driver's station messages, doesn't
 // break.
 TEST_F(DrivetrainTest, NoGoalWithRobotState) {
-  RunForTime(chrono::milliseconds(100));
+  SetEnabled(true);
+  RunFor(chrono::milliseconds(100));
 }
 
 // Tests that the robot successfully drives straight forward.
 // This used to not work due to a U-capping bug.
 TEST_F(DrivetrainTest, DriveStraightForward) {
-  my_drivetrain_queue_.goal.MakeWithBuilder()
-      .controller_type(1)
-      .left_goal(4.0)
-      .right_goal(4.0)
-      .Send();
+  SetEnabled(true);
+  {
+    auto message = drivetrain_goal_sender_.MakeMessage();
+    message->controller_type = 1;
+    message->left_goal = 4.0;
+    message->right_goal = 4.0;
+    EXPECT_TRUE(message.Send());
+  }
+
   for (int i = 0; i < 500; ++i) {
-    RunIteration();
-    ASSERT_TRUE(my_drivetrain_queue_.output.FetchLatest());
-    EXPECT_NEAR(my_drivetrain_queue_.output->left_voltage,
-                my_drivetrain_queue_.output->right_voltage, 1e-4);
-    EXPECT_GT(my_drivetrain_queue_.output->left_voltage, -11);
-    EXPECT_GT(my_drivetrain_queue_.output->right_voltage, -11);
+    RunFor(dt());
+    ASSERT_TRUE(drivetrain_output_fetcher_.Fetch());
+    EXPECT_NEAR(drivetrain_output_fetcher_->left_voltage,
+                drivetrain_output_fetcher_->right_voltage, 1e-4);
+    EXPECT_GT(drivetrain_output_fetcher_->left_voltage, -11);
+    EXPECT_GT(drivetrain_output_fetcher_->right_voltage, -11);
   }
   VerifyNearGoal();
 }
@@ -233,16 +208,19 @@
 // Tests that the robot successfully drives close to straight.
 // This used to fail in simulation due to libcdd issues with U-capping.
 TEST_F(DrivetrainTest, DriveAlmostStraightForward) {
-  my_drivetrain_queue_.goal.MakeWithBuilder()
-      .controller_type(1)
-      .left_goal(4.0)
-      .right_goal(3.9)
-      .Send();
+  SetEnabled(true);
+  {
+    auto message = drivetrain_goal_sender_.MakeMessage();
+    message->controller_type = 1;
+    message->left_goal = 4.0;
+    message->right_goal = 3.9;
+    EXPECT_TRUE(message.Send());
+  }
   for (int i = 0; i < 500; ++i) {
-    RunIteration();
-    ASSERT_TRUE(my_drivetrain_queue_.output.FetchLatest());
-    EXPECT_GT(my_drivetrain_queue_.output->left_voltage, -11);
-    EXPECT_GT(my_drivetrain_queue_.output->right_voltage, -11);
+    RunFor(dt());
+    ASSERT_TRUE(drivetrain_output_fetcher_.Fetch());
+    EXPECT_GT(drivetrain_output_fetcher_->left_voltage, -11);
+    EXPECT_GT(drivetrain_output_fetcher_->right_voltage, -11);
   }
   VerifyNearGoal();
 }
@@ -274,81 +252,78 @@
 
 // Tests that a linear motion profile succeeds.
 TEST_F(DrivetrainTest, ProfileStraightForward) {
+  SetEnabled(true);
   {
-    ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
-        goal = my_drivetrain_queue_.goal.MakeMessage();
-    goal->controller_type = 1;
-    goal->left_goal = 4.0;
-    goal->right_goal = 4.0;
-    goal->linear.max_velocity = 1.0;
-    goal->linear.max_acceleration = 3.0;
-    goal->angular.max_velocity = 1.0;
-    goal->angular.max_acceleration = 3.0;
-    goal.Send();
+    auto message = drivetrain_goal_sender_.MakeMessage();
+    message->controller_type = 1;
+    message->left_goal = 4.0;
+    message->right_goal = 4.0;
+    message->linear.max_velocity = 1.0;
+    message->linear.max_acceleration = 3.0;
+    message->angular.max_velocity = 1.0;
+    message->angular.max_acceleration = 3.0;
+    EXPECT_TRUE(message.Send());
   }
 
-  while (monotonic_clock::now() <
-         monotonic_clock::time_point(chrono::seconds(6))) {
-    RunIteration();
-    ASSERT_TRUE(my_drivetrain_queue_.output.FetchLatest());
-    EXPECT_NEAR(my_drivetrain_queue_.output->left_voltage,
-                my_drivetrain_queue_.output->right_voltage, 1e-4);
-    EXPECT_GT(my_drivetrain_queue_.output->left_voltage, -6);
-    EXPECT_GT(my_drivetrain_queue_.output->right_voltage, -6);
-    EXPECT_LT(my_drivetrain_queue_.output->left_voltage, 6);
-    EXPECT_LT(my_drivetrain_queue_.output->right_voltage, 6);
+  while (monotonic_now() < monotonic_clock::time_point(chrono::seconds(6))) {
+    RunFor(dt());
+    ASSERT_TRUE(drivetrain_output_fetcher_.Fetch());
+    EXPECT_NEAR(drivetrain_output_fetcher_->left_voltage,
+                drivetrain_output_fetcher_->right_voltage, 1e-4);
+    EXPECT_GT(drivetrain_output_fetcher_->left_voltage, -6);
+    EXPECT_GT(drivetrain_output_fetcher_->right_voltage, -6);
+    EXPECT_LT(drivetrain_output_fetcher_->left_voltage, 6);
+    EXPECT_LT(drivetrain_output_fetcher_->right_voltage, 6);
   }
   VerifyNearGoal();
 }
 
 // Tests that an angular motion profile succeeds.
 TEST_F(DrivetrainTest, ProfileTurn) {
+  SetEnabled(true);
   {
-    ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
-        goal = my_drivetrain_queue_.goal.MakeMessage();
-    goal->controller_type = 1;
-    goal->left_goal = -1.0;
-    goal->right_goal = 1.0;
-    goal->linear.max_velocity = 1.0;
-    goal->linear.max_acceleration = 3.0;
-    goal->angular.max_velocity = 1.0;
-    goal->angular.max_acceleration = 3.0;
-    goal.Send();
+    auto message = drivetrain_goal_sender_.MakeMessage();
+    message->controller_type = 1;
+    message->left_goal = -1.0;
+    message->right_goal = 1.0;
+    message->linear.max_velocity = 1.0;
+    message->linear.max_acceleration = 3.0;
+    message->angular.max_velocity = 1.0;
+    message->angular.max_acceleration = 3.0;
+    EXPECT_TRUE(message.Send());
   }
 
-  while (monotonic_clock::now() <
-         monotonic_clock::time_point(chrono::seconds(6))) {
-    RunIteration();
-    ASSERT_TRUE(my_drivetrain_queue_.output.FetchLatest());
-    EXPECT_NEAR(my_drivetrain_queue_.output->left_voltage,
-                -my_drivetrain_queue_.output->right_voltage, 1e-4);
-    EXPECT_GT(my_drivetrain_queue_.output->left_voltage, -6);
-    EXPECT_GT(my_drivetrain_queue_.output->right_voltage, -6);
-    EXPECT_LT(my_drivetrain_queue_.output->left_voltage, 6);
-    EXPECT_LT(my_drivetrain_queue_.output->right_voltage, 6);
+  while (monotonic_now() < monotonic_clock::time_point(chrono::seconds(6))) {
+    RunFor(dt());
+    ASSERT_TRUE(drivetrain_output_fetcher_.Fetch());
+    EXPECT_NEAR(drivetrain_output_fetcher_->left_voltage,
+                -drivetrain_output_fetcher_->right_voltage, 1e-4);
+    EXPECT_GT(drivetrain_output_fetcher_->left_voltage, -6);
+    EXPECT_GT(drivetrain_output_fetcher_->right_voltage, -6);
+    EXPECT_LT(drivetrain_output_fetcher_->left_voltage, 6);
+    EXPECT_LT(drivetrain_output_fetcher_->right_voltage, 6);
   }
   VerifyNearGoal();
 }
 
 // Tests that a mixed turn drive saturated profile succeeds.
 TEST_F(DrivetrainTest, SaturatedTurnDrive) {
+  SetEnabled(true);
   {
-    ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
-        goal = my_drivetrain_queue_.goal.MakeMessage();
-    goal->controller_type = 1;
-    goal->left_goal = 5.0;
-    goal->right_goal = 4.0;
-    goal->linear.max_velocity = 6.0;
-    goal->linear.max_acceleration = 4.0;
-    goal->angular.max_velocity = 2.0;
-    goal->angular.max_acceleration = 4.0;
-    goal.Send();
+    auto message = drivetrain_goal_sender_.MakeMessage();
+    message->controller_type = 1;
+    message->left_goal = 5.0;
+    message->right_goal = 4.0;
+    message->linear.max_velocity = 6.0;
+    message->linear.max_acceleration = 4.0;
+    message->angular.max_velocity = 2.0;
+    message->angular.max_acceleration = 4.0;
+    EXPECT_TRUE(message.Send());
   }
 
-  while (monotonic_clock::now() <
-         monotonic_clock::time_point(chrono::seconds(3))) {
-    RunIteration();
-    ASSERT_TRUE(my_drivetrain_queue_.output.FetchLatest());
+  while (monotonic_now() < monotonic_clock::time_point(chrono::seconds(3))) {
+    RunFor(dt());
+    ASSERT_TRUE(drivetrain_output_fetcher_.Fetch());
   }
   VerifyNearGoal();
 }
@@ -356,111 +331,121 @@
 // Tests that being in teleop drive for a bit and then transitioning to closed
 // drive profiles nicely.
 TEST_F(DrivetrainTest, OpenLoopThenClosed) {
-  my_drivetrain_queue_.goal.MakeWithBuilder()
-      .controller_type(0)
-      .wheel(0.0)
-      .throttle(1.0)
-      .highgear(true)
-      .quickturn(false)
-      .Send();
-
-  RunForTime(chrono::seconds(1));
-
-  my_drivetrain_queue_.goal.MakeWithBuilder()
-      .controller_type(0)
-      .wheel(0.0)
-      .throttle(-0.3)
-      .highgear(true)
-      .quickturn(false)
-      .Send();
-
-  RunForTime(chrono::seconds(1));
-
-  my_drivetrain_queue_.goal.MakeWithBuilder()
-      .controller_type(0)
-      .wheel(0.0)
-      .throttle(0.0)
-      .highgear(true)
-      .quickturn(false)
-      .Send();
-
-  RunForTime(chrono::seconds(10));
-
+  SetEnabled(true);
   {
-    ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
-        goal = my_drivetrain_queue_.goal.MakeMessage();
-    goal->controller_type = 1;
-    goal->left_goal = 5.0;
-    goal->right_goal = 4.0;
-    goal->linear.max_velocity = 1.0;
-    goal->linear.max_acceleration = 2.0;
-    goal->angular.max_velocity = 1.0;
-    goal->angular.max_acceleration = 2.0;
-    goal.Send();
+    auto message = drivetrain_goal_sender_.MakeMessage();
+    message->controller_type = 0;
+    message->wheel = 0.0;
+    message->throttle = 1.0;
+    message->highgear = true;
+    message->quickturn = false;
+    EXPECT_TRUE(message.Send());
   }
 
-  const auto end_time = monotonic_clock::now() + chrono::seconds(4);
-  while (monotonic_clock::now() < end_time) {
-    drivetrain_motor_plant_.SendPositionMessage();
-    drivetrain_motor_.Iterate();
-    drivetrain_motor_plant_.Simulate();
-    SimulateTimestep(true, dt_config_.dt);
-    ASSERT_TRUE(my_drivetrain_queue_.output.FetchLatest());
-    EXPECT_GT(my_drivetrain_queue_.output->left_voltage, -6);
-    EXPECT_GT(my_drivetrain_queue_.output->right_voltage, -6);
-    EXPECT_LT(my_drivetrain_queue_.output->left_voltage, 6);
-    EXPECT_LT(my_drivetrain_queue_.output->right_voltage, 6);
+  RunFor(chrono::seconds(1));
+
+  {
+    auto message = drivetrain_goal_sender_.MakeMessage();
+    message->controller_type = 0;
+    message->wheel = 0.0;
+    message->throttle = -0.3;
+    message->highgear = true;
+    message->quickturn = false;
+    EXPECT_TRUE(message.Send());
+  }
+
+  RunFor(chrono::seconds(1));
+
+  {
+    auto message = drivetrain_goal_sender_.MakeMessage();
+    message->controller_type = 0;
+    message->wheel = 0.0;
+    message->throttle = 0.0;
+    message->highgear = true;
+    message->quickturn = false;
+    EXPECT_TRUE(message.Send());
+  }
+
+  RunFor(chrono::seconds(10));
+
+  {
+    auto message = drivetrain_goal_sender_.MakeMessage();
+    message->controller_type = 1;
+    message->left_goal = 5.0;
+    message->right_goal = 4.0;
+    message->linear.max_velocity = 1.0;
+    message->linear.max_acceleration = 2.0;
+    message->angular.max_velocity = 1.0;
+    message->angular.max_acceleration = 2.0;
+    EXPECT_TRUE(message.Send());
+  }
+
+  const auto end_time = monotonic_now() + chrono::seconds(4);
+  while (monotonic_now() < end_time) {
+    RunFor(dt());
+    ASSERT_TRUE(drivetrain_output_fetcher_.Fetch());
+    EXPECT_GT(drivetrain_output_fetcher_->left_voltage, -6);
+    EXPECT_GT(drivetrain_output_fetcher_->right_voltage, -6);
+    EXPECT_LT(drivetrain_output_fetcher_->left_voltage, 6);
+    EXPECT_LT(drivetrain_output_fetcher_->right_voltage, 6);
   }
   VerifyNearGoal();
 }
 
 // Tests that simple spline converges on a goal.
 TEST_F(DrivetrainTest, SplineSimple) {
-  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
-      my_drivetrain_queue_.goal.MakeMessage();
-  goal->controller_type = 2;
-  goal->spline.spline_idx = 1;
-  goal->spline.spline_count = 1;
-  goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
-  goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0}};
-  goal.Send();
-  RunIteration();
+  SetEnabled(true);
+  {
+    auto message = drivetrain_goal_sender_.MakeMessage();
+    message->controller_type = 2;
+    message->spline.spline_idx = 1;
+    message->spline.spline_count = 1;
+    message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+    message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
+    EXPECT_TRUE(message.Send());
+  }
+  RunFor(dt());
 
-  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
-      start_goal = my_drivetrain_queue_.goal.MakeMessage();
-  start_goal->controller_type = 2;
-  start_goal->spline_handle = 1;
-  start_goal.Send();
+  // Send the start goal
+  {
+    auto message = drivetrain_goal_sender_.MakeMessage();
+    message->controller_type = 2;
+    message->spline_handle = 1;
+    EXPECT_TRUE(message.Send());
+  }
   WaitForTrajectoryPlan();
 
-  RunForTime(chrono::milliseconds(2000));
+  RunFor(chrono::milliseconds(2000));
   VerifyNearSplineGoal();
 }
 
 // Tests that we can drive a spline backwards.
 TEST_F(DrivetrainTest, SplineSimpleBackwards) {
-  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
-      my_drivetrain_queue_.goal.MakeMessage();
-  goal->controller_type = 2;
-  goal->spline.spline_idx = 1;
-  goal->spline.spline_count = 1;
-  goal->spline.drive_spline_backwards = true;
-  goal->spline.spline_x = {{0.0, -0.25, -0.5, -0.5, -0.75, -1.0}};
-  goal->spline.spline_y = {{0.0, 0.0, -0.25 ,-0.75, -1.0, -1.0}};
-  goal.Send();
-  RunIteration();
+  SetEnabled(true);
+  {
+    auto message = drivetrain_goal_sender_.MakeMessage();
+    message->controller_type = 2;
+    message->spline.spline_idx = 1;
+    message->spline.spline_count = 1;
+    message->spline.drive_spline_backwards = true;
+    message->spline.spline_x = {{0.0, -0.25, -0.5, -0.5, -0.75, -1.0}};
+    message->spline.spline_y = {{0.0, 0.0, -0.25, -0.75, -1.0, -1.0}};
+    EXPECT_TRUE(message.Send());
+  }
+  RunFor(dt());
 
-  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
-      start_goal = my_drivetrain_queue_.goal.MakeMessage();
-  start_goal->controller_type = 2;
-  start_goal->spline_handle = 1;
-  start_goal.Send();
+  {
+    auto message = drivetrain_goal_sender_.MakeMessage();
+    message->controller_type = 2;
+    message->spline_handle = 1;
+    EXPECT_TRUE(message.Send());
+  }
   WaitForTrajectoryPlan();
 
   // Check that we are right on the spline at the start (otherwise the feedback
   // will tend to correct for going the wrong direction).
   for (int ii = 0; ii < 10; ++ii) {
-    RunForTime(chrono::milliseconds(100));
+    RunFor(chrono::milliseconds(100));
     VerifyNearSplineGoal();
   }
 
@@ -468,249 +453,282 @@
 
   VerifyNearSplineGoal();
   // Check that we are pointed the right direction:
-  my_drivetrain_queue_.status.FetchLatest();
-  auto actual = drivetrain_motor_plant_.state();
+  drivetrain_status_fetcher_.Fetch();
+  auto actual = drivetrain_plant_.state();
   const double expected_theta =
-      my_drivetrain_queue_.status->trajectory_logging.theta;
+      drivetrain_status_fetcher_->trajectory_logging.theta;
   // As a sanity check, compare both against absolute angle and the spline's
   // goal angle.
   EXPECT_NEAR(0.0, ::aos::math::DiffAngle(actual(2), 0.0), 2e-2);
-  EXPECT_NEAR(0.0, ::aos::math::DiffAngle(actual(2), expected_theta),
-              2e-2);
+  EXPECT_NEAR(0.0, ::aos::math::DiffAngle(actual(2), expected_theta), 2e-2);
 }
 
 // Tests that simple spline with a single goal message.
 TEST_F(DrivetrainTest, SplineSingleGoal) {
-  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
-      my_drivetrain_queue_.goal.MakeMessage();
-  goal->controller_type = 2;
-  goal->spline.spline_idx = 1;
-  goal->spline.spline_count = 1;
-  goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
-  goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0}};
-  goal->spline_handle = 1;
-  goal.Send();
+  SetEnabled(true);
+  {
+    auto message = drivetrain_goal_sender_.MakeMessage();
+    message->controller_type = 2;
+    message->spline.spline_idx = 1;
+    message->spline.spline_count = 1;
+    message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+    message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
+    message->spline_handle = 1;
+    EXPECT_TRUE(message.Send());
+  }
   WaitForTrajectoryPlan();
 
-  RunForTime(chrono::milliseconds(2000));
+  RunFor(chrono::milliseconds(2000));
   VerifyNearSplineGoal();
 }
 
 // Tests that a trajectory can be stopped in the middle.
 TEST_F(DrivetrainTest, SplineStop) {
-  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
-      my_drivetrain_queue_.goal.MakeMessage();
-  goal->controller_type = 2;
-  goal->spline.spline_idx = 1;
-  goal->spline.spline_count = 1;
-  goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
-  goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0}};
-  goal->spline_handle = 1;
-  goal.Send();
+  SetEnabled(true);
+  {
+    auto message = drivetrain_goal_sender_.MakeMessage();
+    message->controller_type = 2;
+    message->spline.spline_idx = 1;
+    message->spline.spline_count = 1;
+    message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+    message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
+    message->spline_handle = 1;
+    EXPECT_TRUE(message.Send());
+  }
   WaitForTrajectoryPlan();
 
-  RunForTime(chrono::milliseconds(500));
-  my_drivetrain_queue_.status.FetchLatest();
-  const double goal_x = my_drivetrain_queue_.status->trajectory_logging.x;
-  const double goal_y = my_drivetrain_queue_.status->trajectory_logging.y;
+  RunFor(chrono::milliseconds(500));
+  drivetrain_status_fetcher_.Fetch();
+  const double goal_x = drivetrain_status_fetcher_->trajectory_logging.x;
+  const double goal_y = drivetrain_status_fetcher_->trajectory_logging.y;
 
-  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> stop_goal =
-      my_drivetrain_queue_.goal.MakeMessage();
-  stop_goal->controller_type = 2;
-  stop_goal->spline_handle = 0;
-  stop_goal.Send();
-  RunForTime(chrono::milliseconds(2000));
+  // Now stop.
+  {
+    auto message = drivetrain_goal_sender_.MakeMessage();
+    message->controller_type = 2;
+    message->spline_handle = 0;
+    EXPECT_TRUE(message.Send());
+  }
+  RunFor(chrono::milliseconds(2000));
 
   // The goal shouldn't change after being stopped.
-  my_drivetrain_queue_.status.FetchLatest();
-  EXPECT_NEAR(my_drivetrain_queue_.status->trajectory_logging.x, goal_x, 1e-9);
-  EXPECT_NEAR(my_drivetrain_queue_.status->trajectory_logging.y, goal_y, 1e-9);
+  drivetrain_status_fetcher_.Fetch();
+  EXPECT_NEAR(drivetrain_status_fetcher_->trajectory_logging.x, goal_x, 1e-9);
+  EXPECT_NEAR(drivetrain_status_fetcher_->trajectory_logging.y, goal_y, 1e-9);
 }
 
 // Tests that a spline can't be restarted.
 TEST_F(DrivetrainTest, SplineRestart) {
-  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
-      my_drivetrain_queue_.goal.MakeMessage();
-  goal->controller_type = 2;
-  goal->spline.spline_idx = 1;
-  goal->spline.spline_count = 1;
-  goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
-  goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0}};
-  goal->spline_handle = 1;
-  goal.Send();
+  SetEnabled(true);
+  {
+    auto message = drivetrain_goal_sender_.MakeMessage();
+    message->controller_type = 2;
+    message->spline.spline_idx = 1;
+    message->spline.spline_count = 1;
+    message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+    message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
+    message->spline_handle = 1;
+    EXPECT_TRUE(message.Send());
+  }
   WaitForTrajectoryPlan();
 
-  RunForTime(chrono::milliseconds(500));
-  my_drivetrain_queue_.status.FetchLatest();
-  const double goal_x = my_drivetrain_queue_.status->trajectory_logging.x;
-  const double goal_y = my_drivetrain_queue_.status->trajectory_logging.y;
+  RunFor(chrono::milliseconds(500));
+  drivetrain_status_fetcher_.Fetch();
+  const double goal_x = drivetrain_status_fetcher_->trajectory_logging.x;
+  const double goal_y = drivetrain_status_fetcher_->trajectory_logging.y;
 
-  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> stop_goal =
-      my_drivetrain_queue_.goal.MakeMessage();
-  stop_goal->controller_type = 2;
-  stop_goal->spline_handle = 0;
-  stop_goal.Send();
-  RunForTime(chrono::milliseconds(500));
+  // Send a stop goal.
+  {
+    auto message = drivetrain_goal_sender_.MakeMessage();
+    message->controller_type = 2;
+    message->spline_handle = 0;
+    EXPECT_TRUE(message.Send());
+  }
+  RunFor(chrono::milliseconds(500));
 
-  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> restart_goal =
-      my_drivetrain_queue_.goal.MakeMessage();
-  restart_goal->controller_type = 2;
-  restart_goal->spline_handle = 1;
-  restart_goal.Send();
-  RunForTime(chrono::milliseconds(2000));
+  // Send a restart.
+  {
+    auto message = drivetrain_goal_sender_.MakeMessage();
+    message->controller_type = 2;
+    message->spline_handle = 1;
+    EXPECT_TRUE(message.Send());
+  }
+  RunFor(chrono::milliseconds(2000));
 
   // The goal shouldn't change after being stopped and restarted.
-  my_drivetrain_queue_.status.FetchLatest();
-  EXPECT_NEAR(my_drivetrain_queue_.status->trajectory_logging.x, goal_x, 1e-9);
-  EXPECT_NEAR(my_drivetrain_queue_.status->trajectory_logging.y, goal_y, 1e-9);
+  drivetrain_status_fetcher_.Fetch();
+  EXPECT_NEAR(drivetrain_status_fetcher_->trajectory_logging.x, goal_x, 1e-9);
+  EXPECT_NEAR(drivetrain_status_fetcher_->trajectory_logging.y, goal_y, 1e-9);
 }
 
 // Tests that simple spline converges when it doesn't start where it thinks.
 TEST_F(DrivetrainTest, SplineOffset) {
-  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
-      my_drivetrain_queue_.goal.MakeMessage();
-  goal->controller_type = 2;
-  goal->spline.spline_idx = 1;
-  goal->spline.spline_count = 1;
-  goal->spline.spline_x = {{0.2, 0.25, 0.5, 0.5, 0.75, 1.0}};
-  goal->spline.spline_y = {{0.2, 0.0, 0.25 ,0.75, 1.0, 1.0}};
-  goal.Send();
-  RunIteration();
+  SetEnabled(true);
+  {
+    auto message = drivetrain_goal_sender_.MakeMessage();
+    message->controller_type = 2;
+    message->spline.spline_idx = 1;
+    message->spline.spline_count = 1;
+    message->spline.spline_x = {{0.2, 0.25, 0.5, 0.5, 0.75, 1.0}};
+    message->spline.spline_y = {{0.2, 0.0, 0.25, 0.75, 1.0, 1.0}};
+    EXPECT_TRUE(message.Send());
+  }
+  RunFor(dt());
 
-  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
-      start_goal = my_drivetrain_queue_.goal.MakeMessage();
-  start_goal->controller_type = 2;
-  start_goal->spline_handle = 1;
-  start_goal.Send();
+  {
+    auto message = drivetrain_goal_sender_.MakeMessage();
+    message->controller_type = 2;
+    message->spline_handle = 1;
+    EXPECT_TRUE(message.Send());
+  }
   WaitForTrajectoryPlan();
 
-  RunForTime(chrono::milliseconds(2000));
+  RunFor(chrono::milliseconds(2000));
   VerifyNearSplineGoal();
 }
 
 // Tests that simple spline converges when it starts to the side of where it
 // thinks.
 TEST_F(DrivetrainTest, SplineSideOffset) {
-  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
-      my_drivetrain_queue_.goal.MakeMessage();
-  goal->controller_type = 2;
-  goal->spline.spline_idx = 1;
-  goal->spline.spline_count = 1;
-  goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
-  goal->spline.spline_y = {{0.5, 0.0, 0.25 ,0.75, 1.0, 1.0}};
-  goal.Send();
-  RunIteration();
+  SetEnabled(true);
+  {
+    auto message = drivetrain_goal_sender_.MakeMessage();
+    message->controller_type = 2;
+    message->spline.spline_idx = 1;
+    message->spline.spline_count = 1;
+    message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+    message->spline.spline_y = {{0.5, 0.0, 0.25, 0.75, 1.0, 1.0}};
+    EXPECT_TRUE(message.Send());
+  }
+  RunFor(dt());
 
-  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
-      start_goal = my_drivetrain_queue_.goal.MakeMessage();
-  start_goal->controller_type = 2;
-  start_goal->spline_handle = 1;
-  start_goal.Send();
+  {
+    auto message = drivetrain_goal_sender_.MakeMessage();
+    message->controller_type = 2;
+    message->spline_handle = 1;
+    EXPECT_TRUE(message.Send());
+  }
   WaitForTrajectoryPlan();
 
-  RunForTime(chrono::milliseconds(2000));
+  RunFor(chrono::milliseconds(2000));
   VerifyNearSplineGoal();
 }
 
 // Tests that a multispline converges on a goal.
 TEST_F(DrivetrainTest, MultiSpline) {
-  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
-      my_drivetrain_queue_.goal.MakeMessage();
-  goal->controller_type = 2;
-  goal->spline.spline_idx = 1;
-  goal->spline.spline_count = 2;
-  goal->spline.spline_x = {
-      {0.0, 0.25, 0.5, 0.5, 0.75, 1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
-  goal->spline.spline_y = {
-      {0.0, 0.0, 0.25, 0.75, 1.0, 1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
-  goal.Send();
-  RunIteration();
+  SetEnabled(true);
+  {
+    auto message = drivetrain_goal_sender_.MakeMessage();
+    message->controller_type = 2;
+    message->spline.spline_idx = 1;
+    message->spline.spline_count = 2;
+    message->spline.spline_x = {
+        {0.0, 0.25, 0.5, 0.5, 0.75, 1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
+    message->spline.spline_y = {
+        {0.0, 0.0, 0.25, 0.75, 1.0, 1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
+    EXPECT_TRUE(message.Send());
+  }
+  RunFor(dt());
 
-  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
-      start_goal = my_drivetrain_queue_.goal.MakeMessage();
-  start_goal->controller_type = 2;
-  start_goal->spline_handle = 1;
-  start_goal.Send();
+  {
+    auto message = drivetrain_goal_sender_.MakeMessage();
+    message->controller_type = 2;
+    message->spline_handle = 1;
+    EXPECT_TRUE(message.Send());
+  }
   WaitForTrajectoryPlan();
 
-  RunForTime(chrono::milliseconds(4000));
+  RunFor(chrono::milliseconds(4000));
   VerifyNearSplineGoal();
 }
 
 // Tests that several splines converges on a goal.
 TEST_F(DrivetrainTest, SequentialSplines) {
-  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
-      my_drivetrain_queue_.goal.MakeMessage();
-  goal->controller_type = 2;
-  goal->spline.spline_idx = 1;
-  goal->spline.spline_count = 1;
-  goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
-  goal->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
-  goal.Send();
-  RunIteration();
+  SetEnabled(true);
+  {
+    auto message = drivetrain_goal_sender_.MakeMessage();
+    message->controller_type = 2;
+    message->spline.spline_idx = 1;
+    message->spline.spline_count = 1;
+    message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+    message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
+    EXPECT_TRUE(message.Send());
+  }
+  RunFor(dt());
 
-  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
-      start_goal = my_drivetrain_queue_.goal.MakeMessage();
-  start_goal->controller_type = 2;
-  start_goal->spline_handle = 1;
-  start_goal.Send();
+  {
+    auto message = drivetrain_goal_sender_.MakeMessage();
+    message->controller_type = 2;
+    message->spline_handle = 1;
+    EXPECT_TRUE(message.Send());
+  }
   WaitForTrajectoryPlan();
 
   WaitForTrajectoryExecution();
 
   VerifyNearSplineGoal();
 
-  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
-      second_spline_goal = my_drivetrain_queue_.goal.MakeMessage();
-  second_spline_goal->controller_type = 2;
-  second_spline_goal->spline.spline_idx = 2;
-  second_spline_goal->spline.spline_count = 1;
-  second_spline_goal->spline.spline_x = {{1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
-  second_spline_goal->spline.spline_y = {{1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
-  second_spline_goal.Send();
-  RunIteration();
+  // Second spline.
+  {
+    auto message = drivetrain_goal_sender_.MakeMessage();
+    message->controller_type = 2;
+    message->spline.spline_idx = 2;
+    message->spline.spline_count = 1;
+    message->spline.spline_x = {{1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
+    message->spline.spline_y = {{1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
+    EXPECT_TRUE(message.Send());
+  }
+  RunFor(dt());
 
-  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
-      second_start_goal = my_drivetrain_queue_.goal.MakeMessage();
-  second_start_goal->controller_type = 2;
-  second_start_goal->spline_handle = 2;
-  second_start_goal.Send();
+  // And then start it.
+  {
+    auto message = drivetrain_goal_sender_.MakeMessage();
+    message->controller_type = 2;
+    message->spline_handle = 2;
+    EXPECT_TRUE(message.Send());
+  }
   WaitForTrajectoryPlan();
 
-  RunForTime(chrono::milliseconds(2000));
+  RunFor(chrono::milliseconds(2000));
   VerifyNearSplineGoal();
 }
 // Tests that a second spline will run if the first is stopped.
 TEST_F(DrivetrainTest, SplineStopFirst) {
-  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
-      my_drivetrain_queue_.goal.MakeMessage();
-  goal->controller_type = 2;
-  goal->spline.spline_idx = 1;
-  goal->spline.spline_count = 1;
-  goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
-  goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0}};
-  goal->spline_handle = 1;
-  goal.Send();
+  SetEnabled(true);
+  {
+    auto message = drivetrain_goal_sender_.MakeMessage();
+    message->controller_type = 2;
+    message->spline.spline_idx = 1;
+    message->spline.spline_count = 1;
+    message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+    message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
+    message->spline_handle = 1;
+    EXPECT_TRUE(message.Send());
+  }
   WaitForTrajectoryPlan();
 
-  RunForTime(chrono::milliseconds(1000));
+  RunFor(chrono::milliseconds(1000));
 
-  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> stop_goal =
-      my_drivetrain_queue_.goal.MakeMessage();
-  stop_goal->controller_type = 2;
-  stop_goal->spline_handle = 0;
-  stop_goal.Send();
-  RunForTime(chrono::milliseconds(500));
+  // Stop goal
+  {
+    auto message = drivetrain_goal_sender_.MakeMessage();
+    message->controller_type = 2;
+    message->spline_handle = 0;
+    EXPECT_TRUE(message.Send());
+  }
+  RunFor(chrono::milliseconds(500));
 
-  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
-      second_spline_goal = my_drivetrain_queue_.goal.MakeMessage();
-  second_spline_goal->controller_type = 2;
-  second_spline_goal->spline.spline_idx = 2;
-  second_spline_goal->spline.spline_count = 1;
-  second_spline_goal->spline.spline_x = {{1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
-  second_spline_goal->spline.spline_y = {{1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
-  second_spline_goal->spline_handle = 2;
-  second_spline_goal.Send();
+  // Second spline goal.
+  {
+    auto message = drivetrain_goal_sender_.MakeMessage();
+    message->controller_type = 2;
+    message->spline.spline_idx = 2;
+    message->spline.spline_count = 1;
+    message->spline.spline_x = {{1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
+    message->spline.spline_y = {{1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
+    message->spline_handle = 2;
+    EXPECT_TRUE(message.Send());
+  }
   WaitForTrajectoryPlan();
 
   WaitForTrajectoryExecution();
@@ -720,37 +738,42 @@
 // Tests that we can run a second spline after having planned but never executed
 // the first spline.
 TEST_F(DrivetrainTest, CancelSplineBeforeExecuting) {
-  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
-      my_drivetrain_queue_.goal.MakeMessage();
-  goal->controller_type = 2;
-  goal->spline.spline_idx = 1;
-  goal->spline.spline_count = 1;
-  goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
-  goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0}};
-  // Don't start running the splane.
-  goal->spline_handle = 0;
-  goal.Send();
+  SetEnabled(true);
+  {
+    auto message = drivetrain_goal_sender_.MakeMessage();
+    message->controller_type = 2;
+    message->spline.spline_idx = 1;
+    message->spline.spline_count = 1;
+    message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+    message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
+    // Don't start running the splane.
+    message->spline_handle = 0;
+    EXPECT_TRUE(message.Send());
+  }
   WaitForTrajectoryPlan();
 
-  RunForTime(chrono::milliseconds(1000));
+  RunFor(chrono::milliseconds(1000));
 
   // Plan another spline, but don't start it yet:
-  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
-      second_spline_goal = my_drivetrain_queue_.goal.MakeMessage();
-  second_spline_goal->controller_type = 2;
-  second_spline_goal->spline.spline_idx = 2;
-  second_spline_goal->spline.spline_count = 1;
-  second_spline_goal->spline.spline_x = {{0.0, 0.75, 1.25, 1.5, 1.25, 1.0}};
-  second_spline_goal->spline.spline_y = {{0.0, 0.75, 1.25, 1.5, 1.75, 2.0}};
-  second_spline_goal->spline_handle = 0;
-  second_spline_goal.Send();
+  {
+    auto message = drivetrain_goal_sender_.MakeMessage();
+    message->controller_type = 2;
+    message->spline.spline_idx = 2;
+    message->spline.spline_count = 1;
+    message->spline.spline_x = {{0.0, 0.75, 1.25, 1.5, 1.25, 1.0}};
+    message->spline.spline_y = {{0.0, 0.75, 1.25, 1.5, 1.75, 2.0}};
+    message->spline_handle = 0;
+    EXPECT_TRUE(message.Send());
+  }
   WaitForTrajectoryPlan();
 
-  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
-      execute_goal = my_drivetrain_queue_.goal.MakeMessage();
-  execute_goal->controller_type = 2;
-  execute_goal->spline_handle = 2;
-  execute_goal.Send();
+  // Now execute it.
+  {
+    auto message = drivetrain_goal_sender_.MakeMessage();
+    message->controller_type = 2;
+    message->spline_handle = 2;
+    EXPECT_TRUE(message.Send());
+  }
 
   WaitForTrajectoryExecution();
   VerifyNearSplineGoal();
@@ -759,77 +782,90 @@
 
 // Tests that splines can excecute and plan at the same time.
 TEST_F(DrivetrainTest, ParallelSplines) {
-  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
-      my_drivetrain_queue_.goal.MakeMessage();
-  goal->controller_type = 2;
-  goal->spline.spline_idx = 1;
-  goal->spline.spline_count = 1;
-  goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
-  goal->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
-  goal.Send();
+  SetEnabled(true);
+  {
+    auto message = drivetrain_goal_sender_.MakeMessage();
+    message->controller_type = 2;
+    message->spline.spline_idx = 1;
+    message->spline.spline_count = 1;
+    message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+    message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
+    EXPECT_TRUE(message.Send());
+  }
   WaitForTrajectoryPlan();
 
-  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
-      second_spline_goal = my_drivetrain_queue_.goal.MakeMessage();
-  second_spline_goal->spline_handle = 1;
-  second_spline_goal->controller_type = 2;
-  second_spline_goal->spline.spline_idx = 2;
-  second_spline_goal->spline.spline_count = 1;
-  second_spline_goal->spline.spline_x = {{1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
-  second_spline_goal->spline.spline_y = {{1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
-  second_spline_goal.Send();
+  // Second spline goal
+  {
+    auto message = drivetrain_goal_sender_.MakeMessage();
+    message->spline_handle = 1;
+    message->controller_type = 2;
+    message->spline.spline_idx = 2;
+    message->spline.spline_count = 1;
+    message->spline.spline_x = {{1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
+    message->spline.spline_y = {{1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
+    EXPECT_TRUE(message.Send());
+  }
   WaitForTrajectoryExecution();
 
-  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
-      second_start_goal = my_drivetrain_queue_.goal.MakeMessage();
-  second_start_goal->controller_type = 2;
-  second_start_goal->spline_handle = 2;
-  second_start_goal.Send();
+  // Second start goal
+  {
+    auto message = drivetrain_goal_sender_.MakeMessage();
+    message->controller_type = 2;
+    message->spline_handle = 2;
+    EXPECT_TRUE(message.Send());
+  }
 
-  RunForTime(chrono::milliseconds(4000));
+  RunFor(chrono::milliseconds(4000));
   VerifyNearSplineGoal();
 }
 
-//Tests that a trajectory never told to execute will not replan.
+// Tests that a trajectory never told to execute will not replan.
 TEST_F(DrivetrainTest, OnlyPlanSpline) {
-  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
-      my_drivetrain_queue_.goal.MakeMessage();
-  goal->controller_type = 2;
-  goal->spline.spline_idx = 1;
-  goal->spline.spline_count = 1;
-  goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
-  goal->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
-  goal.Send();
+  SetEnabled(true);
+  {
+    auto message = drivetrain_goal_sender_.MakeMessage();
+    message->controller_type = 2;
+    message->spline.spline_idx = 1;
+    message->spline.spline_count = 1;
+    message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+    message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
+    EXPECT_TRUE(message.Send());
+  }
   WaitForTrajectoryPlan();
 
   for (int i = 0; i < 100; ++i) {
-    RunIteration();
-    my_drivetrain_queue_.status.FetchLatest();
-    EXPECT_EQ(my_drivetrain_queue_.status->trajectory_logging.planning_state, 3);
+    RunFor(dt());
+    drivetrain_status_fetcher_.Fetch();
+    EXPECT_EQ(drivetrain_status_fetcher_->trajectory_logging.planning_state,
+              3);
     ::std::this_thread::sleep_for(::std::chrono::milliseconds(2));
   }
   VerifyNearSplineGoal();
 }
 
-//Tests that a trajectory can be executed after it is planned.
+// Tests that a trajectory can be executed after it is planned.
 TEST_F(DrivetrainTest, SplineExecuteAfterPlan) {
-  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
-      my_drivetrain_queue_.goal.MakeMessage();
-  goal->controller_type = 2;
-  goal->spline.spline_idx = 1;
-  goal->spline.spline_count = 1;
-  goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
-  goal->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
-  goal.Send();
+  SetEnabled(true);
+  {
+    auto message = drivetrain_goal_sender_.MakeMessage();
+    message->controller_type = 2;
+    message->spline.spline_idx = 1;
+    message->spline.spline_count = 1;
+    message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+    message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
+    EXPECT_TRUE(message.Send());
+  }
   WaitForTrajectoryPlan();
-  RunForTime(chrono::milliseconds(2000));
+  RunFor(chrono::milliseconds(2000));
 
-  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
-      start_goal = my_drivetrain_queue_.goal.MakeMessage();
-  start_goal->controller_type = 2;
-  start_goal->spline_handle = 1;
-  start_goal.Send();
-  RunForTime(chrono::milliseconds(2000));
+  // Start goal
+  {
+    auto message = drivetrain_goal_sender_.MakeMessage();
+    message->controller_type = 2;
+    message->spline_handle = 1;
+    EXPECT_TRUE(message.Send());
+  }
+  RunFor(chrono::milliseconds(2000));
 
   VerifyNearPosition(1.0, 1.0);
 }
@@ -837,58 +873,66 @@
 // The LineFollowDrivetrain logic is tested in line_follow_drivetrain_test. This
 // tests that the integration with drivetrain_lib worked properly.
 TEST_F(DrivetrainTest, BasicLineFollow) {
+  SetEnabled(true);
   localizer_.target_selector()->set_has_target(true);
   localizer_.target_selector()->set_pose({{1.0, 1.0, 0.0}, M_PI_4});
-  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
-      my_drivetrain_queue_.goal.MakeMessage();
-  goal->controller_type = 3;
-  goal->throttle = 0.5;
-  goal.Send();
+  {
+    auto message = drivetrain_goal_sender_.MakeMessage();
+    message->controller_type = 3;
+    message->throttle = 0.5;
+    EXPECT_TRUE(message.Send());
+  }
 
-  RunForTime(chrono::seconds(5));
+  RunFor(chrono::seconds(5));
 
-  my_drivetrain_queue_.status.FetchLatest();
-  EXPECT_TRUE(my_drivetrain_queue_.status->line_follow_logging.frozen);
-  EXPECT_TRUE(my_drivetrain_queue_.status->line_follow_logging.have_target);
-  EXPECT_EQ(1.0, my_drivetrain_queue_.status->line_follow_logging.x);
-  EXPECT_EQ(1.0, my_drivetrain_queue_.status->line_follow_logging.y);
+  drivetrain_status_fetcher_.Fetch();
+  EXPECT_TRUE(drivetrain_status_fetcher_->line_follow_logging.frozen);
+  EXPECT_TRUE(drivetrain_status_fetcher_->line_follow_logging.have_target);
+  EXPECT_EQ(1.0, drivetrain_status_fetcher_->line_follow_logging.x);
+  EXPECT_EQ(1.0, drivetrain_status_fetcher_->line_follow_logging.y);
   EXPECT_FLOAT_EQ(M_PI_4,
-                  my_drivetrain_queue_.status->line_follow_logging.theta);
+                  drivetrain_status_fetcher_->line_follow_logging.theta);
 
   // Should have run off the end of the target, running along the y=x line.
-  EXPECT_LT(1.0, drivetrain_motor_plant_.GetPosition().x());
-  EXPECT_NEAR(drivetrain_motor_plant_.GetPosition().x(),
-              drivetrain_motor_plant_.GetPosition().y(), 0.1);
+  EXPECT_LT(1.0, drivetrain_plant_.GetPosition().x());
+  EXPECT_NEAR(drivetrain_plant_.GetPosition().x(),
+              drivetrain_plant_.GetPosition().y(), 0.1);
 }
 
 // Tests that the line follower will not run and defer to regular open-loop
 // driving when there is no target yet:
 TEST_F(DrivetrainTest, LineFollowDefersToOpenLoop) {
+  SetEnabled(true);
   localizer_.target_selector()->set_has_target(false);
   localizer_.target_selector()->set_pose({{1.0, 1.0, 0.0}, M_PI_4});
-  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
-      my_drivetrain_queue_.goal.MakeMessage();
-  goal->controller_type = 3;
-  goal->throttle = 0.5;
-  goal.Send();
+  {
+    auto message = drivetrain_goal_sender_.MakeMessage();
+    message->controller_type = 3;
+    message->throttle = 0.5;
+    EXPECT_TRUE(message.Send());
+  }
 
-  RunForTime(chrono::seconds(5));
+  RunFor(chrono::seconds(5));
   // Should have run straight (because we just set throttle, with wheel = 0)
   // along X-axis.
-  EXPECT_LT(1.0, drivetrain_motor_plant_.GetPosition().x());
-  EXPECT_NEAR(0.0, drivetrain_motor_plant_.GetPosition().y(), 1e-4);
+  EXPECT_LT(1.0, drivetrain_plant_.GetPosition().x());
+  EXPECT_NEAR(0.0, drivetrain_plant_.GetPosition().y(), 1e-4);
 }
 
 // Tests that we can reset the localizer to a new position.
 TEST_F(DrivetrainTest, ResetLocalizer) {
+  SetEnabled(true);
   EXPECT_EQ(0.0, localizer_.x());
   EXPECT_EQ(0.0, localizer_.y());
   EXPECT_EQ(0.0, localizer_.theta());
-  ::aos::Queue<LocalizerControl> localizer_queue(
-      ".frc971.control_loops.drivetrain.localizer_control");
-  ASSERT_TRUE(
-      localizer_queue.MakeWithBuilder().x(9.0).y(7.0).theta(1.0).Send());
-  RunIteration();
+  {
+    auto message = localizer_control_sender_.MakeMessage();
+    message->x = 9.0;
+    message->y = 7.0;
+    message->theta = 1.0;
+    ASSERT_TRUE(message.Send());
+  }
+  RunFor(dt());
 
   EXPECT_EQ(9.0, localizer_.x());
   EXPECT_EQ(7.0, localizer_.y());
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
index a5cf6af..34c9907 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
@@ -6,12 +6,18 @@
 
 #include "frc971/control_loops/drivetrain/trajectory.h"
 #include "frc971/control_loops/state_feedback_loop.h"
+#include "gflags/gflags.h"
+#if defined(SUPPORT_PLOT)
+#include "third_party/matplotlib-cpp/matplotlibcpp.h"
+#endif
 #include "y2016/constants.h"
 #include "y2016/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
 #include "y2016/control_loops/drivetrain/hybrid_velocity_drivetrain.h"
 #include "y2016/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
 #include "y2016/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
 
+DEFINE_bool(plot, false, "If true, plot");
+
 namespace frc971 {
 namespace control_loops {
 namespace drivetrain {
@@ -86,16 +92,23 @@
     : event_loop_(event_loop),
       robot_state_fetcher_(
           event_loop_->MakeFetcher<::aos::RobotState>(".aos.robot_state")),
-      dt_config_(dt_config),
-      drivetrain_plant_(MakePlantFromConfig(dt_config_)),
-      my_drivetrain_queue_(".frc971.control_loops.drivetrain_queue",
-                           ".frc971.control_loops.drivetrain_queue.goal",
-                           ".frc971.control_loops.drivetrain_queue.position",
-                           ".frc971.control_loops.drivetrain_queue.output",
-                           ".frc971.control_loops.drivetrain_queue.status"),
+      drivetrain_position_sender_(
+          event_loop_
+              ->MakeSender<::frc971::control_loops::DrivetrainQueue::Position>(
+                  ".frc971.control_loops.drivetrain_queue.position")),
+      drivetrain_output_fetcher_(
+          event_loop_
+              ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Output>(
+                  ".frc971.control_loops.drivetrain_queue.output")),
+      drivetrain_status_fetcher_(
+          event_loop_
+              ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Status>(
+                  ".frc971.control_loops.drivetrain_queue.status")),
       gyro_reading_sender_(
           event_loop->MakeSender<::frc971::sensors::GyroReading>(
               ".frc971.sensors.gyro_reading")),
+      dt_config_(dt_config),
+      drivetrain_plant_(MakePlantFromConfig(dt_config_)),
       velocity_drivetrain_(
           ::std::unique_ptr<StateFeedbackLoop<2, 2, 2, double,
                                               StateFeedbackHybridPlant<2, 2, 2>,
@@ -106,6 +119,29 @@
                   dt_config_.make_hybrid_drivetrain_velocity_loop()))) {
   Reinitialize();
   last_U_.setZero();
+
+  event_loop_->AddPhasedLoop(
+      [this](int) {
+        // Skip this the first time.
+        if (!first_) {
+          Simulate();
+          if (FLAGS_plot) {
+            EXPECT_TRUE(drivetrain_status_fetcher_.Fetch());
+
+            ::Eigen::Matrix<double, 2, 1> actual_position = GetPosition();
+            actual_x_.push_back(actual_position(0));
+            actual_y_.push_back(actual_position(1));
+
+            trajectory_x_.push_back(
+                drivetrain_status_fetcher_->trajectory_logging.x);
+            trajectory_y_.push_back(
+                drivetrain_status_fetcher_->trajectory_logging.y);
+          }
+        }
+        first_ = false;
+        SendPositionMessage();
+      },
+      dt_config_.dt);
 }
 
 void DrivetrainSimulation::Reinitialize() {
@@ -123,8 +159,8 @@
   const double right_encoder = GetRightPosition();
 
   {
-    ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Position>
-        position = my_drivetrain_queue_.position.MakeMessage();
+    ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Position>::Message
+        position = drivetrain_position_sender_.MakeMessage();
     position->left_encoder = left_encoder;
     position->right_encoder = right_encoder;
     position->left_shifter_position = left_gear_high_ ? 1.0 : 0.0;
@@ -146,10 +182,10 @@
 void DrivetrainSimulation::Simulate() {
   last_left_position_ = drivetrain_plant_.Y(0, 0);
   last_right_position_ = drivetrain_plant_.Y(1, 0);
-  EXPECT_TRUE(my_drivetrain_queue_.output.FetchLatest());
+  EXPECT_TRUE(drivetrain_output_fetcher_.Fetch());
   ::Eigen::Matrix<double, 2, 1> U = last_U_;
-  last_U_ << my_drivetrain_queue_.output->left_voltage,
-      my_drivetrain_queue_.output->right_voltage;
+  last_U_ << drivetrain_output_fetcher_->left_voltage,
+      drivetrain_output_fetcher_->right_voltage;
   {
     robot_state_fetcher_.Fetch();
     const double scalar = robot_state_fetcher_.get()
@@ -157,8 +193,8 @@
                               : 1.0;
     last_U_ *= scalar;
   }
-  left_gear_high_ = my_drivetrain_queue_.output->left_high;
-  right_gear_high_ = my_drivetrain_queue_.output->right_high;
+  left_gear_high_ = drivetrain_output_fetcher_->left_high;
+  right_gear_high_ = drivetrain_output_fetcher_->right_high;
 
   if (left_gear_high_) {
     if (right_gear_high_) {
@@ -187,6 +223,20 @@
       state_, U, dt_float);
 }
 
+void DrivetrainSimulation::MaybePlot() {
+#if defined(SUPPORT_PLOT)
+  if (FLAGS_plot) {
+    std::cout << "Plotting." << ::std::endl;
+    matplotlibcpp::figure();
+    matplotlibcpp::plot(actual_x_, actual_y_, {{"label", "actual position"}});
+    matplotlibcpp::plot(trajectory_x_, trajectory_y_,
+                        {{"label", "trajectory position"}});
+    matplotlibcpp::legend();
+    matplotlibcpp::show();
+  }
+#endif
+}
+
 }  // namespace testing
 }  // namespace drivetrain
 }  // namespace control_loops
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.h b/frc971/control_loops/drivetrain/drivetrain_test_lib.h
index cdde176..9a4f177 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.h
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.h
@@ -52,12 +52,6 @@
   double GetLeftPosition() const { return drivetrain_plant_.Y(0, 0); }
   double GetRightPosition() const { return drivetrain_plant_.Y(1, 0); }
 
-  // Sends out the position queue messages.
-  void SendPositionMessage();
-
-  // Simulates the drivetrain moving for one timestep.
-  void Simulate();
-
   void set_left_voltage_offset(double left_voltage_offset) {
     drivetrain_plant_.set_left_voltage_offset(left_voltage_offset);
   }
@@ -73,17 +67,30 @@
     return state_.block<2,1>(0,0);
   }
 
+  void MaybePlot();
+
  private:
+  // Sends out the position queue messages.
+  void SendPositionMessage();
+
+  // Simulates the drivetrain moving for one timestep.
+  void Simulate();
+
   ::aos::EventLoop *event_loop_;
   ::aos::Fetcher<::aos::RobotState> robot_state_fetcher_;
 
+  ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Position>
+      drivetrain_position_sender_;
+  ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Output>
+      drivetrain_output_fetcher_;
+  ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Status>
+      drivetrain_status_fetcher_;
+  ::aos::Sender<::frc971::sensors::GyroReading> gyro_reading_sender_;
+
   DrivetrainConfig<double> dt_config_;
 
   DrivetrainPlant drivetrain_plant_;
 
-  ::frc971::control_loops::DrivetrainQueue my_drivetrain_queue_;
-  ::aos::Sender<::frc971::sensors::GyroReading> gyro_reading_sender_;
-
   // This state is [x, y, theta, left_velocity, right_velocity].
   ::Eigen::Matrix<double, 5, 1> state_ = ::Eigen::Matrix<double, 5, 1>::Zero();
   ::std::unique_ptr<
@@ -96,6 +103,12 @@
 
   bool left_gear_high_ = false;
   bool right_gear_high_ = false;
+  bool first_ = true;
+
+  ::std::vector<double> actual_x_;
+  ::std::vector<double> actual_y_;
+  ::std::vector<double> trajectory_x_;
+  ::std::vector<double> trajectory_y_;
 };
 
 }  // namespace testing
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf.h b/frc971/control_loops/drivetrain/hybrid_ekf.h
index 68f67f6..16b0b61 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf.h
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -488,6 +488,9 @@
   // tuning things. I haven't yet tried messing with these values again.
   encoder_noise_ = 0.5 * R_kf_drivetrain(0, 0);
   gyro_noise_ = 0.1 * R_kf_drivetrain(2, 2);
+
+  X_hat_.setZero();
+  P_.setZero();
 }
 
 }  // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc b/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
index 7d401d6..f4d718e 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
@@ -9,7 +9,7 @@
 #include "gtest/gtest.h"
 #include "third_party/matplotlib-cpp/matplotlibcpp.h"
 
-DEFINE_bool(plot, false, "If true, plot");
+DECLARE_bool(plot);
 
 namespace chrono = ::std::chrono;
 
diff --git a/frc971/control_loops/drivetrain/localizer.h b/frc971/control_loops/drivetrain/localizer.h
index 23a978c..7b2ceae 100644
--- a/frc971/control_loops/drivetrain/localizer.h
+++ b/frc971/control_loops/drivetrain/localizer.h
@@ -1,6 +1,7 @@
 #ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_FIELD_ESTIMATOR_H_
 #define FRC971_CONTROL_LOOPS_DRIVETRAIN_FIELD_ESTIMATOR_H_
 
+#include "aos/events/event-loop.h"
 #include "frc971/control_loops/drivetrain/drivetrain_config.h"
 #include "frc971/control_loops/drivetrain/hybrid_ekf.h"
 #include "frc971/control_loops/pose.h"
@@ -94,18 +95,22 @@
 class DeadReckonEkf : public LocalizerInterface {
   typedef HybridEkf<double> Ekf;
   typedef typename Ekf::StateIdx StateIdx;
+
  public:
-  DeadReckonEkf(const DrivetrainConfig<double> &dt_config) : ekf_(dt_config) {
-    ekf_.ResetInitialState(::aos::monotonic_clock::now(), Ekf::State::Zero(),
-                           ekf_.P());
+  DeadReckonEkf(::aos::EventLoop *event_loop,
+                const DrivetrainConfig<double> &dt_config)
+      : ekf_(dt_config) {
+    event_loop->OnRun([this, event_loop]() {
+      ekf_.ResetInitialState(event_loop->monotonic_now(), Ekf::State::Zero(),
+                             ekf_.P());
+    });
     target_selector_.set_has_target(false);
   }
 
   void Update(const ::Eigen::Matrix<double, 2, 1> &U,
-                      ::aos::monotonic_clock::time_point now,
-                      double left_encoder, double right_encoder,
-                      double gyro_rate,
-                      double /*longitudinal_accelerometer*/) override {
+              ::aos::monotonic_clock::time_point now, double left_encoder,
+              double right_encoder, double gyro_rate,
+              double /*longitudinal_accelerometer*/) override {
     ekf_.UpdateEncodersAndGyro(left_encoder, right_encoder, gyro_rate, U, now);
   }
 
@@ -115,7 +120,8 @@
     const double left_encoder = ekf_.X_hat(StateIdx::kLeftEncoder);
     const double right_encoder = ekf_.X_hat(StateIdx::kRightEncoder);
     ekf_.ResetInitialState(t, (Ekf::State() << x, y, theta, left_encoder, 0,
-                               right_encoder, 0, 0, 0, 0).finished(),
+                               right_encoder, 0, 0, 0,
+                               0).finished(),
                            ekf_.P());
   };
 
diff --git a/frc971/control_loops/profiled_subsystem.q b/frc971/control_loops/profiled_subsystem.q
index 1b6b965..ac23907 100644
--- a/frc971/control_loops/profiled_subsystem.q
+++ b/frc971/control_loops/profiled_subsystem.q
@@ -195,4 +195,4 @@
 struct StaticZeroingSingleDOFProfiledSubsystemGoal {
   double unsafe_goal;
   .frc971.ProfileParameters profile_params;
-};
\ No newline at end of file
+};
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 de9c22b..58096f3 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,9 +1,11 @@
 #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 "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.q.h"
 #include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test_integral_plant.h"
 #include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test_plant.h"
 
@@ -25,7 +27,14 @@
     zeroing::AbsoluteEncoderZeroingEstimator,
     ::frc971::control_loops::AbsoluteEncoderProfiledJointStatus>;
 
-typedef ::testing::Types<SZSDPS_AbsEncoder, SZSDPS_PotAndAbsEncoder> TestTypes;
+typedef ::testing::Types<
+    ::std::pair<
+        SZSDPS_AbsEncoder,
+        StaticZeroingSingleDOFProfiledSubsystemAbsoluteEncoderTestQueueGroup>,
+    ::std::pair<
+        SZSDPS_PotAndAbsEncoder,
+        StaticZeroingSingleDOFProfiledSubsystemPotAndAbsoluteEncoderTestQueueGroup>>
+    TestTypes;
 
 constexpr ::frc971::constants::Range kRange{
     .lower_hard = -0.01, .upper_hard = 0.250, .lower = 0.01, .upper = 0.235};
@@ -69,170 +78,78 @@
   return params;
 }
 
-template <typename ZeroingEstimator, typename ProfiledJointStatus>
-struct TestIntakeSystemData {
-  ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal goal;
-
-  ProfiledJointStatus status;
-
-  typename ZeroingEstimator::Position position;
-
-  double output;
-};
-
 }  // namespace
 
-template <typename SZSDPS>
+template <typename SZSDPS, typename QueueGroup>
 class TestIntakeSystemSimulation {
  public:
-  TestIntakeSystemSimulation()
-      : subsystem_plant_(new CappedTestPlant(
+  typedef typename std::remove_reference<decltype(
+      *(static_cast<QueueGroup *>(NULL)->goal.MakeMessage().get()))>::type
+      GoalType;
+  typedef typename std::remove_reference<decltype(
+      *(static_cast<QueueGroup *>(NULL)->position.MakeMessage().get()))>::type
+      PositionType;
+  typedef typename std::remove_reference<decltype(
+      *(static_cast<QueueGroup *>(NULL)->status.MakeMessage().get()))>::type
+      StatusType;
+  typedef typename std::remove_reference<decltype(
+      *(static_cast<QueueGroup *>(NULL)->output.MakeMessage().get()))>::type
+      OutputType;
+
+  TestIntakeSystemSimulation(::aos::EventLoop *event_loop,
+                             chrono::nanoseconds dt)
+      : event_loop_(event_loop),
+        dt_(dt),
+        subsystem_position_sender_(
+            event_loop_->MakeSender<PositionType>(".position")),
+        subsystem_status_fetcher_(
+            event_loop_->MakeFetcher<StatusType>(".status")),
+        subsystem_output_fetcher_(
+            event_loop_->MakeFetcher<OutputType>(".output")),
+        subsystem_plant_(new CappedTestPlant(
             ::frc971::control_loops::MakeTestIntakeSystemPlant())),
         subsystem_sensor_sim_(kEncoderIndexDifference) {
     // Start the subsystem out in the middle by default.
     InitializeSubsystemPosition((kRange.lower + kRange.upper) / 2.0);
+
+    event_loop_->AddPhasedLoop(
+        [this](int) {
+          // Skip this the first time.
+          if (!first_) {
+            this->Simulate();
+          }
+          first_ = false;
+          this->SendPositionMessage();
+        },
+        dt);
   }
 
   void InitializeSubsystemPosition(double start_pos) {
-    subsystem_plant_->mutable_X(0, 0) = start_pos;
-    subsystem_plant_->mutable_X(1, 0) = 0.0;
+    this->subsystem_plant_->mutable_X(0, 0) = start_pos;
+    this->subsystem_plant_->mutable_X(1, 0) = 0.0;
 
-    InitializeSensorSim(start_pos);
+    this->InitializeSensorSim(start_pos);
   }
 
   void InitializeSensorSim(double start_pos);
 
-  // Updates the position message with the position of the subsystem.
-  void UpdatePositionMessage(
-      typename SZSDPS::ZeroingEstimator::Position *position) {
-    subsystem_sensor_sim_.GetSensorValues(position);
-  }
-
-  double subsystem_position() const { return subsystem_plant_->X(0, 0); }
-  double subsystem_velocity() const { return subsystem_plant_->X(1, 0); }
+  double subsystem_position() const { return this->subsystem_plant_->X(0, 0); }
+  double subsystem_velocity() const { return this->subsystem_plant_->X(1, 0); }
 
   // Sets the difference between the commanded and applied powers.
   // This lets us test that the integrators work.
   void set_subsystem_voltage_offset(double voltage_offset) {
-    subsystem_plant_->set_voltage_offset(voltage_offset);
+    this->subsystem_plant_->set_voltage_offset(voltage_offset);
   }
 
-  // Simulates the subsystem for a single timestep.
-  void Simulate(const double output_voltage, const int32_t state) {
-    const double voltage_check_subsystem =
-        (static_cast<typename SZSDPS::State>(state) == SZSDPS::State::RUNNING)
-            ? kOperatingVoltage
-            : kZeroingVoltage;
+  // Sends a queue message with the position.
+  void SendPositionMessage() {
+    typename ::aos::Sender<PositionType>::Message position =
+        subsystem_position_sender_.MakeMessage();
 
-    EXPECT_LE(::std::abs(output_voltage), voltage_check_subsystem);
+    this->subsystem_sensor_sim_.GetSensorValues(&position->position);
 
-    ::Eigen::Matrix<double, 1, 1> subsystem_U;
-    subsystem_U << output_voltage + subsystem_plant_->voltage_offset();
-    subsystem_plant_->Update(subsystem_U);
-
-    const double position_subsystem = subsystem_plant_->Y(0, 0);
-
-    subsystem_sensor_sim_.MoveTo(position_subsystem);
-
-    EXPECT_GE(position_subsystem, kRange.lower_hard);
-    EXPECT_LE(position_subsystem, kRange.upper_hard);
-  }
-
- private:
-  ::std::unique_ptr<CappedTestPlant> subsystem_plant_;
-  PositionSensorSimulator subsystem_sensor_sim_;
-};
-
-template <>
-void TestIntakeSystemSimulation<SZSDPS_PotAndAbsEncoder>::InitializeSensorSim(
-    double start_pos) {
-  subsystem_sensor_sim_.Initialize(
-      start_pos, kNoiseScalar, 0.0,
-      TestIntakeSystemValues<
-          typename SZSDPS_PotAndAbsEncoder::ZeroingEstimator>::kZeroing
-          .measured_absolute_position);
-}
-
-template <>
-void TestIntakeSystemSimulation<SZSDPS_AbsEncoder>::InitializeSensorSim(
-    double start_pos) {
-  subsystem_sensor_sim_.Initialize(
-      start_pos, kNoiseScalar, 0.0,
-      TestIntakeSystemValues<
-          typename SZSDPS_PotAndAbsEncoder::ZeroingEstimator>::kZeroing
-          .measured_absolute_position);
-}
-
-template <typename TSZSDPS>
-class IntakeSystemTest : public ::aos::testing::ControlLoopTest {
- protected:
-  using SZSDPS = TSZSDPS;
-  using ZeroingEstimator = typename SZSDPS::ZeroingEstimator;
-  using ProfiledJointStatus = typename SZSDPS::ProfiledJointStatus;
-
-  IntakeSystemTest()
-      : robot_state_fetcher_(
-            simulation_event_loop_.MakeFetcher<::aos::RobotState>(
-                ".aos.robot_state")),
-        subsystem_(TestIntakeSystemValues<
-                   typename SZSDPS::ZeroingEstimator>::make_params()),
-        subsystem_plant_() {}
-
-  void VerifyNearGoal() {
-    EXPECT_NEAR(subsystem_data_.goal.unsafe_goal,
-                subsystem_data_.status.position, 0.001);
-    EXPECT_NEAR(subsystem_data_.goal.unsafe_goal,
-                subsystem_plant_.subsystem_position(), 0.001);
-    EXPECT_NEAR(subsystem_data_.status.velocity, 0, 0.001);
-  }
-
-  // Runs one iteration of the whole simulation.
-  void RunIteration(bool enabled = true, bool null_goal = false) {
-    SendMessages(enabled);
-    subsystem_plant_.UpdatePositionMessage(&subsystem_data_.position);
-
-    // Checks if the robot was reset and resets the subsystem.
-    // Required since there is no ControlLoop to reset it (ie. a superstructure)
-    robot_state_fetcher_.Fetch();
-    if (robot_state_fetcher_.get()) {
-      if (sensor_reader_pid_ != robot_state_fetcher_->reader_pid) {
-        LOG(ERROR, "WPILib reset, restarting\n");
-        subsystem_.Reset();
-      }
-      sensor_reader_pid_ = robot_state_fetcher_->reader_pid;
-    }
-    subsystem_goal_.unsafe_goal = subsystem_data_.goal.unsafe_goal;
-    subsystem_goal_.profile_params = subsystem_data_.goal.profile_params;
-
-    subsystem_.Iterate(null_goal ? nullptr : &subsystem_goal_,
-                       &subsystem_data_.position, &subsystem_data_.output,
-                       &subsystem_data_.status);
-
-    subsystem_plant_.Simulate(subsystem_data_.output,
-                              subsystem_data_.status.state);
-
-    TickTime(::std::chrono::microseconds(5050));
-  }
-
-  // Runs iterations until the specified amount of simulated time has elapsed.
-  void RunForTime(const monotonic_clock::duration run_for, bool enabled = true,
-                  bool null_goal = false) {
-    const auto start_time = monotonic_clock::now();
-    while (monotonic_clock::now() < start_time + run_for) {
-      const auto loop_start_time = monotonic_clock::now();
-      double begin_subsystem_velocity = subsystem_plant_.subsystem_velocity();
-
-      RunIteration(enabled, null_goal);
-
-      const double loop_time = ::aos::time::DurationInSeconds(
-          monotonic_clock::now() - loop_start_time);
-      const double subsystem_acceleration =
-          (subsystem_plant_.subsystem_velocity() - begin_subsystem_velocity) /
-          loop_time;
-      EXPECT_NEAR(subsystem_acceleration, 0.0, peak_subsystem_acceleration_);
-      EXPECT_NEAR(subsystem_plant_.subsystem_velocity(), 0.0,
-                  peak_subsystem_velocity_);
-    }
+    position.Send();
   }
 
   void set_peak_subsystem_acceleration(double value) {
@@ -242,53 +159,233 @@
     peak_subsystem_velocity_ = value;
   }
 
-  // Create a new instance of the test queue so that it invalidates the queue
-  // that it points to.  Otherwise, we will have a pointer to shared memory
-  // that is no longer valid.
-  // TestIntakeSystemData subsystem_data_;
-  ::aos::ShmEventLoop simulation_event_loop_;
-  ::aos::Fetcher<::aos::RobotState> robot_state_fetcher_;
+  // Simulates the subsystem for a single timestep.
+  void Simulate() {
+    EXPECT_TRUE(subsystem_output_fetcher_.Fetch());
+    EXPECT_TRUE(subsystem_status_fetcher_.Fetch());
 
-  // Create a control loop and simulation.
-  SZSDPS subsystem_;
-  TestIntakeSystemSimulation<SZSDPS> subsystem_plant_;
+    const double begin_subsystem_velocity = subsystem_velocity();
 
-  StaticZeroingSingleDOFProfiledSubsystemGoal subsystem_goal_;
+    const double voltage_check_subsystem =
+        (static_cast<typename SZSDPS::State>(
+             subsystem_status_fetcher_->status.state) == SZSDPS::State::RUNNING)
+            ? kOperatingVoltage
+            : kZeroingVoltage;
 
-  TestIntakeSystemData<typename SZSDPS::ZeroingEstimator,
-                       typename SZSDPS::ProfiledJointStatus>
-      subsystem_data_;
+    EXPECT_LE(::std::abs(subsystem_output_fetcher_->output),
+              voltage_check_subsystem);
+
+    ::Eigen::Matrix<double, 1, 1> subsystem_U;
+    subsystem_U << subsystem_output_fetcher_->output +
+                       subsystem_plant_->voltage_offset();
+    subsystem_plant_->Update(subsystem_U);
+
+    const double position_subsystem = subsystem_plant_->Y(0, 0);
+
+    subsystem_sensor_sim_.MoveTo(position_subsystem);
+
+    EXPECT_GE(position_subsystem, kRange.lower_hard);
+    EXPECT_LE(position_subsystem, kRange.upper_hard);
+
+    const double loop_time = ::aos::time::DurationInSeconds(dt_);
+    const double subsystem_acceleration =
+        (subsystem_velocity() - begin_subsystem_velocity) / loop_time;
+    EXPECT_NEAR(subsystem_acceleration, 0.0, peak_subsystem_acceleration_);
+    EXPECT_NEAR(subsystem_velocity(), 0.0, peak_subsystem_velocity_);
+  }
 
  private:
+  ::aos::EventLoop *event_loop_;
+  chrono::nanoseconds dt_;
+
+  bool first_ = true;
+
+  typename ::aos::Sender<PositionType> subsystem_position_sender_;
+  typename ::aos::Fetcher<StatusType> subsystem_status_fetcher_;
+  typename ::aos::Fetcher<OutputType> subsystem_output_fetcher_;
+
+  ::std::unique_ptr<CappedTestPlant> subsystem_plant_;
+  PositionSensorSimulator subsystem_sensor_sim_;
+
   // The acceleration limits to check for while moving.
   double peak_subsystem_acceleration_ = 1e10;
   // The velocity limits to check for while moving.
   double peak_subsystem_velocity_ = 1e10;
+};
 
-  int32_t sensor_reader_pid_ = 0;
+template <>
+void TestIntakeSystemSimulation<
+    SZSDPS_PotAndAbsEncoder,
+    StaticZeroingSingleDOFProfiledSubsystemPotAndAbsoluteEncoderTestQueueGroup>::
+    InitializeSensorSim(double start_pos) {
+  subsystem_sensor_sim_.Initialize(
+      start_pos, kNoiseScalar, 0.0,
+      TestIntakeSystemValues<
+          typename SZSDPS_PotAndAbsEncoder::ZeroingEstimator>::kZeroing
+          .measured_absolute_position);
+}
+
+template <>
+void TestIntakeSystemSimulation<
+    SZSDPS_AbsEncoder,
+    StaticZeroingSingleDOFProfiledSubsystemAbsoluteEncoderTestQueueGroup>::
+    InitializeSensorSim(double start_pos) {
+  subsystem_sensor_sim_.Initialize(
+      start_pos, kNoiseScalar, 0.0,
+      TestIntakeSystemValues<
+          typename SZSDPS_PotAndAbsEncoder::ZeroingEstimator>::kZeroing
+          .measured_absolute_position);
+}
+
+// Class to represent a module using a subsystem.  This lets us use event loops
+// to wrap it.
+template <typename QueueGroup, typename SZSDPS>
+class Subsystem : public ::aos::controls::ControlLoop<QueueGroup> {
+ public:
+  typedef typename std::remove_reference<decltype(
+      *(static_cast<QueueGroup *>(NULL)->goal.MakeMessage().get()))>::type
+      GoalType;
+  typedef typename std::remove_reference<decltype(
+      *(static_cast<QueueGroup *>(NULL)->position.MakeMessage().get()))>::type
+      PositionType;
+  typedef typename std::remove_reference<decltype(
+      *(static_cast<QueueGroup *>(NULL)->status.MakeMessage().get()))>::type
+      StatusType;
+  typedef typename std::remove_reference<decltype(
+      *(static_cast<QueueGroup *>(NULL)->output.MakeMessage().get()))>::type
+      OutputType;
+
+  Subsystem(::aos::EventLoop *event_loop, const ::std::string &name)
+      : aos::controls::ControlLoop<QueueGroup>(event_loop, name),
+        subsystem_(TestIntakeSystemValues<
+                   typename SZSDPS::ZeroingEstimator>::make_params()) {}
+
+  void RunIteration(const GoalType *unsafe_goal, const PositionType *position,
+                    OutputType *output, StatusType *status) {
+    if (this->WasReset()) {
+      LOG(ERROR, "WPILib reset, restarting\n");
+      subsystem_.Reset();
+    }
+
+    // Convert one goal type to another...
+    StaticZeroingSingleDOFProfiledSubsystemGoal goal;
+    if (unsafe_goal != nullptr ) {
+      goal.unsafe_goal = unsafe_goal->unsafe_goal;
+      goal.profile_params.max_velocity =
+          unsafe_goal->profile_params.max_velocity;
+      goal.profile_params.max_acceleration =
+          unsafe_goal->profile_params.max_acceleration;
+    }
+
+    subsystem_.Iterate(
+        unsafe_goal == nullptr ? nullptr : &goal, &position->position,
+        output == nullptr ? nullptr : &output->output, &status->status);
+  }
+
+  SZSDPS *subsystem() { return &subsystem_; }
+
+ private:
+  SZSDPS subsystem_;
+};
+
+template <typename TSZSDPS>
+class IntakeSystemTest : public ::aos::testing::ControlLoopTest {
+ protected:
+  using SZSDPS = typename TSZSDPS::first_type;
+  using QueueGroup = typename TSZSDPS::second_type;
+  using ZeroingEstimator = typename SZSDPS::ZeroingEstimator;
+  using ProfiledJointStatus = typename SZSDPS::ProfiledJointStatus;
+
+  typedef typename std::remove_reference<decltype(
+      *(static_cast<QueueGroup *>(NULL)->goal.MakeMessage().get()))>::type
+      GoalType;
+  typedef typename std::remove_reference<decltype(
+      *(static_cast<QueueGroup *>(NULL)->position.MakeMessage().get()))>::type
+      PositionType;
+  typedef typename std::remove_reference<decltype(
+      *(static_cast<QueueGroup *>(NULL)->status.MakeMessage().get()))>::type
+      StatusType;
+  typedef typename std::remove_reference<decltype(
+      *(static_cast<QueueGroup *>(NULL)->output.MakeMessage().get()))>::type
+      OutputType;
+
+  IntakeSystemTest()
+      : ::aos::testing::ControlLoopTest(chrono::microseconds(5050)),
+        test_event_loop_(MakeEventLoop()),
+        subsystem_goal_sender_(test_event_loop_->MakeSender<GoalType>(".goal")),
+        subsystem_goal_fetcher_(
+            test_event_loop_->MakeFetcher<GoalType>(".goal")),
+        subsystem_status_fetcher_(
+            test_event_loop_->MakeFetcher<StatusType>(".status")),
+        subsystem_event_loop_(MakeEventLoop()),
+        subsystem_(subsystem_event_loop_.get(), ""),
+        subsystem_plant_event_loop_(MakeEventLoop()),
+        subsystem_plant_(subsystem_plant_event_loop_.get(), dt()) {}
+
+  void VerifyNearGoal() {
+    subsystem_goal_fetcher_.Fetch();
+    EXPECT_TRUE(subsystem_goal_fetcher_.get() != nullptr);
+    EXPECT_TRUE(subsystem_status_fetcher_.Fetch());
+
+    EXPECT_NEAR(subsystem_goal_fetcher_->unsafe_goal,
+                subsystem_status_fetcher_->status.position, 0.001);
+    EXPECT_NEAR(subsystem_goal_fetcher_->unsafe_goal,
+                subsystem_plant_.subsystem_position(), 0.001);
+    EXPECT_NEAR(subsystem_status_fetcher_->status.velocity, 0, 0.001);
+  }
+
+  SZSDPS *subsystem() { return subsystem_.subsystem(); }
+
+  void set_peak_subsystem_acceleration(double value) {
+    set_peak_subsystem_acceleration(value);
+  }
+  void set_peak_subsystem_velocity(double value) {
+    set_peak_subsystem_velocity(value);
+  }
+
+  ::std::unique_ptr<::aos::EventLoop> test_event_loop_;
+  ::aos::Sender<GoalType> subsystem_goal_sender_;
+  ::aos::Fetcher<GoalType> subsystem_goal_fetcher_;
+  ::aos::Fetcher<StatusType> subsystem_status_fetcher_;
+
+  // Create a control loop and simulation.
+  ::std::unique_ptr<::aos::EventLoop> subsystem_event_loop_;
+  Subsystem<QueueGroup, SZSDPS> subsystem_;
+
+  ::std::unique_ptr<::aos::EventLoop> subsystem_plant_event_loop_;
+  TestIntakeSystemSimulation<SZSDPS, QueueGroup> subsystem_plant_;
 };
 
 TYPED_TEST_CASE_P(IntakeSystemTest);
 
 // Tests that the subsystem does nothing when the goal is zero.
 TYPED_TEST_P(IntakeSystemTest, DoesNothing) {
+  this->SetEnabled(true);
   // Intake system uses 0.05 to test for 0.
-  this->subsystem_data_.goal.unsafe_goal = 0.05;
-  this->RunForTime(chrono::seconds(5));
+  {
+    auto message = this->subsystem_goal_sender_.MakeMessage();
+    message->unsafe_goal = 0.05;
+    EXPECT_TRUE(message.Send());
+  }
+  this->RunFor(chrono::seconds(5));
 
   this->VerifyNearGoal();
 }
 
 // Tests that the subsystem loop can reach a goal.
 TYPED_TEST_P(IntakeSystemTest, ReachesGoal) {
+  this->SetEnabled(true);
   // Set a reasonable goal.
-  auto &goal = this->subsystem_data_.goal;
-  goal.unsafe_goal = 0.1;
-  goal.profile_params.max_velocity = 1;
-  goal.profile_params.max_acceleration = 0.5;
+  {
+    auto message = this->subsystem_goal_sender_.MakeMessage();
+    message->unsafe_goal = 0.1;
+    message->profile_params.max_velocity = 1;
+    message->profile_params.max_acceleration = 0.5;
+    EXPECT_TRUE(message.Send());
+  }
 
   // Give it a lot of time to get there.
-  this->RunForTime(chrono::seconds(8));
+  this->RunFor(chrono::seconds(8));
 
   this->VerifyNearGoal();
 }
@@ -296,35 +393,43 @@
 // Makes sure that the voltage on a motor is properly pulled back after
 // saturation such that we don't get weird or bad (e.g. oscillating) behaviour.
 TYPED_TEST_P(IntakeSystemTest, SaturationTest) {
+  this->SetEnabled(true);
   // Zero it before we move.
-  auto &goal = this->subsystem_data_.goal;
-  goal.unsafe_goal = kRange.upper;
-  this->RunForTime(chrono::seconds(8));
+  {
+    auto message = this->subsystem_goal_sender_.MakeMessage();
+    message->unsafe_goal = kRange.upper;
+    EXPECT_TRUE(message.Send());
+  }
+  this->RunFor(chrono::seconds(8));
   this->VerifyNearGoal();
 
   // Try a low acceleration move with a high max velocity and verify the
   // acceleration is capped like expected.
   {
-    goal.unsafe_goal = kRange.lower;
-    goal.profile_params.max_velocity = 20.0;
-    goal.profile_params.max_acceleration = 0.1;
+    auto message = this->subsystem_goal_sender_.MakeMessage();
+    message->unsafe_goal = kRange.lower;
+    message->profile_params.max_velocity = 20.0;
+    message->profile_params.max_acceleration = 0.1;
+    EXPECT_TRUE(message.Send());
   }
   this->set_peak_subsystem_velocity(23.0);
   this->set_peak_subsystem_acceleration(0.2);
 
-  this->RunForTime(chrono::seconds(8));
+  this->RunFor(chrono::seconds(8));
   this->VerifyNearGoal();
 
   // Now do a high acceleration move with a low velocity limit.
   {
-    goal.unsafe_goal = kRange.upper;
-    goal.profile_params.max_velocity = 0.1;
-    goal.profile_params.max_acceleration = 100;
+    auto message = this->subsystem_goal_sender_.MakeMessage();
+    message->unsafe_goal = kRange.upper;
+    message->profile_params.max_velocity = 0.1;
+    message->profile_params.max_acceleration = 100;
+    EXPECT_TRUE(message.Send());
   }
 
   this->set_peak_subsystem_velocity(0.2);
   this->set_peak_subsystem_acceleration(103);
-  this->RunForTime(chrono::seconds(8));
+  this->RunFor(chrono::seconds(8));
 
   this->VerifyNearGoal();
 }
@@ -332,184 +437,244 @@
 // Tests that the subsystem loop doesn't try and go beyond it's physical range
 // of the mechanisms.
 TYPED_TEST_P(IntakeSystemTest, RespectsRange) {
-  auto &goal = this->subsystem_data_.goal;
+  this->SetEnabled(true);
+
   // Set some ridiculous goals to test upper limits.
   {
-    goal.unsafe_goal = 100.0;
-    goal.profile_params.max_velocity = 1;
-    goal.profile_params.max_acceleration = 0.5;
+    auto message = this->subsystem_goal_sender_.MakeMessage();
+    message->unsafe_goal = 100.0;
+    message->profile_params.max_velocity = 1;
+    message->profile_params.max_acceleration = 0.5;
+    EXPECT_TRUE(message.Send());
   }
-  this->RunForTime(chrono::seconds(10));
+  this->RunFor(chrono::seconds(10));
 
   // Check that we are near our soft limit.
-  EXPECT_NEAR(kRange.upper, this->subsystem_data_.status.position, 0.001);
+  EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
+  EXPECT_NEAR(kRange.upper, this->subsystem_status_fetcher_->status.position,
+              0.001);
 
   // Set some ridiculous goals to test lower limits.
   {
-    goal.unsafe_goal = -100.0;
-    goal.profile_params.max_velocity = 1;
-    goal.profile_params.max_acceleration = 0.5;
+    auto message = this->subsystem_goal_sender_.MakeMessage();
+    message->unsafe_goal = -100.0;
+    message->profile_params.max_velocity = 1;
+    message->profile_params.max_acceleration = 0.5;
+    EXPECT_TRUE(message.Send());
   }
 
-  this->RunForTime(chrono::seconds(10));
+  this->RunFor(chrono::seconds(10));
 
   // Check that we are near our soft limit.
-  EXPECT_NEAR(kRange.lower, this->subsystem_data_.status.position, 0.001);
+  EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
+  EXPECT_NEAR(kRange.lower, this->subsystem_status_fetcher_->status.position,
+              0.001);
 }
 
 // Tests that the subsystem loop zeroes when run for a while.
 TYPED_TEST_P(IntakeSystemTest, ZeroTest) {
-  auto &goal = this->subsystem_data_.goal;
+  this->SetEnabled(true);
+
   {
-    goal.unsafe_goal = kRange.upper;
-    goal.profile_params.max_velocity = 1;
-    goal.profile_params.max_acceleration = 0.5;
+    auto message = this->subsystem_goal_sender_.MakeMessage();
+    message->unsafe_goal = kRange.upper;
+    message->profile_params.max_velocity = 1;
+    message->profile_params.max_acceleration = 0.5;
+    EXPECT_TRUE(message.Send());
   }
 
-  this->RunForTime(chrono::seconds(10));
+  this->RunFor(chrono::seconds(10));
 
   this->VerifyNearGoal();
 }
 
 // Tests that the loop zeroes when run for a while without a goal.
 TYPED_TEST_P(IntakeSystemTest, ZeroNoGoal) {
-  this->RunForTime(chrono::seconds(5));
+  this->SetEnabled(true);
+  this->RunFor(chrono::seconds(5));
 
-  EXPECT_EQ(TestFixture::SZSDPS::State::RUNNING, this->subsystem_.state());
+  EXPECT_EQ(TestFixture::SZSDPS::State::RUNNING, this->subsystem()->state());
 }
 
 TYPED_TEST_P(IntakeSystemTest, LowerHardstopStartup) {
+  this->SetEnabled(true);
   this->subsystem_plant_.InitializeSubsystemPosition(kRange.lower_hard);
-  this->subsystem_data_.goal.unsafe_goal = kRange.upper;
-  this->RunForTime(chrono::seconds(10));
+  {
+    auto message = this->subsystem_goal_sender_.MakeMessage();
+    message->unsafe_goal = kRange.upper;
+    EXPECT_TRUE(message.Send());
+  }
+  this->RunFor(chrono::seconds(10));
 
   this->VerifyNearGoal();
 }
 
 // Tests that starting at the upper hardstops doesn't cause an abort.
 TYPED_TEST_P(IntakeSystemTest, UpperHardstopStartup) {
+  this->SetEnabled(true);
+
   this->subsystem_plant_.InitializeSubsystemPosition(kRange.upper_hard);
-  this->subsystem_data_.goal.unsafe_goal = kRange.upper;
-  this->RunForTime(chrono::seconds(10));
+  {
+    auto message = this->subsystem_goal_sender_.MakeMessage();
+    message->unsafe_goal = kRange.upper;
+    EXPECT_TRUE(message.Send());
+  }
+  this->RunFor(chrono::seconds(10));
 
   this->VerifyNearGoal();
 }
 
 // Tests that resetting WPILib results in a rezero.
 TYPED_TEST_P(IntakeSystemTest, ResetTest) {
-  this->subsystem_plant_.InitializeSubsystemPosition(kRange.upper);
-  this->subsystem_data_.goal.unsafe_goal = kRange.upper - 0.1;
-  this->RunForTime(chrono::seconds(10));
+  this->SetEnabled(true);
 
-  EXPECT_EQ(TestFixture::SZSDPS::State::RUNNING, this->subsystem_.state());
+  this->subsystem_plant_.InitializeSubsystemPosition(kRange.upper);
+  {
+    auto message = this->subsystem_goal_sender_.MakeMessage();
+    message->unsafe_goal = kRange.upper - 0.1;
+    EXPECT_TRUE(message.Send());
+  }
+  this->RunFor(chrono::seconds(10));
+
+  EXPECT_EQ(TestFixture::SZSDPS::State::RUNNING, this->subsystem()->state());
 
   this->VerifyNearGoal();
   this->SimulateSensorReset();
-  this->RunForTime(chrono::milliseconds(100));
+  this->RunFor(chrono::milliseconds(100));
 
   EXPECT_EQ(TestFixture::SZSDPS::State::UNINITIALIZED,
-            this->subsystem_.state());
+            this->subsystem()->state());
 
-  this->RunForTime(chrono::seconds(10));
+  this->RunFor(chrono::seconds(10));
 
-  EXPECT_EQ(TestFixture::SZSDPS::State::RUNNING, this->subsystem_.state());
+  EXPECT_EQ(TestFixture::SZSDPS::State::RUNNING, this->subsystem()->state());
   this->VerifyNearGoal();
 }
 
 // Tests that the internal goals don't change while disabled.
 TYPED_TEST_P(IntakeSystemTest, DisabledGoalTest) {
-  this->subsystem_data_.goal.unsafe_goal = kRange.lower + 0.03;
+  {
+    auto message = this->subsystem_goal_sender_.MakeMessage();
+    message->unsafe_goal = kRange.lower + 0.03;
+    EXPECT_TRUE(message.Send());
+  }
 
   // Checks that the subsystem has not moved from its starting position at 0
-  this->RunForTime(chrono::milliseconds(100), false);
-  EXPECT_EQ(0.0, this->subsystem_.goal(0));
+  this->RunFor(chrono::milliseconds(100));
+  EXPECT_EQ(0.0, this->subsystem()->goal(0));
 
   // Now make sure they move correctly
-  this->RunForTime(chrono::seconds(4), true);
-  EXPECT_NE(0.0, this->subsystem_.goal(0));
+  this->SetEnabled(true);
+  this->RunFor(chrono::seconds(4));
+  EXPECT_NE(0.0, this->subsystem()->goal(0));
 }
 
 // Tests that zeroing while disabled works.
 TYPED_TEST_P(IntakeSystemTest, DisabledZeroTest) {
-  this->subsystem_data_.goal.unsafe_goal = kRange.lower;
+  {
+    auto message = this->subsystem_goal_sender_.MakeMessage();
+    message->unsafe_goal = kRange.lower;
+    EXPECT_TRUE(message.Send());
+  }
 
   // Run disabled for 2 seconds
-  this->RunForTime(chrono::seconds(2), false);
-  EXPECT_EQ(TestFixture::SZSDPS::State::RUNNING, this->subsystem_.state());
+  this->RunFor(chrono::seconds(2));
+  EXPECT_EQ(TestFixture::SZSDPS::State::RUNNING, this->subsystem()->state());
 
-  this->RunForTime(chrono::seconds(4), true);
+  this->SetEnabled(true);
+  this->RunFor(chrono::seconds(4));
 
   this->VerifyNearGoal();
 }
 
 // Tests that set_min_position limits range properly
 TYPED_TEST_P(IntakeSystemTest, MinPositionTest) {
-  this->subsystem_data_.goal.unsafe_goal = kRange.lower_hard;
-  this->RunForTime(chrono::seconds(2), true);
+  this->SetEnabled(true);
+  {
+    auto message = this->subsystem_goal_sender_.MakeMessage();
+    message->unsafe_goal = kRange.lower_hard;
+    EXPECT_TRUE(message.Send());
+  }
+  this->RunFor(chrono::seconds(2));
 
   // Check that kRange.lower is used as the default min position
-  EXPECT_EQ(this->subsystem_.goal(0), kRange.lower);
-  EXPECT_NEAR(this->subsystem_data_.status.position, kRange.lower, 0.001);
+  EXPECT_EQ(this->subsystem()->goal(0), kRange.lower);
+  EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
+  EXPECT_NEAR(kRange.lower, this->subsystem_status_fetcher_->status.position,
+              0.001);
 
   // Set min position and check that the subsystem increases to that position
-  this->subsystem_.set_min_position(kRange.lower + 0.05);
-  this->RunForTime(chrono::seconds(2), true);
-  EXPECT_EQ(this->subsystem_.goal(0), kRange.lower + 0.05);
-  EXPECT_NEAR(this->subsystem_data_.status.position, kRange.lower + 0.05,
+  this->subsystem()->set_min_position(kRange.lower + 0.05);
+  this->RunFor(chrono::seconds(2));
+  EXPECT_EQ(this->subsystem()->goal(0), kRange.lower + 0.05);
+  EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
+  EXPECT_NEAR(kRange.lower + 0.05, this->subsystem_status_fetcher_->status.position,
               0.001);
 
   // Clear min position and check that the subsystem returns to kRange.lower
-  this->subsystem_.clear_min_position();
-  this->RunForTime(chrono::seconds(2), true);
-  EXPECT_EQ(this->subsystem_.goal(0), kRange.lower);
-  EXPECT_NEAR(this->subsystem_data_.status.position, kRange.lower, 0.001);
+  this->subsystem()->clear_min_position();
+  this->RunFor(chrono::seconds(2));
+  EXPECT_EQ(this->subsystem()->goal(0), kRange.lower);
+  EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
+  EXPECT_NEAR(kRange.lower, this->subsystem_status_fetcher_->status.position,
+              0.001);
 }
 
 // Tests that set_max_position limits range properly
 TYPED_TEST_P(IntakeSystemTest, MaxPositionTest) {
-  this->subsystem_data_.goal.unsafe_goal = kRange.upper_hard;
-  this->RunForTime(chrono::seconds(2), true);
+  this->SetEnabled(true);
+
+  {
+    auto message = this->subsystem_goal_sender_.MakeMessage();
+    message->unsafe_goal = kRange.upper_hard;
+    EXPECT_TRUE(message.Send());
+  }
+  this->RunFor(chrono::seconds(2));
 
   // Check that kRange.upper is used as the default max position
-  EXPECT_EQ(this->subsystem_.goal(0), kRange.upper);
-  EXPECT_NEAR(this->subsystem_data_.status.position, kRange.upper, 0.001);
+  EXPECT_EQ(this->subsystem()->goal(0), kRange.upper);
+  EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
+  EXPECT_NEAR(kRange.upper, this->subsystem_status_fetcher_->status.position,
+              0.001);
 
   // Set max position and check that the subsystem lowers to that position
-  this->subsystem_.set_max_position(kRange.upper - 0.05);
-  this->RunForTime(chrono::seconds(2), true);
-  EXPECT_EQ(this->subsystem_.goal(0), kRange.upper - 0.05);
-  EXPECT_NEAR(this->subsystem_data_.status.position, kRange.upper - 0.05,
+  this->subsystem()->set_max_position(kRange.upper - 0.05);
+  this->RunFor(chrono::seconds(2));
+  EXPECT_EQ(this->subsystem()->goal(0), kRange.upper - 0.05);
+  EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
+  EXPECT_NEAR(kRange.upper - 0.05, this->subsystem_status_fetcher_->status.position,
               0.001);
 
   // Clear max position and check that the subsystem returns to kRange.upper
-  this->subsystem_.clear_max_position();
-  this->RunForTime(chrono::seconds(2), true);
-  EXPECT_EQ(this->subsystem_.goal(0), kRange.upper);
-  EXPECT_NEAR(this->subsystem_data_.status.position, kRange.upper, 0.001);
+  this->subsystem()->clear_max_position();
+  this->RunFor(chrono::seconds(2));
+  EXPECT_EQ(this->subsystem()->goal(0), kRange.upper);
+  EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
+  EXPECT_NEAR(kRange.upper, this->subsystem_status_fetcher_->status.position,
+              0.001);
 }
 
 // Tests that the subsystem maintains its current position when sent a null goal
 TYPED_TEST_P(IntakeSystemTest, NullGoalTest) {
-  this->subsystem_data_.goal.unsafe_goal = kRange.lower + 0.05;
-  this->RunForTime(chrono::seconds(2), true);
+  this->SetEnabled(true);
 
-  this->VerifyNearGoal();
+  this->subsystem_plant_.InitializeSubsystemPosition(kRange.upper);
 
-  // Run with a null goal
-  this->RunForTime(chrono::seconds(2), true, true);
+  this->RunFor(chrono::seconds(5));
 
-  // Check that the subsystem has not moved
-  this->VerifyNearGoal();
+  EXPECT_NEAR(kRange.upper, this->subsystem_plant_.subsystem_position(), 0.001);
+  EXPECT_NEAR(this->subsystem_plant_.subsystem_velocity(), 0, 0.001);
 }
 
 // Tests that the subsystem estops when a zeroing error occurs
 TYPED_TEST_P(IntakeSystemTest, ZeroingErrorTest) {
-  this->RunForTime(chrono::seconds(2), true);
+  this->SetEnabled(true);
+  this->RunFor(chrono::seconds(2));
 
-  EXPECT_EQ(this->subsystem_.state(), TestFixture::SZSDPS::State::RUNNING);
-  this->subsystem_.TriggerEstimatorError();
-  this->RunIteration(true, false);
-  EXPECT_EQ(this->subsystem_.state(), TestFixture::SZSDPS::State::ESTOP);
+  EXPECT_EQ(this->subsystem()->state(), TestFixture::SZSDPS::State::RUNNING);
+  this->subsystem()->TriggerEstimatorError();
+  this->RunFor(this->dt());
+  EXPECT_EQ(this->subsystem()->state(), TestFixture::SZSDPS::State::ESTOP);
 }
 
 REGISTER_TYPED_TEST_CASE_P(IntakeSystemTest, DoesNothing, ReachesGoal,
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.q b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.q
new file mode 100644
index 0000000..3b35837
--- /dev/null
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.q
@@ -0,0 +1,46 @@
+package frc971.control_loops;
+
+import "frc971/control_loops/profiled_subsystem.q";
+
+message StaticZeroingSingleDOFProfiledSubsystemTestGoal {
+  double unsafe_goal;
+  .frc971.ProfileParameters profile_params;
+};
+
+message StaticZeroingSingleDOFProfiledSubsystemOutput {
+  double output;
+};
+
+queue_group StaticZeroingSingleDOFProfiledSubsystemPotAndAbsoluteEncoderTestQueueGroup {
+  implements aos.control_loops.ControlLoop;
+
+  message Status {
+    PotAndAbsoluteEncoderProfiledJointStatus status;
+  };
+
+  message Position {
+    PotAndAbsolutePosition position;
+  };
+
+  queue StaticZeroingSingleDOFProfiledSubsystemTestGoal goal;
+  queue StaticZeroingSingleDOFProfiledSubsystemOutput output;
+  queue Status status;
+  queue Position position;
+};
+
+queue_group StaticZeroingSingleDOFProfiledSubsystemAbsoluteEncoderTestQueueGroup {
+  implements aos.control_loops.ControlLoop;
+
+  message Status {
+    AbsoluteEncoderProfiledJointStatus status;
+  };
+
+  message Position {
+    AbsolutePosition position;
+  };
+
+  queue StaticZeroingSingleDOFProfiledSubsystemTestGoal goal;
+  queue StaticZeroingSingleDOFProfiledSubsystemOutput output;
+  queue Status status;
+  queue Position position;
+};
diff --git a/frc971/control_loops/zeroed_joint.h b/frc971/control_loops/zeroed_joint.h
index 75d0b56..41269eb 100644
--- a/frc971/control_loops/zeroed_joint.h
+++ b/frc971/control_loops/zeroed_joint.h
@@ -157,9 +157,9 @@
   // position is null if the position data is stale, output_enabled is true if
   // the output will actually go to the motors, and goal_angle and goal_velocity
   // are the goal position and velocities.
-  double Update(const ZeroedJoint<kNumZeroSensors>::PositionData *position,
-                bool output_enabled,
-                double goal_angle, double goal_velocity);
+  double Update(const ::aos::monotonic_clock::time_point monotonic_now,
+                const ZeroedJoint<kNumZeroSensors>::PositionData *position,
+                bool output_enabled, double goal_angle, double goal_velocity);
 
   // True if the code is zeroing.
   bool is_zeroing() const { return state_ == ZEROING; }
@@ -270,9 +270,9 @@
 // Updates the zeroed joint controller and state machine.
 template <int kNumZeroSensors>
 double ZeroedJoint<kNumZeroSensors>::Update(
+    const ::aos::monotonic_clock::time_point monotonic_now,
     const ZeroedJoint<kNumZeroSensors>::PositionData *position,
-    bool output_enabled,
-    double goal_angle, double goal_velocity) {
+    bool output_enabled, double goal_angle, double goal_velocity) {
   // Uninitialize the bot if too many cycles pass without an encoder.
   if (position == NULL) {
     LOG(WARNING, "no new pos given\n");
@@ -282,7 +282,7 @@
     output_enabled = false;
     LOG(WARNING, "err_count is %d so disabling\n", error_count_);
 
-    if (::aos::monotonic_clock::now() > kRezeroTime + last_good_time_) {
+    if (monotonic_now > kRezeroTime + last_good_time_) {
       LOG(WARNING, "err_count is %d (or 1st time) so forcing a re-zero\n",
           error_count_);
       state_ = UNINITIALIZED;
@@ -290,7 +290,7 @@
     }
   }
   if (position != NULL) {
-    last_good_time_ = ::aos::monotonic_clock::now();
+    last_good_time_ = monotonic_now;
     error_count_ = 0;
   }