diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 5f89be1..71c7278 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -1,30 +1,117 @@
 package(default_visibility = ["//visibility:public"])
 
-load("//aos/build:queues.bzl", "queue_library")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos:config.bzl", "aos_config")
 load("//tools:environments.bzl", "mcu_cpus")
-load("//tools/build_rules:select.bzl", "cpu_select", "compiler_select")
+load("//tools/build_rules:select.bzl", "compiler_select", "cpu_select")
 
-cc_binary(
-    name = "replay_drivetrain",
-    srcs = [
-        "replay_drivetrain.cc",
-    ],
-    deps = [
-        ":drivetrain_queue",
-        "//aos:init",
-        "//aos/controls:replay_control_loop",
-        "//frc971/queues:gyro",
-    ],
+flatbuffer_cc_library(
+    name = "drivetrain_goal_fbs",
+    srcs = ["drivetrain_goal.fbs"],
+    compatible_with = mcu_cpus,
+    gen_reflections = 1,
+    includes = ["//frc971/control_loops:control_loops_fbs_includes"],
 )
 
-queue_library(
-    name = "drivetrain_queue",
-    srcs = [
-        "drivetrain.q",
+flatbuffer_cc_library(
+    name = "drivetrain_output_fbs",
+    srcs = ["drivetrain_output.fbs"],
+    compatible_with = mcu_cpus,
+    gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
+    name = "drivetrain_position_fbs",
+    srcs = ["drivetrain_position.fbs"],
+    compatible_with = mcu_cpus,
+    gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
+    name = "drivetrain_status_fbs",
+    srcs = ["drivetrain_status.fbs"],
+    compatible_with = mcu_cpus,
+    gen_reflections = 1,
+    includes = ["//frc971/control_loops:control_loops_fbs_includes"],
+)
+
+genrule(
+    name = "drivetrain_goal_float_fbs_generated",
+    srcs = ["drivetrain_goal.fbs"],
+    outs = ["drivetrain_goal_float.fbs"],
+    cmd = "cat $(SRCS) | sed 's/double/float/g' > $(OUTS)",
+    compatible_with = mcu_cpus,
+)
+
+genrule(
+    name = "drivetrain_position_float_fbs_generated",
+    srcs = ["drivetrain_position.fbs"],
+    outs = ["drivetrain_position_float.fbs"],
+    cmd = "cat $(SRCS) | sed 's/double/float/g' > $(OUTS)",
+    compatible_with = mcu_cpus,
+)
+
+genrule(
+    name = "drivetrain_output_float_fbs_generated",
+    srcs = ["drivetrain_output.fbs"],
+    outs = ["drivetrain_output_float.fbs"],
+    cmd = "cat $(SRCS) | sed 's/double/float/g' > $(OUTS)",
+    compatible_with = mcu_cpus,
+)
+
+genrule(
+    name = "drivetrain_status_float_fbs_generated",
+    srcs = ["drivetrain_status.fbs"],
+    outs = ["drivetrain_status_float.fbs"],
+    cmd = "cat $(SRCS) | sed 's/double/float/g' > $(OUTS)",
+    compatible_with = mcu_cpus,
+)
+
+flatbuffer_cc_library(
+    name = "drivetrain_goal_float_fbs",
+    srcs = ["drivetrain_goal_float.fbs"],
+    compatible_with = mcu_cpus,
+    gen_reflections = 1,
+    includes = ["//frc971/control_loops:control_loops_fbs_includes"],
+)
+
+flatbuffer_cc_library(
+    name = "drivetrain_output_float_fbs",
+    srcs = ["drivetrain_output_float.fbs"],
+    compatible_with = mcu_cpus,
+    gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
+    name = "drivetrain_position_float_fbs",
+    srcs = ["drivetrain_position_float.fbs"],
+    compatible_with = mcu_cpus,
+    gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
+    name = "drivetrain_status_float_fbs",
+    srcs = ["drivetrain_status_float.fbs"],
+    compatible_with = mcu_cpus,
+    gen_reflections = 1,
+    includes = ["//frc971/control_loops:control_loops_fbs_includes"],
+)
+
+aos_config(
+    name = "config",
+    src = "drivetrain_config.json",
+    flatbuffers = [
+        ":drivetrain_goal_fbs",
+        ":drivetrain_output_fbs",
+        ":drivetrain_status_fbs",
+        ":drivetrain_position_fbs",
+        ":localizer_fbs",
+        "//frc971/queues:gyro",
+        "//frc971/wpilib:imu_fbs",
     ],
+    visibility = ["//visibility:public"],
     deps = [
-        "//aos/controls:control_loop_queues",
-        "//frc971/control_loops:queues",
+        "//aos/robot_state:config",
     ],
 )
 
@@ -49,7 +136,7 @@
         "//aos/util:math",
         "//frc971/control_loops:c2d",
         "//frc971/control_loops:runge_kutta",
-        "//third_party/eigen",
+        "@org_tuxfamily_eigen//:eigen",
     ],
 )
 
@@ -66,11 +153,10 @@
     ],
 )
 
-queue_library(
-    name = "localizer_queue",
-    srcs = [
-        "localizer.q",
-    ],
+flatbuffer_cc_library(
+    name = "localizer_fbs",
+    srcs = ["localizer.fbs"],
+    gen_reflections = 1,
 )
 
 cc_library(
@@ -102,11 +188,14 @@
     deps = [
         ":distance_spline",
         ":drivetrain_config",
-        ":drivetrain_queue",
+        ":drivetrain_goal_fbs",
+        ":drivetrain_output_fbs",
+        ":drivetrain_status_fbs",
         ":spline",
         ":trajectory",
         "//aos:init",
         "//aos/util:math",
+        "//frc971/control_loops:control_loops_fbs",
     ],
 )
 
@@ -120,14 +209,19 @@
     ],
     deps = [
         ":drivetrain_config",
-        ":drivetrain_queue",
+        ":drivetrain_goal_fbs",
+        ":drivetrain_output_fbs",
+        ":drivetrain_status_fbs",
         ":localizer",
         "//aos:math",
         "//aos/util:math",
         "//frc971/control_loops:c2d",
+        "//frc971/control_loops:control_loops_fbs",
         "//frc971/control_loops:dlqr",
         "//frc971/control_loops:pose",
-        "//third_party/eigen",
+        "//frc971/control_loops:profiled_subsystem_fbs",
+        "//y2019/control_loops/superstructure:superstructure_goal_fbs",
+        "@org_tuxfamily_eigen//:eigen",
     ],
 )
 
@@ -158,19 +252,20 @@
     ],
     deps = [
         ":drivetrain_config",
-        ":drivetrain_queue",
+        ":drivetrain_goal_fbs",
+        ":drivetrain_output_fbs",
+        ":drivetrain_status_fbs",
         ":gear",
         ":localizer",
         "//aos:math",
         "//aos/controls:control_loop",
         "//aos/controls:polytope",
-        "//aos/logging:matrix_logging",
-        "//aos/logging:queue_logging",
-        "//aos/robot_state",
+        "//aos/robot_state:robot_state_fbs",
         "//aos/util:log_interval",
         "//aos/util:trapezoid_profile",
         "//frc971:shifter_hall_effect",
         "//frc971/control_loops:coerce_goal",
+        "//frc971/control_loops:control_loops_fbs",
         "//frc971/control_loops:state_feedback_loop",
     ],
 )
@@ -185,15 +280,17 @@
     ],
     deps = [
         ":drivetrain_config",
-        ":drivetrain_queue",
+        ":drivetrain_goal_fbs",
+        ":drivetrain_output_fbs",
+        ":drivetrain_position_fbs",
+        ":drivetrain_status_fbs",
         ":gear",
         "//aos:math",
         "//aos/controls:polytope",
-        "//aos/logging:matrix_logging",
-        "//aos/logging:queue_logging",
-        "//aos/robot_state",
+        "//aos/robot_state:robot_state_fbs",
         "//aos/util:log_interval",
         "//frc971/control_loops:coerce_goal",
+        "//frc971/control_loops:control_loops_fbs",
         "//frc971/control_loops:state_feedback_loop",
     ],
 )
@@ -213,20 +310,24 @@
 cc_library(
     name = "polydrivetrain_uc",
     srcs = [
-        "drivetrain_uc.q.cc",
         "polydrivetrain.cc",
     ],
     hdrs = [
-        "drivetrain_uc.q.h",
         "polydrivetrain.h",
     ],
+    copts = ["-Wno-type-limits"],
     restricted_to = mcu_cpus,
     deps = [
         ":drivetrain_config_uc",
+        ":drivetrain_goal_float_fbs",
+        ":drivetrain_output_float_fbs",
+        ":drivetrain_position_float_fbs",
+        ":drivetrain_status_float_fbs",
         ":gear",
         "//aos:math",
         "//aos/controls:polytope_uc",
         "//frc971/control_loops:coerce_goal_uc",
+        "//frc971/control_loops:control_loops_fbs",
         "//frc971/control_loops:state_feedback_loop_uc",
     ],
 )
@@ -267,21 +368,22 @@
     ],
     deps = [
         ":down_estimator",
-        ":drivetrain_queue",
+        ":drivetrain_goal_fbs",
+        ":drivetrain_output_fbs",
+        ":drivetrain_position_fbs",
+        ":drivetrain_status_fbs",
         ":gear",
         ":line_follow_drivetrain",
         ":localizer",
-        ":localizer_queue",
+        ":localizer_fbs",
         ":polydrivetrain",
         ":splinedrivetrain",
         ":ssdrivetrain",
         "//aos/controls:control_loop",
-        "//aos/logging:matrix_logging",
-        "//aos/logging:queue_logging",
         "//aos/util:log_interval",
         "//frc971/control_loops:runge_kutta",
         "//frc971/queues:gyro",
-        "//frc971/wpilib:imu_queue",
+        "//frc971/wpilib:imu_fbs",
     ],
 )
 
@@ -292,10 +394,14 @@
     hdrs = ["drivetrain_test_lib.h"],
     deps = [
         ":drivetrain_config",
-        ":drivetrain_queue",
+        ":drivetrain_goal_fbs",
+        ":drivetrain_output_fbs",
+        ":drivetrain_position_fbs",
+        ":drivetrain_status_fbs",
         ":trajectory",
-        "//aos/events:event-loop",
+        "//aos/events:event_loop",
         "//aos/testing:googletest",
+        "//frc971/control_loops:control_loops_fbs",
         "//frc971/control_loops:state_feedback_loop",
         "//frc971/queues:gyro",
         "//y2016:constants",
@@ -308,6 +414,7 @@
     srcs = [
         "drivetrain_lib_test.cc",
     ],
+    data = ["config.json"],
     defines =
         cpu_select({
             "amd64": [
@@ -319,10 +426,12 @@
     deps = [
         ":drivetrain_config",
         ":drivetrain_lib",
-        ":localizer_queue",
-        ":drivetrain_queue",
+        ":localizer_fbs",
+        ":drivetrain_goal_fbs",
+        ":drivetrain_status_fbs",
+        ":drivetrain_position_fbs",
+        ":drivetrain_output_fbs",
         ":drivetrain_test_lib",
-        "//aos:queues",
         "//aos/controls:control_loop_test",
         "//aos/testing:googletest",
         "//frc971/queues:gyro",
@@ -399,7 +508,7 @@
     hdrs = ["spline.h"],
     deps = [
         "//frc971/control_loops:binomial",
-        "//third_party/eigen",
+        "@org_tuxfamily_eigen//:eigen",
     ],
 )
 
@@ -413,8 +522,8 @@
         ":trajectory",
         "//aos/logging:implementations",
         "//aos/network:team_number",
-        "//third_party/eigen",
         "//y2019/control_loops/drivetrain:drivetrain_base",
+        "@org_tuxfamily_eigen//:eigen",
     ],
 )
 
@@ -440,7 +549,7 @@
         ":spline",
         "//aos/logging",
         "//frc971/control_loops:fixed_quadrature",
-        "//third_party/eigen",
+        "@org_tuxfamily_eigen//:eigen",
     ],
 )
 
@@ -477,13 +586,12 @@
     deps = [
         ":distance_spline",
         ":drivetrain_config",
-        "//aos/logging:matrix_logging",
         "//frc971/control_loops:c2d",
         "//frc971/control_loops:dlqr",
         "//frc971/control_loops:hybrid_state_feedback_loop",
         "//frc971/control_loops:runge_kutta",
         "//frc971/control_loops:state_feedback_loop",
-        "//third_party/eigen",
+        "@org_tuxfamily_eigen//:eigen",
     ],
 )
 
@@ -497,12 +605,11 @@
         ":distance_spline",
         ":trajectory",
         "//aos/logging:implementations",
-        "//aos/logging:matrix_logging",
         "//aos/network:team_number",
-        "//third_party/eigen",
         "//third_party/matplotlib-cpp",
         "//y2019/control_loops/drivetrain:drivetrain_base",
         "@com_github_gflags_gflags//:gflags",
+        "@org_tuxfamily_eigen//:eigen",
     ],
 )
 
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 4685ed6..78cb378 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -7,18 +7,18 @@
 #include "Eigen/Dense"
 
 #include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
-#include "aos/logging/matrix_logging.h"
-
 #include "frc971/control_loops/drivetrain/down_estimator.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
 #include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
 #include "frc971/control_loops/drivetrain/polydrivetrain.h"
 #include "frc971/control_loops/drivetrain/ssdrivetrain.h"
 #include "frc971/control_loops/runge_kutta.h"
-#include "frc971/queues/gyro.q.h"
+#include "frc971/queues/gyro_generated.h"
 #include "frc971/shifter_hall_effect.h"
-#include "frc971/wpilib/imu.q.h"
+#include "frc971/wpilib/imu_generated.h"
 
 using ::aos::monotonic_clock;
 namespace chrono = ::std::chrono;
@@ -31,16 +31,16 @@
                                ::aos::EventLoop *event_loop,
                                LocalizerInterface *localizer,
                                const ::std::string &name)
-    : aos::controls::ControlLoop<::frc971::control_loops::DrivetrainQueue>(
-          event_loop, name),
+    : aos::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
+                                                                 name),
       dt_config_(dt_config),
-      localizer_control_fetcher_(event_loop->MakeFetcher<LocalizerControl>(
-          ".frc971.control_loops.drivetrain.localizer_control")),
+      localizer_control_fetcher_(
+          event_loop->MakeFetcher<LocalizerControl>("/drivetrain")),
       imu_values_fetcher_(
-          event_loop->MakeFetcher<::frc971::IMUValues>(".frc971.imu_values")),
+          event_loop->MakeFetcher<::frc971::IMUValues>("/drivetrain")),
       gyro_reading_fetcher_(
           event_loop->MakeFetcher<::frc971::sensors::GyroReading>(
-              ".frc971.sensors.gyro_reading")),
+              "/drivetrain")),
       localizer_(localizer),
       kf_(dt_config_.make_kf_drivetrain_loop()),
       dt_openloop_(dt_config_, &kf_),
@@ -89,10 +89,9 @@
 }
 
 void DrivetrainLoop::RunIteration(
-    const ::frc971::control_loops::DrivetrainQueue::Goal *goal,
-    const ::frc971::control_loops::DrivetrainQueue::Position *position,
-    ::frc971::control_loops::DrivetrainQueue::Output *output,
-    ::frc971::control_loops::DrivetrainQueue::Status *status) {
+    const drivetrain::Goal *goal, const drivetrain::Position *position,
+    aos::Sender<drivetrain::Output>::Builder *output,
+    aos::Sender<drivetrain::Status>::Builder *status) {
   const monotonic_clock::time_point monotonic_now =
       event_loop()->monotonic_now();
 
@@ -118,9 +117,9 @@
       }
       break;
     case ShifterType::HALL_EFFECT_SHIFTER:
-      left_gear_ = ComputeGear(position->left_shifter_position,
+      left_gear_ = ComputeGear(position->left_shifter_position(),
                                dt_config_.left_drive, left_high_requested_);
-      right_gear_ = ComputeGear(position->right_shifter_position,
+      right_gear_ = ComputeGear(position->right_shifter_position(),
                                 dt_config_.right_drive, right_high_requested_);
       break;
     case ShifterType::NO_SHIFTER:
@@ -129,35 +128,39 @@
 
   kf_.set_index(ControllerIndexFromGears());
 
+  flatbuffers::Offset<GearLogging> gear_logging_offset;
   // Set the gear-logging parts of the status
   if (status) {
-    status->gear_logging.left_state = static_cast<uint32_t>(left_gear_);
-    status->gear_logging.right_state = static_cast<uint32_t>(right_gear_);
-    status->gear_logging.left_loop_high = MaybeHigh(left_gear_);
-    status->gear_logging.right_loop_high = MaybeHigh(right_gear_);
-    status->gear_logging.controller_index = kf_.index();
+    GearLogging::Builder gear_logging_builder =
+        status->MakeBuilder<GearLogging>();
+    gear_logging_builder.add_left_state(static_cast<uint32_t>(left_gear_));
+    gear_logging_builder.add_right_state(static_cast<uint32_t>(right_gear_));
+    gear_logging_builder.add_left_loop_high(MaybeHigh(left_gear_));
+    gear_logging_builder.add_right_loop_high(MaybeHigh(right_gear_));
+    gear_logging_builder.add_controller_index(kf_.index());
+    gear_logging_offset = gear_logging_builder.Finish();
   }
 
   const bool is_latest_imu_values = imu_values_fetcher_.Fetch();
   if (is_latest_imu_values) {
-    const double rate = -imu_values_fetcher_->gyro_y;
+    const double rate = -imu_values_fetcher_->gyro_y();
     const double accel_squared =
-        ::std::pow(imu_values_fetcher_->accelerometer_x, 2.0) +
-        ::std::pow(imu_values_fetcher_->accelerometer_y, 2.0) +
-        ::std::pow(imu_values_fetcher_->accelerometer_z, 2.0);
-    const double angle = ::std::atan2(imu_values_fetcher_->accelerometer_x,
-                                      imu_values_fetcher_->accelerometer_z) +
+        ::std::pow(imu_values_fetcher_->accelerometer_x(), 2.0) +
+        ::std::pow(imu_values_fetcher_->accelerometer_y(), 2.0) +
+        ::std::pow(imu_values_fetcher_->accelerometer_z(), 2.0);
+    const double angle = ::std::atan2(imu_values_fetcher_->accelerometer_x(),
+                                      imu_values_fetcher_->accelerometer_z()) +
                          0.008;
 
     switch (dt_config_.imu_type) {
       case IMUType::IMU_X:
-        last_accel_ = -imu_values_fetcher_->accelerometer_x;
+        last_accel_ = -imu_values_fetcher_->accelerometer_x();
         break;
       case IMUType::IMU_FLIPPED_X:
-        last_accel_ = imu_values_fetcher_->accelerometer_x;
+        last_accel_ = imu_values_fetcher_->accelerometer_x();
         break;
       case IMUType::IMU_Y:
-        last_accel_ = -imu_values_fetcher_->accelerometer_y;
+        last_accel_ = -imu_values_fetcher_->accelerometer_y();
         break;
     }
 
@@ -185,43 +188,37 @@
   switch (dt_config_.gyro_type) {
     case GyroType::IMU_X_GYRO:
       if (is_latest_imu_values) {
-        AOS_LOG_STRUCT(DEBUG, "using", *imu_values_fetcher_.get());
-        last_gyro_rate_ = imu_values_fetcher_->gyro_x;
+        last_gyro_rate_ = imu_values_fetcher_->gyro_x();
         last_gyro_time_ = monotonic_now;
       }
       break;
     case GyroType::IMU_Y_GYRO:
       if (is_latest_imu_values) {
-        AOS_LOG_STRUCT(DEBUG, "using", *imu_values_fetcher_.get());
-        last_gyro_rate_ = imu_values_fetcher_->gyro_y;
+        last_gyro_rate_ = imu_values_fetcher_->gyro_y();
         last_gyro_time_ = monotonic_now;
       }
       break;
     case GyroType::IMU_Z_GYRO:
       if (is_latest_imu_values) {
-        AOS_LOG_STRUCT(DEBUG, "using", *imu_values_fetcher_.get());
-        last_gyro_rate_ = imu_values_fetcher_->gyro_z;
+        last_gyro_rate_ = imu_values_fetcher_->gyro_z();
         last_gyro_time_ = monotonic_now;
       }
       break;
     case GyroType::FLIPPED_IMU_Z_GYRO:
       if (is_latest_imu_values) {
-        AOS_LOG_STRUCT(DEBUG, "using", *imu_values_fetcher_.get());
-        last_gyro_rate_ = -imu_values_fetcher_->gyro_z;
+        last_gyro_rate_ = -imu_values_fetcher_->gyro_z();
         last_gyro_time_ = monotonic_now;
       }
       break;
     case GyroType::SPARTAN_GYRO:
       if (gyro_reading_fetcher_.Fetch()) {
-        AOS_LOG_STRUCT(DEBUG, "using", *gyro_reading_fetcher_.get());
-        last_gyro_rate_ = gyro_reading_fetcher_->velocity;
+        last_gyro_rate_ = gyro_reading_fetcher_->velocity();
         last_gyro_time_ = monotonic_now;
       }
       break;
     case GyroType::FLIPPED_SPARTAN_GYRO:
       if (gyro_reading_fetcher_.Fetch()) {
-        AOS_LOG_STRUCT(DEBUG, "using", *gyro_reading_fetcher_.get());
-        last_gyro_rate_ = -gyro_reading_fetcher_->velocity;
+        last_gyro_rate_ = -gyro_reading_fetcher_->velocity();
         last_gyro_time_ = monotonic_now;
       }
       break;
@@ -236,7 +233,7 @@
 
   {
     Eigen::Matrix<double, 4, 1> Y;
-    Y << position->left_encoder, position->right_encoder, last_gyro_rate_,
+    Y << position->left_encoder(), position->right_encoder(), last_gyro_rate_,
         last_accel_;
     kf_.Correct(Y);
     // If we get a new message setting the absolute position, then reset the
@@ -244,33 +241,36 @@
     // TODO(james): Use a watcher (instead of a fetcher) once we support it in
     // simulation.
     if (localizer_control_fetcher_.Fetch()) {
-      AOS_LOG_STRUCT(DEBUG, "localizer_control", *localizer_control_fetcher_);
+      VLOG(1) << "localizer_control "
+              << aos::FlatbufferToJson(localizer_control_fetcher_.get());
       localizer_->ResetPosition(
-          monotonic_now, localizer_control_fetcher_->x,
-          localizer_control_fetcher_->y, localizer_control_fetcher_->theta,
-          localizer_control_fetcher_->theta_uncertainty,
-          !localizer_control_fetcher_->keep_current_theta);
+          monotonic_now, localizer_control_fetcher_->x(),
+          localizer_control_fetcher_->y(), localizer_control_fetcher_->theta(),
+          localizer_control_fetcher_->theta_uncertainty(),
+          !localizer_control_fetcher_->keep_current_theta());
     }
     localizer_->Update({last_last_left_voltage_, last_last_right_voltage_},
-                       monotonic_now, position->left_encoder,
-                       position->right_encoder, last_gyro_rate_, last_accel_);
+                       monotonic_now, position->left_encoder(),
+                       position->right_encoder(), last_gyro_rate_, last_accel_);
   }
 
   dt_openloop_.SetPosition(position, left_gear_, right_gear_);
 
-  int controller_type = 0;
+  ControllerType controller_type = ControllerType_POLYDRIVE;
   if (goal) {
-    controller_type = goal->controller_type;
+    controller_type = goal->controller_type();
 
-    dt_closedloop_.SetGoal(*goal);
-    dt_openloop_.SetGoal(*goal);
-    dt_spline_.SetGoal(*goal);
-    dt_line_follow_.SetGoal(monotonic_now, *goal);
+    dt_closedloop_.SetGoal(goal);
+    dt_openloop_.SetGoal(goal->wheel(), goal->throttle(), goal->quickturn(),
+                         goal->highgear());
+    dt_spline_.SetGoal(goal);
+    dt_line_follow_.SetGoal(monotonic_now, goal);
   }
 
-  dt_openloop_.Update(robot_state().voltage_battery);
+  dt_openloop_.Update(robot_state().voltage_battery());
 
-  dt_closedloop_.Update(output != NULL && controller_type == 1);
+  dt_closedloop_.Update(output != nullptr &&
+                        controller_type == ControllerType_MOTION_PROFILE);
 
   const Eigen::Matrix<double, 5, 1> trajectory_state =
       (Eigen::Matrix<double, 5, 1>() << localizer_->x(), localizer_->y(),
@@ -278,25 +278,30 @@
        localizer_->right_velocity())
           .finished();
 
-  dt_spline_.Update(output != NULL && controller_type == 2, trajectory_state);
+  dt_spline_.Update(
+      output != nullptr && controller_type == ControllerType_SPLINE_FOLLOWER,
+      trajectory_state);
 
   dt_line_follow_.Update(monotonic_now, trajectory_state);
 
+  OutputT output_struct;
+
   switch (controller_type) {
-    case 0:
-      dt_openloop_.SetOutput(output);
+    case ControllerType_POLYDRIVE:
+      dt_openloop_.SetOutput(output != nullptr ? &output_struct : nullptr);
       break;
-    case 1:
-      dt_closedloop_.SetOutput(output);
+    case ControllerType_MOTION_PROFILE:
+      dt_closedloop_.SetOutput(output != nullptr ? &output_struct : nullptr);
       break;
-    case 2:
-      dt_spline_.SetOutput(output);
+    case ControllerType_SPLINE_FOLLOWER:
+      dt_spline_.SetOutput(output != nullptr ? &output_struct : nullptr);
       break;
-    case 3:
-      if (!dt_line_follow_.SetOutput(output)) {
+    case ControllerType_LINE_FOLLOWER:
+      if (!dt_line_follow_.SetOutput(output != nullptr ? &output_struct
+                                                       : nullptr)) {
         // If the line follow drivetrain was unable to execute (generally due to
         // not having a target), execute the regular teleop drivetrain.
-        dt_openloop_.SetOutput(output);
+        dt_openloop_.SetOutput(output != nullptr ? &output_struct : nullptr);
       }
       break;
   }
@@ -305,8 +310,6 @@
 
   // set the output status of the control loop state
   if (status) {
-    status->robot_speed = (kf_.X_hat(1) + kf_.X_hat(3)) / 2.0;
-
     Eigen::Matrix<double, 2, 1> linear =
         dt_config_.LeftRightToLinear(kf_.X_hat());
     Eigen::Matrix<double, 2, 1> angular =
@@ -317,42 +320,60 @@
     Eigen::Matrix<double, 4, 1> gyro_left_right =
         dt_config_.AngularLinearToLeftRight(linear, angular);
 
-    status->estimated_left_position = gyro_left_right(0, 0);
-    status->estimated_right_position = gyro_left_right(2, 0);
+    const flatbuffers::Offset<CIMLogging> cim_logging_offset =
+        dt_openloop_.PopulateStatus(status->fbb());
 
-    status->estimated_left_velocity = gyro_left_right(1, 0);
-    status->estimated_right_velocity = gyro_left_right(3, 0);
-    status->output_was_capped = dt_closedloop_.output_was_capped();
-    status->uncapped_left_voltage = kf_.U_uncapped(0, 0);
-    status->uncapped_right_voltage = kf_.U_uncapped(1, 0);
+    flatbuffers::Offset<LineFollowLogging> line_follow_logging_offset =
+        dt_line_follow_.PopulateStatus(status);
+    flatbuffers::Offset<TrajectoryLogging> trajectory_logging_offset =
+        dt_spline_.MakeTrajectoryLogging(status);
 
-    status->left_voltage_error = kf_.X_hat(4);
-    status->right_voltage_error = kf_.X_hat(5);
-    status->estimated_angular_velocity_error = kf_.X_hat(6);
-    status->estimated_heading = localizer_->theta();
+    StatusBuilder builder = status->MakeBuilder<Status>();
 
-    status->x = localizer_->x();
-    status->y = localizer_->y();
-    status->theta = ::aos::math::NormalizeAngle(localizer_->theta());
+    dt_closedloop_.PopulateStatus(&builder);
 
-    status->ground_angle = down_estimator_.X_hat(0) + dt_config_.down_offset;
+    builder.add_estimated_left_position(gyro_left_right(0, 0));
+    builder.add_estimated_right_position(gyro_left_right(2, 0));
 
-    dt_openloop_.PopulateStatus(status);
-    dt_closedloop_.PopulateStatus(status);
-    dt_spline_.PopulateStatus(status);
-    dt_line_follow_.PopulateStatus(status);
+    builder.add_estimated_left_velocity(gyro_left_right(1, 0));
+    builder.add_estimated_right_velocity(gyro_left_right(3, 0));
+
+    if (dt_spline_.enable()) {
+      dt_spline_.PopulateStatus(&builder);
+    } else {
+      builder.add_robot_speed((kf_.X_hat(1) + kf_.X_hat(3)) / 2.0);
+      builder.add_output_was_capped(dt_closedloop_.output_was_capped());
+      builder.add_uncapped_left_voltage(kf_.U_uncapped(0, 0));
+      builder.add_uncapped_right_voltage(kf_.U_uncapped(1, 0));
+    }
+
+    builder.add_left_voltage_error(kf_.X_hat(4));
+    builder.add_right_voltage_error(kf_.X_hat(5));
+    builder.add_estimated_angular_velocity_error(kf_.X_hat(6));
+    builder.add_estimated_heading(localizer_->theta());
+
+    builder.add_x(localizer_->x());
+    builder.add_y(localizer_->y());
+    builder.add_theta(::aos::math::NormalizeAngle(localizer_->theta()));
+
+    builder.add_ground_angle(down_estimator_.X_hat(0) + dt_config_.down_offset);
+    builder.add_cim_logging(cim_logging_offset);
+    builder.add_gear_logging(gear_logging_offset);
+    builder.add_line_follow_logging(line_follow_logging_offset);
+    builder.add_trajectory_logging(trajectory_logging_offset);
+    status->Send(builder.Finish());
   }
 
   double left_voltage = 0.0;
   double right_voltage = 0.0;
   if (output) {
-    left_voltage = output->left_voltage;
-    right_voltage = output->right_voltage;
-    left_high_requested_ = output->left_high;
-    right_high_requested_ = output->right_high;
+    left_voltage = output_struct.left_voltage;
+    right_voltage = output_struct.right_voltage;
+    left_high_requested_ = output_struct.left_high;
+    right_high_requested_ = output_struct.right_high;
   }
 
-  const double scalar = robot_state().voltage_battery / 12.0;
+  const double scalar = robot_state().voltage_battery() / 12.0;
 
   left_voltage *= scalar;
   right_voltage *= scalar;
@@ -375,14 +396,20 @@
 
   last_state_ = kf_.X_hat();
   kf_.UpdateObserver(U, dt_config_.dt);
+
+  if (output) {
+    output->Send(Output::Pack(*output->fbb(), &output_struct));
+  }
 }
 
-void DrivetrainLoop::Zero(
-    ::frc971::control_loops::DrivetrainQueue::Output *output) {
-  output->left_voltage = 0;
-  output->right_voltage = 0;
-  output->left_high = dt_config_.default_high_gear;
-  output->right_high = dt_config_.default_high_gear;
+flatbuffers::Offset<Output> DrivetrainLoop::Zero(
+    aos::Sender<Output>::Builder *output) {
+  Output::Builder builder = output->MakeBuilder<Output>();
+  builder.add_left_voltage(0);
+  builder.add_right_voltage(0);
+  builder.add_left_high(dt_config_.default_high_gear);
+  builder.add_right_high(dt_config_.default_high_gear);
+  return builder.Finish();
 }
 
 }  // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index 33ff770..9f6a34c 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -6,43 +6,49 @@
 #include "aos/controls/control_loop.h"
 #include "aos/controls/polytope.h"
 #include "aos/util/log_interval.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
 #include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
 #include "frc971/control_loops/drivetrain/gear.h"
 #include "frc971/control_loops/drivetrain/line_follow_drivetrain.h"
 #include "frc971/control_loops/drivetrain/localizer.h"
-#include "frc971/control_loops/drivetrain/localizer.q.h"
+#include "frc971/control_loops/drivetrain/localizer_generated.h"
 #include "frc971/control_loops/drivetrain/polydrivetrain.h"
 #include "frc971/control_loops/drivetrain/splinedrivetrain.h"
 #include "frc971/control_loops/drivetrain/ssdrivetrain.h"
-#include "frc971/queues/gyro.q.h"
-#include "frc971/wpilib/imu.q.h"
+#include "frc971/queues/gyro_generated.h"
+#include "frc971/wpilib/imu_generated.h"
 
 namespace frc971 {
 namespace control_loops {
 namespace drivetrain {
 
-class DrivetrainLoop : public aos::controls::ControlLoop<
-                           ::frc971::control_loops::DrivetrainQueue> {
+class DrivetrainLoop
+    : public aos::controls::ControlLoop<Goal, Position, Status, Output> {
  public:
   // Constructs a control loop which can take a Drivetrain or defaults to the
   // drivetrain at frc971::control_loops::drivetrain
-  explicit DrivetrainLoop(
-      const DrivetrainConfig<double> &dt_config, ::aos::EventLoop *event_loop,
-      LocalizerInterface *localizer,
-      const ::std::string &name = ".frc971.control_loops.drivetrain_queue");
+  explicit DrivetrainLoop(const DrivetrainConfig<double> &dt_config,
+                          ::aos::EventLoop *event_loop,
+                          LocalizerInterface *localizer,
+                          const ::std::string &name = "/drivetrain");
 
   int ControllerIndexFromGears();
 
  protected:
   // Executes one cycle of the control loop.
   void RunIteration(
-      const ::frc971::control_loops::DrivetrainQueue::Goal *goal,
-      const ::frc971::control_loops::DrivetrainQueue::Position *position,
-      ::frc971::control_loops::DrivetrainQueue::Output *output,
-      ::frc971::control_loops::DrivetrainQueue::Status *status) override;
+      const ::frc971::control_loops::drivetrain::Goal *goal,
+      const ::frc971::control_loops::drivetrain::Position *position,
+      aos::Sender<::frc971::control_loops::drivetrain::Output>::Builder *output,
+      aos::Sender<::frc971::control_loops::drivetrain::Status>::Builder *status)
+      override;
 
-  void Zero(::frc971::control_loops::DrivetrainQueue::Output *output) override;
+  flatbuffers::Offset<drivetrain::Output> Zero(
+      aos::Sender<drivetrain::Output>::Builder *builder) override;
 
   double last_gyro_rate_ = 0.0;
 
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
deleted file mode 100644
index c7a307e..0000000
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ /dev/null
@@ -1,224 +0,0 @@
-package frc971.control_loops;
-
-import "aos/controls/control_loops.q";
-import "frc971/control_loops/control_loops.q";
-
-// For logging information about what the code is doing with the shifters.
-struct GearLogging {
-  // Which controller is being used.
-  int8_t controller_index;
-
-  // Whether each loop for the drivetrain sides is the high-gear one.
-  bool left_loop_high;
-  bool right_loop_high;
-
-  // The states of each drivetrain shifter.
-  int8_t left_state;
-  int8_t right_state;
-};
-
-// For logging information about the state of the shifters.
-struct CIMLogging {
-  // Whether the code thinks each drivetrain side is currently in gear.
-  bool left_in_gear;
-  bool right_in_gear;
-
-  // The angular velocities (in rad/s, positive forward) the code thinks motors
-  // on each side of the drivetrain are moving at.
-  double left_motor_speed;
-  double right_motor_speed;
-
-  // The velocity estimates for each drivetrain side of the robot (in m/s,
-  // positive forward) that can be used for shifting.
-  double left_velocity;
-  double right_velocity;
-};
-
-// For logging information about the state of the trajectory planning.
-struct TrajectoryLogging {
-  // state of planning the trajectory.
-  //  0: not currently planning
-  //  1: received a multispline to plan
-  //  2: Built the spline and planning.
-  //  3: Finished the plan and ready to excecute.
-  int8_t planning_state;
-
-  // State of the spline execution.
-  bool is_executing;
-  // Whether we have finished the spline specified by current_spline_idx.
-  bool is_executed;
-
-  // The handle of the goal spline.  0 means stop requested.
-  int32_t goal_spline_handle;
-  // Handle of the executing spline.  -1 means none requested.  If there was no
-  // spline executing when a spline finished optimizing, it will become the
-  // current spline even if we aren't ready to start yet.
-  int32_t current_spline_idx;
-  // Handle of the spline that is being optimized and staged.
-  int32_t planning_spline_idx;
-
-  // Expected position and velocity on the spline
-  float x;
-  float y;
-  float theta;
-  float left_velocity;
-  float right_velocity;
-  float distance_remaining;
-};
-
-// For logging state of the line follower.
-struct LineFollowLogging {
-  // Whether we are currently freezing target choice.
-  bool frozen;
-  // Whether we currently have a target.
-  bool have_target;
-  // Absolute position of the current goal.
-  float x;
-  float y;
-  float theta;
-  // Current lateral offset from line pointing straight out of the target.
-  float offset;
-  // Current distance from the plane of the target, in meters.
-  float distance_to_target;
-  // Current goal heading.
-  float goal_theta;
-  // Current relative heading.
-  float rel_theta;
-};
-
-// Published on ".frc971.control_loops.drivetrain_queue"
-queue_group DrivetrainQueue {
-  implements aos.control_loops.ControlLoop;
-
-  message Goal {
-    // Position of the steering wheel (positive = turning left when going
-    // forwards).
-    float wheel;
-    float wheel_velocity;
-    float wheel_torque;
-
-    // Position of the throttle (positive forwards).
-    float throttle;
-    float throttle_velocity;
-    float throttle_torque;
-
-    // True to shift into high, false to shift into low.
-    bool highgear;
-
-    // True to activate quickturn.
-    bool quickturn;
-
-    // Type of controller in charge of the drivetrain.
-    //  0: polydrive
-    //  1: motion profiled position drive (statespace)
-    //  2: spline follower
-    //  3: line follower (for guiding into a target)
-    uint8_t controller_type;
-
-    // Position goals for each drivetrain side (in meters) when the
-    // closed-loop controller is active.
-    double left_goal;
-    double right_goal;
-
-    float max_ss_voltage;
-
-    // Motion profile parameters.
-    // The control loop will profile if these are all non-zero.
-    .frc971.ProfileParameters linear;
-    .frc971.ProfileParameters angular;
-
-    // Parameters for a spline to follow. This just contains info on a spline to
-    // compute. Each time this is sent, spline drivetrain will compute a new
-    // spline.
-    .frc971.MultiSpline spline;
-
-    // Which spline to follow.
-    int32_t spline_handle;
-  };
-
-  message Position {
-    // Relative position of each drivetrain side (in meters).
-    double left_encoder;
-    double right_encoder;
-
-    // The speed in m/s of each drivetrain side from the most recent encoder
-    // pulse, or 0 if there was no edge within the last 5ms.
-    double left_speed;
-    double right_speed;
-
-    // Position of each drivetrain shifter, scaled from 0.0 to 1.0 where smaller
-    // is towards low gear.
-    double left_shifter_position;
-    double right_shifter_position;
-
-    // Raw analog voltages of each shifter hall effect for logging purposes.
-    double low_left_hall;
-    double high_left_hall;
-    double low_right_hall;
-    double high_right_hall;
-  };
-
-  message Output {
-    // Voltage to send to motor(s) on either side of the drivetrain.
-    double left_voltage;
-    double right_voltage;
-
-    // Whether to set each shifter piston to high gear.
-    bool left_high;
-    bool right_high;
-  };
-
-  message Status {
-    // Estimated speed of the center of the robot in m/s (positive forwards).
-    double robot_speed;
-
-    // Estimated relative position of each drivetrain side (in meters).
-    double estimated_left_position;
-    double estimated_right_position;
-
-    // Estimated velocity of each drivetrain side (in m/s).
-    double estimated_left_velocity;
-    double estimated_right_velocity;
-
-    // The voltage we wanted to send to each drivetrain side last cycle.
-    double uncapped_left_voltage;
-    double uncapped_right_voltage;
-
-    // The voltage error for the left and right sides.
-    double left_voltage_error;
-    double right_voltage_error;
-
-    // The profiled goal states.
-    double profiled_left_position_goal;
-    double profiled_right_position_goal;
-    double profiled_left_velocity_goal;
-    double profiled_right_velocity_goal;
-
-    // The KF offset
-    double estimated_angular_velocity_error;
-    // The KF estimated heading.
-    double estimated_heading;
-
-    // xytheta of the robot.
-    double x;
-    double y;
-    double theta;
-
-    // True if the output voltage was capped last cycle.
-    bool output_was_capped;
-
-    // The angle of the robot relative to the ground.
-    double ground_angle;
-
-    // Information about shifting logic and curent gear, for logging purposes
-    GearLogging gear_logging;
-    CIMLogging cim_logging;
-    TrajectoryLogging trajectory_logging;
-    LineFollowLogging line_follow_logging;
-  };
-
-  queue Goal goal;
-  queue Position position;
-  queue Output output;
-  queue Status status;
-};
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.json b/frc971/control_loops/drivetrain/drivetrain_config.json
new file mode 100644
index 0000000..dc828bb
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain_config.json
@@ -0,0 +1,43 @@
+{
+  "channels":
+  [
+    {
+      "name": "/drivetrain",
+      "type": "frc971.IMUValues",
+      "frequency": 200
+    },
+    {
+      "name": "/drivetrain",
+      "type": "frc971.sensors.GyroReading",
+      "frequency": 200
+    },
+    {
+      "name": "/drivetrain",
+      "type": "frc971.control_loops.drivetrain.Goal",
+      "frequency": 200
+    },
+    {
+      "name": "/drivetrain",
+      "type": "frc971.control_loops.drivetrain.Position",
+      "frequency": 200
+    },
+    {
+      "name": "/drivetrain",
+      "type": "frc971.control_loops.drivetrain.Status",
+      "frequency": 200
+    },
+    {
+      "name": "/drivetrain",
+      "type": "frc971.control_loops.drivetrain.Output",
+      "frequency": 200
+    },
+    {
+      "name": "/drivetrain",
+      "type": "frc971.control_loops.drivetrain.LocalizerControl",
+      "frequency": 200
+    }
+  ],
+  "imports": [
+    "../../../aos/robot_state/robot_state_config.json"
+  ]
+}
diff --git a/frc971/control_loops/drivetrain/drivetrain_goal.fbs b/frc971/control_loops/drivetrain/drivetrain_goal.fbs
new file mode 100644
index 0000000..8a67849
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain_goal.fbs
@@ -0,0 +1,65 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace frc971.control_loops.drivetrain;
+
+enum ControllerType : byte {
+  POLYDRIVE,
+  MOTION_PROFILE,
+  SPLINE_FOLLOWER,
+  LINE_FOLLOWER,
+}
+
+table SplineGoal {
+  // index of the spline. Zero indicates the spline should not be computed.
+  spline_idx:int;
+
+  // Acutal spline.
+  spline:frc971.MultiSpline;
+
+  // Whether to follow the spline driving forwards or backwards.
+  drive_spline_backwards:bool;
+}
+
+table Goal {
+  // Position of the steering wheel (positive = turning left when going
+  // forwards).
+  wheel:float;
+  wheel_velocity:float;
+  wheel_torque:float;
+
+  // Position of the throttle (positive forwards).
+  throttle:float;
+  throttle_velocity:float;
+  throttle_torque:float;
+
+  // True to shift into high, false to shift into low.
+  highgear:bool;
+
+  // True to activate quickturn.
+  quickturn:bool;
+
+  // Type of controller in charge of the drivetrain.
+  controller_type:ControllerType;
+
+  // Position goals for each drivetrain side (in meters) when the
+  // closed-loop controller is active.
+  left_goal:double;
+  right_goal:double;
+
+  max_ss_voltage:float;
+
+  // Motion profile parameters.
+  // The control loop will profile if these are all non-zero.
+  linear:ProfileParameters;
+  angular:ProfileParameters;
+
+  // Parameters for a spline to follow. This just contains info on a spline to
+  // compute. Each time this is sent, spline drivetrain will compute a new
+  // spline.
+  spline:SplineGoal;
+
+  // Which spline to follow.
+  spline_handle:int;
+}
+
+root_type Goal;
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index e4de9bb..a46defd 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -5,19 +5,21 @@
 
 #include "aos/controls/control_loop_test.h"
 #include "aos/controls/polytope.h"
-#include "aos/events/event-loop.h"
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/event_loop.h"
 #include "aos/time/time.h"
 #include "gflags/gflags.h"
 #include "gtest/gtest.h"
 
 #include "frc971/control_loops/coerce_goal.h"
 #include "frc971/control_loops/drivetrain/drivetrain.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
 #include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
 #include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
-#include "frc971/control_loops/drivetrain/localizer.q.h"
-#include "frc971/queues/gyro.q.h"
+#include "frc971/control_loops/drivetrain/localizer_generated.h"
+#include "frc971/queues/gyro_generated.h"
 
 namespace frc971 {
 namespace control_loops {
@@ -30,27 +32,21 @@
 class DrivetrainTest : public ::aos::testing::ControlLoopTest {
  protected:
   DrivetrainTest()
-      : ::aos::testing::ControlLoopTest(GetTestDrivetrainConfig().dt),
+      : ::aos::testing::ControlLoopTest(
+            aos::configuration::ReadConfig(
+                "frc971/control_loops/drivetrain/config.json"),
+            GetTestDrivetrainConfig().dt),
         test_event_loop_(MakeEventLoop()),
         drivetrain_goal_sender_(
-            test_event_loop_
-                ->MakeSender<::frc971::control_loops::DrivetrainQueue::Goal>(
-                    ".frc971.control_loops.drivetrain_queue.goal")),
+            test_event_loop_->MakeSender<Goal>("/drivetrain")),
         drivetrain_goal_fetcher_(
-            test_event_loop_
-                ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Goal>(
-                    ".frc971.control_loops.drivetrain_queue.goal")),
+            test_event_loop_->MakeFetcher<Goal>("/drivetrain")),
         drivetrain_status_fetcher_(
-            test_event_loop_
-                ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Status>(
-                    ".frc971.control_loops.drivetrain_queue.status")),
+            test_event_loop_->MakeFetcher<Status>("/drivetrain")),
         drivetrain_output_fetcher_(
-            test_event_loop_
-                ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Output>(
-                    ".frc971.control_loops.drivetrain_queue.output")),
+            test_event_loop_->MakeFetcher<Output>("/drivetrain")),
         localizer_control_sender_(
-            test_event_loop_->MakeSender<LocalizerControl>(
-                ".frc971.control_loops.drivetrain.localizer_control")),
+            test_event_loop_->MakeSender<LocalizerControl>("/drivetrain")),
         drivetrain_event_loop_(MakeEventLoop()),
         dt_config_(GetTestDrivetrainConfig()),
         localizer_(drivetrain_event_loop_.get(), dt_config_),
@@ -65,9 +61,9 @@
 
   void VerifyNearGoal() {
     drivetrain_goal_fetcher_.Fetch();
-    EXPECT_NEAR(drivetrain_goal_fetcher_->left_goal,
+    EXPECT_NEAR(drivetrain_goal_fetcher_->left_goal(),
                 drivetrain_plant_.GetLeftPosition(), 1e-3);
-    EXPECT_NEAR(drivetrain_goal_fetcher_->right_goal,
+    EXPECT_NEAR(drivetrain_goal_fetcher_->right_goal(),
                 drivetrain_plant_.GetRightPosition(), 1e-3);
   }
 
@@ -79,8 +75,10 @@
 
   void VerifyNearSplineGoal() {
     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 double expected_x =
+        CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->x();
+    const double expected_y =
+        CHECK_NOTNULL(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);
@@ -92,7 +90,8 @@
       ::std::this_thread::sleep_for(::std::chrono::milliseconds(5));
       RunFor(dt());
       EXPECT_TRUE(drivetrain_status_fetcher_.Fetch());
-    } while (drivetrain_status_fetcher_->trajectory_logging.planning_state !=
+    } while (CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())
+                 ->planning_state() !=
              (int8_t)SplineDrivetrain::PlanState::kPlannedTrajectory);
   }
 
@@ -100,17 +99,18 @@
     do {
       RunFor(dt());
       EXPECT_TRUE(drivetrain_status_fetcher_.Fetch());
-    } while (!drivetrain_status_fetcher_->trajectory_logging.is_executed);
+    } while (!CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())
+                  ->is_executed());
   }
 
   ::std::unique_ptr<::aos::EventLoop> test_event_loop_;
-  ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Goal>
+  ::aos::Sender<::frc971::control_loops::drivetrain::Goal>
       drivetrain_goal_sender_;
-  ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Goal>
+  ::aos::Fetcher<::frc971::control_loops::drivetrain::Goal>
       drivetrain_goal_fetcher_;
-  ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Status>
+  ::aos::Fetcher<::frc971::control_loops::drivetrain::Status>
       drivetrain_status_fetcher_;
-  ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Output>
+  ::aos::Fetcher<::frc971::control_loops::drivetrain::Output>
       drivetrain_output_fetcher_;
   ::aos::Sender<LocalizerControl> localizer_control_sender_;
 
@@ -128,11 +128,12 @@
 TEST_F(DrivetrainTest, ConvergesCorrectly) {
   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());
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_controller_type(ControllerType_MOTION_PROFILE);
+    goal_builder.add_left_goal(-1.0);
+    goal_builder.add_right_goal(1.0);
+    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
   RunFor(chrono::seconds(2));
   VerifyNearGoal();
@@ -143,11 +144,12 @@
 TEST_F(DrivetrainTest, ConvergesWithVoltageError) {
   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());
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_controller_type(ControllerType_MOTION_PROFILE);
+    goal_builder.add_left_goal(-1.0);
+    goal_builder.add_right_goal(1.0);
+    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
   drivetrain_plant_.set_left_voltage_offset(1.0);
   drivetrain_plant_.set_right_voltage_offset(1.0);
@@ -158,11 +160,12 @@
 // Tests that it survives disabling.
 TEST_F(DrivetrainTest, SurvivesDisabling) {
   {
-    auto message = drivetrain_goal_sender_.MakeMessage();
-    message->controller_type = 1;
-    message->left_goal = -1.0;
-    message->right_goal = 1.0;
-    EXPECT_TRUE(message.Send());
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_controller_type(ControllerType_MOTION_PROFILE);
+    goal_builder.add_left_goal(-1.0);
+    goal_builder.add_right_goal(1.0);
+    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
   for (int i = 0; i < 500; ++i) {
     if (i > 20 && i < 200) {
@@ -187,20 +190,21 @@
 TEST_F(DrivetrainTest, DriveStraightForward) {
   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());
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_controller_type(ControllerType_MOTION_PROFILE);
+    goal_builder.add_left_goal(4.0);
+    goal_builder.add_right_goal(4.0);
+    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
 
   for (int i = 0; i < 500; ++i) {
     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);
+    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();
 }
@@ -210,17 +214,18 @@
 TEST_F(DrivetrainTest, DriveAlmostStraightForward) {
   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());
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_controller_type(ControllerType_MOTION_PROFILE);
+    goal_builder.add_left_goal(4.0);
+    goal_builder.add_right_goal(3.9);
+    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
   for (int i = 0; i < 500; ++i) {
     RunFor(dt());
     ASSERT_TRUE(drivetrain_output_fetcher_.Fetch());
-    EXPECT_GT(drivetrain_output_fetcher_->left_voltage, -11);
-    EXPECT_GT(drivetrain_output_fetcher_->right_voltage, -11);
+    EXPECT_GT(drivetrain_output_fetcher_->left_voltage(), -11);
+    EXPECT_GT(drivetrain_output_fetcher_->right_voltage(), -11);
   }
   VerifyNearGoal();
 }
@@ -254,26 +259,39 @@
 TEST_F(DrivetrainTest, ProfileStraightForward) {
   SetEnabled(true);
   {
-    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());
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    ProfileParameters::Builder linear_builder =
+        builder.MakeBuilder<ProfileParameters>();
+    linear_builder.add_max_velocity(1.0);
+    linear_builder.add_max_acceleration(3.0);
+    flatbuffers::Offset<ProfileParameters> linear_offset =
+        linear_builder.Finish();
+
+    ProfileParameters::Builder angular_builder =
+        builder.MakeBuilder<ProfileParameters>();
+    angular_builder.add_max_velocity(1.0);
+    angular_builder.add_max_acceleration(3.0);
+    flatbuffers::Offset<ProfileParameters> angular_offset =
+        angular_builder.Finish();
+
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_controller_type(ControllerType_MOTION_PROFILE);
+    goal_builder.add_left_goal(4.0);
+    goal_builder.add_right_goal(4.0);
+    goal_builder.add_linear(linear_offset);
+    goal_builder.add_angular(angular_offset);
+    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
 
   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);
+    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();
 }
@@ -282,26 +300,39 @@
 TEST_F(DrivetrainTest, ProfileTurn) {
   SetEnabled(true);
   {
-    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());
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    ProfileParameters::Builder linear_builder =
+        builder.MakeBuilder<ProfileParameters>();
+    linear_builder.add_max_velocity(1.0);
+    linear_builder.add_max_acceleration(3.0);
+    flatbuffers::Offset<ProfileParameters> linear_offset =
+        linear_builder.Finish();
+
+    ProfileParameters::Builder angular_builder =
+        builder.MakeBuilder<ProfileParameters>();
+    angular_builder.add_max_velocity(1.0);
+    angular_builder.add_max_acceleration(3.0);
+    flatbuffers::Offset<ProfileParameters> angular_offset =
+        angular_builder.Finish();
+
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_controller_type(ControllerType_MOTION_PROFILE);
+    goal_builder.add_left_goal(-1.0);
+    goal_builder.add_right_goal(1.0);
+    goal_builder.add_linear(linear_offset);
+    goal_builder.add_angular(angular_offset);
+    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
 
   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);
+    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();
 }
@@ -310,15 +341,28 @@
 TEST_F(DrivetrainTest, SaturatedTurnDrive) {
   SetEnabled(true);
   {
-    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());
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    ProfileParameters::Builder linear_builder =
+        builder.MakeBuilder<ProfileParameters>();
+    linear_builder.add_max_velocity(6.0);
+    linear_builder.add_max_acceleration(4.0);
+    flatbuffers::Offset<ProfileParameters> linear_offset =
+        linear_builder.Finish();
+
+    ProfileParameters::Builder angular_builder =
+        builder.MakeBuilder<ProfileParameters>();
+    angular_builder.add_max_velocity(2.0);
+    angular_builder.add_max_acceleration(4.0);
+    flatbuffers::Offset<ProfileParameters> angular_offset =
+        angular_builder.Finish();
+
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_controller_type(ControllerType_MOTION_PROFILE);
+    goal_builder.add_left_goal(5.0);
+    goal_builder.add_right_goal(4.0);
+    goal_builder.add_linear(linear_offset);
+    goal_builder.add_angular(angular_offset);
+    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
 
   while (monotonic_now() < monotonic_clock::time_point(chrono::seconds(3))) {
@@ -333,61 +377,77 @@
 TEST_F(DrivetrainTest, OpenLoopThenClosed) {
   SetEnabled(true);
   {
-    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());
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_controller_type(ControllerType_POLYDRIVE);
+    goal_builder.add_wheel(0.0);
+    goal_builder.add_throttle(1.0);
+    goal_builder.add_highgear(true);
+    goal_builder.add_quickturn(false);
+    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
 
   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());
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_controller_type(ControllerType_POLYDRIVE);
+    goal_builder.add_wheel(0.0);
+    goal_builder.add_throttle(-0.3);
+    goal_builder.add_highgear(true);
+    goal_builder.add_quickturn(false);
+    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
 
   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());
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_controller_type(ControllerType_POLYDRIVE);
+    goal_builder.add_wheel(0.0);
+    goal_builder.add_throttle(0.0);
+    goal_builder.add_highgear(true);
+    goal_builder.add_quickturn(false);
+    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
 
   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());
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    ProfileParameters::Builder linear_builder =
+        builder.MakeBuilder<ProfileParameters>();
+    linear_builder.add_max_velocity(1.0);
+    linear_builder.add_max_acceleration(2.0);
+    flatbuffers::Offset<ProfileParameters> linear_offset =
+        linear_builder.Finish();
+
+    ProfileParameters::Builder angular_builder =
+        builder.MakeBuilder<ProfileParameters>();
+    angular_builder.add_max_velocity(1.0);
+    angular_builder.add_max_acceleration(2.0);
+    flatbuffers::Offset<ProfileParameters> angular_offset =
+        angular_builder.Finish();
+
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_controller_type(ControllerType_MOTION_PROFILE);
+    goal_builder.add_left_goal(5.0);
+    goal_builder.add_right_goal(4.0);
+    goal_builder.add_linear(linear_offset);
+    goal_builder.add_angular(angular_offset);
+    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
 
   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);
+    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();
 }
@@ -396,22 +456,44 @@
 TEST_F(DrivetrainTest, SplineSimple) {
   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());
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+    flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+        builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
+    flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+        builder.fbb()->CreateVector<float>({0.0, 0.0, 0.25, 0.75, 1.0, 1.0});
+
+    MultiSpline::Builder multispline_builder =
+        builder.MakeBuilder<MultiSpline>();
+
+    multispline_builder.add_spline_count(1);
+    multispline_builder.add_spline_x(spline_x_offset);
+    multispline_builder.add_spline_y(spline_y_offset);
+
+    flatbuffers::Offset<MultiSpline> multispline_offset =
+        multispline_builder.Finish();
+
+    SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+    spline_goal_builder.add_spline_idx(1);
+    spline_goal_builder.add_drive_spline_backwards(false);
+    spline_goal_builder.add_spline(multispline_offset);
+    flatbuffers::Offset<SplineGoal> spline_goal_offset =
+        spline_goal_builder.Finish();
+
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+    goal_builder.add_spline(spline_goal_offset);
+    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
   RunFor(dt());
 
   // Send the start goal
   {
-    auto message = drivetrain_goal_sender_.MakeMessage();
-    message->controller_type = 2;
-    message->spline_handle = 1;
-    EXPECT_TRUE(message.Send());
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+    goal_builder.add_spline_handle(1);
+    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
   WaitForTrajectoryPlan();
 
@@ -423,22 +505,43 @@
 TEST_F(DrivetrainTest, SplineSimpleBackwards) {
   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());
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+    flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+        builder.fbb()->CreateVector<float>({0.0, -0.25, -0.5, -0.5, -0.75, -1.0});
+    flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+        builder.fbb()->CreateVector<float>({0.0, 0.0, -0.25, -0.75, -1.0, -1.0});
+
+    MultiSpline::Builder multispline_builder =
+        builder.MakeBuilder<MultiSpline>();
+
+    multispline_builder.add_spline_count(1);
+    multispline_builder.add_spline_x(spline_x_offset);
+    multispline_builder.add_spline_y(spline_y_offset);
+
+    flatbuffers::Offset<MultiSpline> multispline_offset =
+        multispline_builder.Finish();
+
+    SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+    spline_goal_builder.add_spline_idx(1);
+    spline_goal_builder.add_drive_spline_backwards(true);
+    spline_goal_builder.add_spline(multispline_offset);
+    flatbuffers::Offset<SplineGoal> spline_goal_offset =
+        spline_goal_builder.Finish();
+
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+    goal_builder.add_spline(spline_goal_offset);
+    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
   RunFor(dt());
 
   {
-    auto message = drivetrain_goal_sender_.MakeMessage();
-    message->controller_type = 2;
-    message->spline_handle = 1;
-    EXPECT_TRUE(message.Send());
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+    goal_builder.add_spline_handle(1);
+    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
   WaitForTrajectoryPlan();
 
@@ -456,7 +559,7 @@
   drivetrain_status_fetcher_.Fetch();
   auto actual = drivetrain_plant_.state();
   const double expected_theta =
-      drivetrain_status_fetcher_->trajectory_logging.theta;
+      CHECK_NOTNULL(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);
@@ -467,14 +570,35 @@
 TEST_F(DrivetrainTest, SplineSingleGoal) {
   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());
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+    flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+        builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
+    flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+        builder.fbb()->CreateVector<float>({0.0, 0.0, 0.25, 0.75, 1.0, 1.0});
+
+    MultiSpline::Builder multispline_builder =
+        builder.MakeBuilder<MultiSpline>();
+
+    multispline_builder.add_spline_count(1);
+    multispline_builder.add_spline_x(spline_x_offset);
+    multispline_builder.add_spline_y(spline_y_offset);
+
+    flatbuffers::Offset<MultiSpline> multispline_offset =
+        multispline_builder.Finish();
+
+    SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+    spline_goal_builder.add_spline_idx(1);
+    spline_goal_builder.add_drive_spline_backwards(false);
+    spline_goal_builder.add_spline(multispline_offset);
+    flatbuffers::Offset<SplineGoal> spline_goal_offset =
+        spline_goal_builder.Finish();
+
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+    goal_builder.add_spline(spline_goal_offset);
+    goal_builder.add_spline_handle(1);
+    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
   WaitForTrajectoryPlan();
 
@@ -486,100 +610,179 @@
 TEST_F(DrivetrainTest, SplineStop) {
   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());
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+    flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+        builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
+    flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+        builder.fbb()->CreateVector<float>({0.0, 0.0, 0.25, 0.75, 1.0, 1.0});
+
+    MultiSpline::Builder multispline_builder =
+        builder.MakeBuilder<MultiSpline>();
+
+    multispline_builder.add_spline_count(1);
+    multispline_builder.add_spline_x(spline_x_offset);
+    multispline_builder.add_spline_y(spline_y_offset);
+
+    flatbuffers::Offset<MultiSpline> multispline_offset =
+        multispline_builder.Finish();
+
+    SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+    spline_goal_builder.add_spline_idx(1);
+    spline_goal_builder.add_drive_spline_backwards(false);
+    spline_goal_builder.add_spline(multispline_offset);
+    flatbuffers::Offset<SplineGoal> spline_goal_offset =
+        spline_goal_builder.Finish();
+
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+    goal_builder.add_spline(spline_goal_offset);
+    goal_builder.add_spline_handle(1);
+    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
   WaitForTrajectoryPlan();
 
   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;
+  const double goal_x =
+      CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->x();
+  const double goal_y =
+      CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->y();
 
   // Now stop.
   {
-    auto message = drivetrain_goal_sender_.MakeMessage();
-    message->controller_type = 2;
-    message->spline_handle = 0;
-    EXPECT_TRUE(message.Send());
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+    goal_builder.add_spline_handle(0);
+    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
   RunFor(chrono::milliseconds(2000));
 
   // The goal shouldn't change after being stopped.
   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);
+  EXPECT_NEAR(
+      CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->x(),
+      goal_x, 1e-9);
+  EXPECT_NEAR(
+      CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->y(),
+      goal_y, 1e-9);
 }
 
 // Tests that a spline can't be restarted.
 TEST_F(DrivetrainTest, SplineRestart) {
   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());
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+    flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+        builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
+    flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+        builder.fbb()->CreateVector<float>({0.0, 0.0, 0.25, 0.75, 1.0, 1.0});
+
+    MultiSpline::Builder multispline_builder =
+        builder.MakeBuilder<MultiSpline>();
+
+    multispline_builder.add_spline_count(1);
+    multispline_builder.add_spline_x(spline_x_offset);
+    multispline_builder.add_spline_y(spline_y_offset);
+
+    flatbuffers::Offset<MultiSpline> multispline_offset =
+        multispline_builder.Finish();
+
+    SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+    spline_goal_builder.add_spline_idx(1);
+    spline_goal_builder.add_drive_spline_backwards(false);
+    spline_goal_builder.add_spline(multispline_offset);
+    flatbuffers::Offset<SplineGoal> spline_goal_offset =
+        spline_goal_builder.Finish();
+
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+    goal_builder.add_spline(spline_goal_offset);
+    goal_builder.add_spline_handle(1);
+    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
   WaitForTrajectoryPlan();
 
   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;
+  const double goal_x =
+      CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->x();
+  const double goal_y =
+      CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->y();
 
   // Send a stop goal.
   {
-    auto message = drivetrain_goal_sender_.MakeMessage();
-    message->controller_type = 2;
-    message->spline_handle = 0;
-    EXPECT_TRUE(message.Send());
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+    goal_builder.add_spline_handle(0);
+    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
   RunFor(chrono::milliseconds(500));
 
   // Send a restart.
   {
-    auto message = drivetrain_goal_sender_.MakeMessage();
-    message->controller_type = 2;
-    message->spline_handle = 1;
-    EXPECT_TRUE(message.Send());
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+    goal_builder.add_spline_handle(1);
+    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
   RunFor(chrono::milliseconds(2000));
 
   // The goal shouldn't change after being stopped and restarted.
   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);
+  EXPECT_NEAR(
+      CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->x(),
+      goal_x, 1e-9);
+  EXPECT_NEAR(
+      CHECK_NOTNULL(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) {
   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());
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+    flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+        builder.fbb()->CreateVector<float>({0.2, 0.25, 0.5, 0.5, 0.75, 1.0});
+    flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+        builder.fbb()->CreateVector<float>({0.2, 0.0, 0.25, 0.75, 1.0, 1.0});
+
+    MultiSpline::Builder multispline_builder =
+        builder.MakeBuilder<MultiSpline>();
+
+    multispline_builder.add_spline_count(1);
+    multispline_builder.add_spline_x(spline_x_offset);
+    multispline_builder.add_spline_y(spline_y_offset);
+
+    flatbuffers::Offset<MultiSpline> multispline_offset =
+        multispline_builder.Finish();
+
+    SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+    spline_goal_builder.add_spline_idx(1);
+    spline_goal_builder.add_drive_spline_backwards(false);
+    spline_goal_builder.add_spline(multispline_offset);
+    flatbuffers::Offset<SplineGoal> spline_goal_offset =
+        spline_goal_builder.Finish();
+
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+    goal_builder.add_spline(spline_goal_offset);
+    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
   RunFor(dt());
 
   {
-    auto message = drivetrain_goal_sender_.MakeMessage();
-    message->controller_type = 2;
-    message->spline_handle = 1;
-    EXPECT_TRUE(message.Send());
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+    goal_builder.add_spline_handle(1);
+    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
   WaitForTrajectoryPlan();
 
@@ -592,21 +795,43 @@
 TEST_F(DrivetrainTest, SplineSideOffset) {
   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());
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+    flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+        builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
+    flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+        builder.fbb()->CreateVector<float>({0.5, 0.0, 0.25, 0.75, 1.0, 1.0});
+
+    MultiSpline::Builder multispline_builder =
+        builder.MakeBuilder<MultiSpline>();
+
+    multispline_builder.add_spline_count(1);
+    multispline_builder.add_spline_x(spline_x_offset);
+    multispline_builder.add_spline_y(spline_y_offset);
+
+    flatbuffers::Offset<MultiSpline> multispline_offset =
+        multispline_builder.Finish();
+
+    SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+    spline_goal_builder.add_spline_idx(1);
+    spline_goal_builder.add_drive_spline_backwards(false);
+    spline_goal_builder.add_spline(multispline_offset);
+    flatbuffers::Offset<SplineGoal> spline_goal_offset =
+        spline_goal_builder.Finish();
+
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+    goal_builder.add_spline(spline_goal_offset);
+    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
   RunFor(dt());
 
   {
-    auto message = drivetrain_goal_sender_.MakeMessage();
-    message->controller_type = 2;
-    message->spline_handle = 1;
-    EXPECT_TRUE(message.Send());
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+    goal_builder.add_spline_handle(1);
+    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
   WaitForTrajectoryPlan();
 
@@ -618,23 +843,45 @@
 TEST_F(DrivetrainTest, MultiSpline) {
   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());
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+    flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+        builder.fbb()->CreateVector<float>(
+            {0.0, 0.25, 0.5, 0.5, 0.75, 1.0, 1.25, 1.5, 1.5, 1.25, 1.0});
+    flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+        builder.fbb()->CreateVector<float>(
+            {0.0, 0.0, 0.25, 0.75, 1.0, 1.0, 1.0, 1.25, 1.5, 1.75, 2.0});
+
+    MultiSpline::Builder multispline_builder =
+        builder.MakeBuilder<MultiSpline>();
+
+    multispline_builder.add_spline_count(2);
+    multispline_builder.add_spline_x(spline_x_offset);
+    multispline_builder.add_spline_y(spline_y_offset);
+
+    flatbuffers::Offset<MultiSpline> multispline_offset =
+        multispline_builder.Finish();
+
+    SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+    spline_goal_builder.add_spline_idx(1);
+    spline_goal_builder.add_drive_spline_backwards(false);
+    spline_goal_builder.add_spline(multispline_offset);
+    flatbuffers::Offset<SplineGoal> spline_goal_offset =
+        spline_goal_builder.Finish();
+
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+    goal_builder.add_spline(spline_goal_offset);
+    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
   RunFor(dt());
 
   {
-    auto message = drivetrain_goal_sender_.MakeMessage();
-    message->controller_type = 2;
-    message->spline_handle = 1;
-    EXPECT_TRUE(message.Send());
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+    goal_builder.add_spline_handle(1);
+    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
   WaitForTrajectoryPlan();
 
@@ -646,21 +893,43 @@
 TEST_F(DrivetrainTest, SequentialSplines) {
   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());
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+    flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+        builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
+    flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+        builder.fbb()->CreateVector<float>({0.0, 0.0, 0.25, 0.75, 1.0, 1.0});
+
+    MultiSpline::Builder multispline_builder =
+        builder.MakeBuilder<MultiSpline>();
+
+    multispline_builder.add_spline_count(1);
+    multispline_builder.add_spline_x(spline_x_offset);
+    multispline_builder.add_spline_y(spline_y_offset);
+
+    flatbuffers::Offset<MultiSpline> multispline_offset =
+        multispline_builder.Finish();
+
+    SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+    spline_goal_builder.add_spline_idx(1);
+    spline_goal_builder.add_drive_spline_backwards(false);
+    spline_goal_builder.add_spline(multispline_offset);
+    flatbuffers::Offset<SplineGoal> spline_goal_offset =
+        spline_goal_builder.Finish();
+
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+    goal_builder.add_spline(spline_goal_offset);
+    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
   RunFor(dt());
 
   {
-    auto message = drivetrain_goal_sender_.MakeMessage();
-    message->controller_type = 2;
-    message->spline_handle = 1;
-    EXPECT_TRUE(message.Send());
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+    goal_builder.add_spline_handle(1);
+    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
   WaitForTrajectoryPlan();
 
@@ -670,22 +939,44 @@
 
   // 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());
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+    flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+        builder.fbb()->CreateVector<float>({1.0, 1.25, 1.5, 1.5, 1.25, 1.0});
+    flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+        builder.fbb()->CreateVector<float>({1.0, 1.0, 1.25, 1.5, 1.75, 2.0});
+
+    MultiSpline::Builder multispline_builder =
+        builder.MakeBuilder<MultiSpline>();
+
+    multispline_builder.add_spline_count(1);
+    multispline_builder.add_spline_x(spline_x_offset);
+    multispline_builder.add_spline_y(spline_y_offset);
+
+    flatbuffers::Offset<MultiSpline> multispline_offset =
+        multispline_builder.Finish();
+
+    SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+    spline_goal_builder.add_spline_idx(2);
+    spline_goal_builder.add_drive_spline_backwards(false);
+    spline_goal_builder.add_spline(multispline_offset);
+    flatbuffers::Offset<SplineGoal> spline_goal_offset =
+        spline_goal_builder.Finish();
+
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+    goal_builder.add_spline(spline_goal_offset);
+    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
   RunFor(dt());
 
   // And then start it.
   {
-    auto message = drivetrain_goal_sender_.MakeMessage();
-    message->controller_type = 2;
-    message->spline_handle = 2;
-    EXPECT_TRUE(message.Send());
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+    goal_builder.add_spline_handle(2);
+    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
   WaitForTrajectoryPlan();
 
@@ -696,14 +987,35 @@
 TEST_F(DrivetrainTest, SplineStopFirst) {
   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());
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+    flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+        builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
+    flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+        builder.fbb()->CreateVector<float>({0.0, 0.0, 0.25, 0.75, 1.0, 1.0});
+
+    MultiSpline::Builder multispline_builder =
+        builder.MakeBuilder<MultiSpline>();
+
+    multispline_builder.add_spline_count(1);
+    multispline_builder.add_spline_x(spline_x_offset);
+    multispline_builder.add_spline_y(spline_y_offset);
+
+    flatbuffers::Offset<MultiSpline> multispline_offset =
+        multispline_builder.Finish();
+
+    SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+    spline_goal_builder.add_spline_idx(1);
+    spline_goal_builder.add_drive_spline_backwards(false);
+    spline_goal_builder.add_spline(multispline_offset);
+    flatbuffers::Offset<SplineGoal> spline_goal_offset =
+        spline_goal_builder.Finish();
+
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+    goal_builder.add_spline_handle(1);
+    goal_builder.add_spline(spline_goal_offset);
+    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
   WaitForTrajectoryPlan();
 
@@ -711,23 +1023,45 @@
 
   // Stop goal
   {
-    auto message = drivetrain_goal_sender_.MakeMessage();
-    message->controller_type = 2;
-    message->spline_handle = 0;
-    EXPECT_TRUE(message.Send());
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+    goal_builder.add_spline_handle(0);
+    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
   RunFor(chrono::milliseconds(500));
 
   // 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());
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+    flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+        builder.fbb()->CreateVector<float>({1.0, 1.25, 1.5, 1.5, 1.25, 1.0});
+    flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+        builder.fbb()->CreateVector<float>({1.0, 1.0, 1.25, 1.5, 1.75, 2.0});
+
+    MultiSpline::Builder multispline_builder =
+        builder.MakeBuilder<MultiSpline>();
+
+    multispline_builder.add_spline_count(1);
+    multispline_builder.add_spline_x(spline_x_offset);
+    multispline_builder.add_spline_y(spline_y_offset);
+
+    flatbuffers::Offset<MultiSpline> multispline_offset =
+        multispline_builder.Finish();
+
+    SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+    spline_goal_builder.add_spline_idx(2);
+    spline_goal_builder.add_drive_spline_backwards(false);
+    spline_goal_builder.add_spline(multispline_offset);
+    flatbuffers::Offset<SplineGoal> spline_goal_offset =
+        spline_goal_builder.Finish();
+
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+    goal_builder.add_spline(spline_goal_offset);
+    goal_builder.add_spline_handle(2);
+    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
   WaitForTrajectoryPlan();
 
@@ -740,15 +1074,35 @@
 TEST_F(DrivetrainTest, CancelSplineBeforeExecuting) {
   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());
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+    flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+        builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
+    flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+        builder.fbb()->CreateVector<float>({0.0, 0.0, 0.25, 0.75, 1.0, 1.0});
+
+    MultiSpline::Builder multispline_builder =
+        builder.MakeBuilder<MultiSpline>();
+
+    multispline_builder.add_spline_count(1);
+    multispline_builder.add_spline_x(spline_x_offset);
+    multispline_builder.add_spline_y(spline_y_offset);
+
+    flatbuffers::Offset<MultiSpline> multispline_offset =
+        multispline_builder.Finish();
+
+    SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+    spline_goal_builder.add_spline_idx(1);
+    spline_goal_builder.add_drive_spline_backwards(false);
+    spline_goal_builder.add_spline(multispline_offset);
+    flatbuffers::Offset<SplineGoal> spline_goal_offset =
+        spline_goal_builder.Finish();
+
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+    goal_builder.add_spline(spline_goal_offset);
+    goal_builder.add_spline_handle(0);
+    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
   WaitForTrajectoryPlan();
 
@@ -756,23 +1110,45 @@
 
   // Plan another spline, but don't start it yet:
   {
-    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());
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+    flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+        builder.fbb()->CreateVector<float>({0.0, 0.75, 1.25, 1.5, 1.25, 1.0});
+    flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+        builder.fbb()->CreateVector<float>({0.0, 0.75, 1.25, 1.5, 1.75, 2.0});
+
+    MultiSpline::Builder multispline_builder =
+        builder.MakeBuilder<MultiSpline>();
+
+    multispline_builder.add_spline_count(1);
+    multispline_builder.add_spline_x(spline_x_offset);
+    multispline_builder.add_spline_y(spline_y_offset);
+
+    flatbuffers::Offset<MultiSpline> multispline_offset =
+        multispline_builder.Finish();
+
+    SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+    spline_goal_builder.add_spline_idx(2);
+    spline_goal_builder.add_drive_spline_backwards(false);
+    spline_goal_builder.add_spline(multispline_offset);
+    flatbuffers::Offset<SplineGoal> spline_goal_offset =
+        spline_goal_builder.Finish();
+
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+    goal_builder.add_spline(spline_goal_offset);
+    goal_builder.add_spline_handle(0);
+    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
   WaitForTrajectoryPlan();
 
   // Now execute it.
   {
-    auto message = drivetrain_goal_sender_.MakeMessage();
-    message->controller_type = 2;
-    message->spline_handle = 2;
-    EXPECT_TRUE(message.Send());
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+    goal_builder.add_spline_handle(2);
+    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
 
   WaitForTrajectoryExecution();
@@ -784,35 +1160,78 @@
 TEST_F(DrivetrainTest, ParallelSplines) {
   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());
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+    flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+        builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
+    flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+        builder.fbb()->CreateVector<float>({0.0, 0.0, 0.25, 0.75, 1.0, 1.0});
+
+    MultiSpline::Builder multispline_builder =
+        builder.MakeBuilder<MultiSpline>();
+
+    multispline_builder.add_spline_count(1);
+    multispline_builder.add_spline_x(spline_x_offset);
+    multispline_builder.add_spline_y(spline_y_offset);
+
+    flatbuffers::Offset<MultiSpline> multispline_offset =
+        multispline_builder.Finish();
+
+    SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+    spline_goal_builder.add_spline_idx(1);
+    spline_goal_builder.add_drive_spline_backwards(false);
+    spline_goal_builder.add_spline(multispline_offset);
+    flatbuffers::Offset<SplineGoal> spline_goal_offset =
+        spline_goal_builder.Finish();
+
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+    goal_builder.add_spline(spline_goal_offset);
+    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
   WaitForTrajectoryPlan();
 
   // 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());
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+    flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+        builder.fbb()->CreateVector<float>({1.0, 1.25, 1.5, 1.5, 1.25, 1.0});
+    flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+        builder.fbb()->CreateVector<float>({1.0, 1.0, 1.25, 1.5, 1.75, 2.0});
+
+    MultiSpline::Builder multispline_builder =
+        builder.MakeBuilder<MultiSpline>();
+
+    multispline_builder.add_spline_count(1);
+    multispline_builder.add_spline_x(spline_x_offset);
+    multispline_builder.add_spline_y(spline_y_offset);
+
+    flatbuffers::Offset<MultiSpline> multispline_offset =
+        multispline_builder.Finish();
+
+    SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+    spline_goal_builder.add_spline_idx(2);
+    spline_goal_builder.add_drive_spline_backwards(false);
+    spline_goal_builder.add_spline(multispline_offset);
+    flatbuffers::Offset<SplineGoal> spline_goal_offset =
+        spline_goal_builder.Finish();
+
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+    goal_builder.add_spline(spline_goal_offset);
+    goal_builder.add_spline_handle(1);
+    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
   WaitForTrajectoryExecution();
 
   // Second start goal
   {
-    auto message = drivetrain_goal_sender_.MakeMessage();
-    message->controller_type = 2;
-    message->spline_handle = 2;
-    EXPECT_TRUE(message.Send());
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+    goal_builder.add_spline_handle(2);
+    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
 
   RunFor(chrono::milliseconds(4000));
@@ -823,20 +1242,42 @@
 TEST_F(DrivetrainTest, OnlyPlanSpline) {
   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());
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+    flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+        builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
+    flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+        builder.fbb()->CreateVector<float>({0.0, 0.0, 0.25, 0.75, 1.0, 1.0});
+
+    MultiSpline::Builder multispline_builder =
+        builder.MakeBuilder<MultiSpline>();
+
+    multispline_builder.add_spline_count(1);
+    multispline_builder.add_spline_x(spline_x_offset);
+    multispline_builder.add_spline_y(spline_y_offset);
+
+    flatbuffers::Offset<MultiSpline> multispline_offset =
+        multispline_builder.Finish();
+
+    SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+    spline_goal_builder.add_spline_idx(1);
+    spline_goal_builder.add_drive_spline_backwards(false);
+    spline_goal_builder.add_spline(multispline_offset);
+    flatbuffers::Offset<SplineGoal> spline_goal_offset =
+        spline_goal_builder.Finish();
+
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+    goal_builder.add_spline(spline_goal_offset);
+    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
   WaitForTrajectoryPlan();
 
   for (int i = 0; i < 100; ++i) {
     RunFor(dt());
     drivetrain_status_fetcher_.Fetch();
-    EXPECT_EQ(drivetrain_status_fetcher_->trajectory_logging.planning_state,
+    EXPECT_EQ(CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())
+                  ->planning_state(),
               3);
     ::std::this_thread::sleep_for(::std::chrono::milliseconds(2));
   }
@@ -847,23 +1288,45 @@
 TEST_F(DrivetrainTest, SplineExecuteAfterPlan) {
   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());
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+    flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+        builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
+    flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+        builder.fbb()->CreateVector<float>({0.0, 0.0, 0.25, 0.75, 1.0, 1.0});
+
+    MultiSpline::Builder multispline_builder =
+        builder.MakeBuilder<MultiSpline>();
+
+    multispline_builder.add_spline_count(1);
+    multispline_builder.add_spline_x(spline_x_offset);
+    multispline_builder.add_spline_y(spline_y_offset);
+
+    flatbuffers::Offset<MultiSpline> multispline_offset =
+        multispline_builder.Finish();
+
+    SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+    spline_goal_builder.add_spline_idx(1);
+    spline_goal_builder.add_drive_spline_backwards(false);
+    spline_goal_builder.add_spline(multispline_offset);
+    flatbuffers::Offset<SplineGoal> spline_goal_offset =
+        spline_goal_builder.Finish();
+
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+    goal_builder.add_spline(spline_goal_offset);
+    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
   WaitForTrajectoryPlan();
   RunFor(chrono::milliseconds(2000));
 
   // Start goal
   {
-    auto message = drivetrain_goal_sender_.MakeMessage();
-    message->controller_type = 2;
-    message->spline_handle = 1;
-    EXPECT_TRUE(message.Send());
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+    goal_builder.add_spline_handle(1);
+    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
   RunFor(chrono::milliseconds(2000));
 
@@ -877,21 +1340,29 @@
   localizer_.target_selector()->set_has_target(true);
   localizer_.target_selector()->set_pose({{1.0, 1.0, 0.0}, M_PI_4});
   {
-    auto message = drivetrain_goal_sender_.MakeMessage();
-    message->controller_type = 3;
-    message->throttle = 0.5;
-    EXPECT_TRUE(message.Send());
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_controller_type(ControllerType_LINE_FOLLOWER);
+    goal_builder.add_throttle(0.5);
+    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
 
   RunFor(chrono::seconds(5));
 
   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,
-                  drivetrain_status_fetcher_->line_follow_logging.theta);
+  EXPECT_TRUE(CHECK_NOTNULL(drivetrain_status_fetcher_->line_follow_logging())
+                  ->frozen());
+  EXPECT_TRUE(CHECK_NOTNULL(drivetrain_status_fetcher_->line_follow_logging())
+                  ->have_target());
+  EXPECT_EQ(
+      1.0,
+      CHECK_NOTNULL(drivetrain_status_fetcher_->line_follow_logging())->x());
+  EXPECT_EQ(
+      1.0,
+      CHECK_NOTNULL(drivetrain_status_fetcher_->line_follow_logging())->y());
+  EXPECT_FLOAT_EQ(
+      M_PI_4, CHECK_NOTNULL(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_plant_.GetPosition().x());
@@ -906,10 +1377,11 @@
   localizer_.target_selector()->set_has_target(false);
   localizer_.target_selector()->set_pose({{1.0, 1.0, 0.0}, M_PI_4});
   {
-    auto message = drivetrain_goal_sender_.MakeMessage();
-    message->controller_type = 3;
-    message->throttle = 0.5;
-    EXPECT_TRUE(message.Send());
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_controller_type(ControllerType_LINE_FOLLOWER);
+    goal_builder.add_throttle(0.5);
+    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
 
   RunFor(chrono::seconds(5));
@@ -926,11 +1398,13 @@
   EXPECT_EQ(0.0, localizer_.y());
   EXPECT_EQ(0.0, localizer_.theta());
   {
-    auto message = localizer_control_sender_.MakeMessage();
-    message->x = 9.0;
-    message->y = 7.0;
-    message->theta = 1.0;
-    ASSERT_TRUE(message.Send());
+    auto builder = localizer_control_sender_.MakeBuilder();
+    LocalizerControl::Builder localizer_control_builder =
+        builder.MakeBuilder<LocalizerControl>();
+    localizer_control_builder.add_x(9.0);
+    localizer_control_builder.add_y(7.0);
+    localizer_control_builder.add_theta(1.0);
+    ASSERT_TRUE(builder.Send(localizer_control_builder.Finish()));
   }
   RunFor(dt());
 
diff --git a/frc971/control_loops/drivetrain/drivetrain_output.fbs b/frc971/control_loops/drivetrain/drivetrain_output.fbs
new file mode 100644
index 0000000..da8c889
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain_output.fbs
@@ -0,0 +1,13 @@
+namespace frc971.control_loops.drivetrain;
+
+table Output {
+  // Voltage to send to motor(s) on either side of the drivetrain.
+  left_voltage:double;
+  right_voltage:double;
+
+  // Whether to set each shifter piston to high gear.
+  left_high:bool;
+  right_high:bool;
+}
+
+root_type Output;
diff --git a/frc971/control_loops/drivetrain/drivetrain_position.fbs b/frc971/control_loops/drivetrain/drivetrain_position.fbs
new file mode 100644
index 0000000..900c036
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain_position.fbs
@@ -0,0 +1,25 @@
+namespace frc971.control_loops.drivetrain;
+
+table Position {
+  // Relative position of each drivetrain side (in meters).
+  left_encoder:double;
+  right_encoder:double;
+
+  // The speed in m/s of each drivetrain side from the most recent encoder
+  // pulse, or 0 if there was no edge within the last 5ms.
+  left_speed:double;
+  right_speed:double;
+
+  // Position of each drivetrain shifter, scaled from 0.0 to 1.0 where smaller
+  // is towards low gear.
+  left_shifter_position:double;
+  right_shifter_position:double;
+
+  // Raw analog voltages of each shifter hall effect for logging purposes.
+  low_left_hall:double;
+  high_left_hall:double;
+  low_right_hall:double;
+  high_right_hall:double;
+}
+
+root_type Position;
diff --git a/frc971/control_loops/drivetrain/drivetrain_status.fbs b/frc971/control_loops/drivetrain/drivetrain_status.fbs
new file mode 100644
index 0000000..67134f5
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain_status.fbs
@@ -0,0 +1,140 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace frc971.control_loops.drivetrain;
+
+// For logging information about what the code is doing with the shifters.
+table GearLogging {
+  // Which controller is being used.
+  controller_index:byte;
+
+  // Whether each loop for the drivetrain sides is the high-gear one.
+  left_loop_high:bool;
+  right_loop_high:bool;
+
+  // The states of each drivetrain shifter.
+  left_state:byte;
+  right_state:byte;
+}
+
+// For logging information about the state of the shifters.
+table CIMLogging {
+  // Whether the code thinks each drivetrain side is currently in gear.
+  left_in_gear:bool;
+  right_in_gear:bool;
+
+  // The angular velocities (in rad/s, positive forward) the code thinks motors
+  // on each side of the drivetrain are moving at.
+  left_motor_speed:double;
+  right_motor_speed:double;
+
+  // The velocity estimates for each drivetrain side of the robot (in m/s,
+  // positive forward) that can be used for shifting.
+  left_velocity:double;
+  right_velocity:double;
+}
+
+enum PlanningState : byte {
+  NO_PLAN,
+  BUILDING_TRAJECTORY,
+  PLANNING_TRAJECTORY,
+  PLANNED,
+}
+
+// For logging information about the state of the trajectory planning.
+table TrajectoryLogging {
+  // state of planning the trajectory.
+  planning_state:PlanningState;
+
+  // State of the spline execution.
+  is_executing:bool;
+  // Whether we have finished the spline specified by current_spline_idx.
+  is_executed:bool;
+
+  // The handle of the goal spline.  0 means stop requested.
+  goal_spline_handle:int;
+  // Handle of the executing spline.  -1 means none requested.  If there was no
+  // spline executing when a spline finished optimizing, it will become the
+  // current spline even if we aren't ready to start yet.
+  current_spline_idx:int;
+  // Handle of the spline that is being optimized and staged.
+  planning_spline_idx:int;
+
+  // Expected position and velocity on the spline
+  x:float;
+  y:float;
+  theta:float;
+  left_velocity:float;
+  right_velocity:float;
+  distance_remaining:float;
+}
+
+// For logging state of the line follower.
+table LineFollowLogging {
+  // Whether we are currently freezing target choice.
+  frozen:bool;
+  // Whether we currently have a target.
+  have_target:bool;
+  // Absolute position of the current goal.
+  x:float;
+  y:float;
+  theta:float;
+  // Current lateral offset from line pointing straight out of the target.
+  offset:float;
+  // Current distance from the plane of the target, in meters.
+  distance_to_target:float;
+  // Current goal heading.
+  goal_theta:float;
+  // Current relative heading.
+  rel_theta:float;
+}
+
+table Status {
+  // Estimated speed of the center of the robot in m/s (positive forwards).
+  robot_speed:double;
+
+  // Estimated relative position of each drivetrain side (in meters).
+  estimated_left_position:double;
+  estimated_right_position:double;
+
+  // Estimated velocity of each drivetrain side (in m/s).
+  estimated_left_velocity:double;
+  estimated_right_velocity:double;
+
+  // The voltage we wanted to send to each drivetrain side last cycle.
+  uncapped_left_voltage:double;
+  uncapped_right_voltage:double;
+
+  // The voltage error for the left and right sides.
+  left_voltage_error:double;
+  right_voltage_error:double;
+
+  // The profiled goal states.
+  profiled_left_position_goal:double;
+  profiled_right_position_goal:double;
+  profiled_left_velocity_goal:double;
+  profiled_right_velocity_goal:double;
+
+  // The KF offset
+  estimated_angular_velocity_error:double;
+  // The KF estimated heading.
+  estimated_heading:double;
+
+  // xytheta of the robot.
+  x:double;
+  y:double;
+  theta:double;
+
+  // True if the output voltage was capped last cycle.
+  output_was_capped:bool;
+
+  // The angle of the robot relative to the ground.
+  ground_angle:double;
+
+  // Information about shifting logic and curent gear, for logging purposes
+  gear_logging:GearLogging;
+  cim_logging:CIMLogging;
+  trajectory_logging:TrajectoryLogging;
+  line_follow_logging:LineFollowLogging;
+}
+
+root_type Status;
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
index 34c9907..8ebbb1b 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
@@ -90,23 +90,20 @@
 DrivetrainSimulation::DrivetrainSimulation(
     ::aos::EventLoop *event_loop, const DrivetrainConfig<double> &dt_config)
     : event_loop_(event_loop),
-      robot_state_fetcher_(
-          event_loop_->MakeFetcher<::aos::RobotState>(".aos.robot_state")),
+      robot_state_fetcher_(event_loop_->MakeFetcher<::aos::RobotState>("/aos")),
       drivetrain_position_sender_(
           event_loop_
-              ->MakeSender<::frc971::control_loops::DrivetrainQueue::Position>(
-                  ".frc971.control_loops.drivetrain_queue.position")),
+              ->MakeSender<::frc971::control_loops::drivetrain::Position>(
+                  "/drivetrain")),
       drivetrain_output_fetcher_(
-          event_loop_
-              ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Output>(
-                  ".frc971.control_loops.drivetrain_queue.output")),
+          event_loop_->MakeFetcher<::frc971::control_loops::drivetrain::Output>(
+              "/drivetrain")),
       drivetrain_status_fetcher_(
-          event_loop_
-              ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Status>(
-                  ".frc971.control_loops.drivetrain_queue.status")),
+          event_loop_->MakeFetcher<::frc971::control_loops::drivetrain::Status>(
+              "/drivetrain")),
       gyro_reading_sender_(
           event_loop->MakeSender<::frc971::sensors::GyroReading>(
-              ".frc971.sensors.gyro_reading")),
+              "/drivetrain")),
       dt_config_(dt_config),
       drivetrain_plant_(MakePlantFromConfig(dt_config_)),
       velocity_drivetrain_(
@@ -133,9 +130,9 @@
             actual_y_.push_back(actual_position(1));
 
             trajectory_x_.push_back(
-                drivetrain_status_fetcher_->trajectory_logging.x);
+                drivetrain_status_fetcher_->trajectory_logging()->x());
             trajectory_y_.push_back(
-                drivetrain_status_fetcher_->trajectory_logging.y);
+                drivetrain_status_fetcher_->trajectory_logging()->y());
           }
         }
         first_ = false;
@@ -159,22 +156,27 @@
   const double right_encoder = GetRightPosition();
 
   {
-    ::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;
-    position->right_shifter_position = right_gear_high_ ? 1.0 : 0.0;
-    position.Send();
+    ::aos::Sender<::frc971::control_loops::drivetrain::Position>::Builder
+        builder = drivetrain_position_sender_.MakeBuilder();
+    frc971::control_loops::drivetrain::Position::Builder position_builder =
+        builder.MakeBuilder<frc971::control_loops::drivetrain::Position>();
+    position_builder.add_left_encoder(left_encoder);
+    position_builder.add_right_encoder(right_encoder);
+    position_builder.add_left_shifter_position(left_gear_high_ ? 1.0 : 0.0);
+    position_builder.add_right_shifter_position(right_gear_high_ ? 1.0 : 0.0);
+    builder.Send(position_builder.Finish());
   }
 
   {
-    auto gyro = gyro_reading_sender_.MakeMessage();
-    gyro->angle =
-        (right_encoder - left_encoder) / (dt_config_.robot_radius * 2.0);
-    gyro->velocity = (drivetrain_plant_.X(3, 0) - drivetrain_plant_.X(1, 0)) /
-                     (dt_config_.robot_radius * 2.0);
-    gyro.Send();
+    auto builder = gyro_reading_sender_.MakeBuilder();
+    frc971::sensors::GyroReading::Builder gyro_builder =
+        builder.MakeBuilder<frc971::sensors::GyroReading>();
+    gyro_builder.add_angle((right_encoder - left_encoder) /
+                           (dt_config_.robot_radius * 2.0));
+    gyro_builder.add_velocity(
+        (drivetrain_plant_.X(3, 0) - drivetrain_plant_.X(1, 0)) /
+        (dt_config_.robot_radius * 2.0));
+    builder.Send(gyro_builder.Finish());
   }
 }
 
@@ -184,17 +186,17 @@
   last_right_position_ = drivetrain_plant_.Y(1, 0);
   EXPECT_TRUE(drivetrain_output_fetcher_.Fetch());
   ::Eigen::Matrix<double, 2, 1> U = last_U_;
-  last_U_ << drivetrain_output_fetcher_->left_voltage,
-      drivetrain_output_fetcher_->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()
-                              ? robot_state_fetcher_->voltage_battery / 12.0
+                              ? robot_state_fetcher_->voltage_battery() / 12.0
                               : 1.0;
     last_U_ *= scalar;
   }
-  left_gear_high_ = drivetrain_output_fetcher_->left_high;
-  right_gear_high_ = drivetrain_output_fetcher_->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_) {
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.h b/frc971/control_loops/drivetrain/drivetrain_test_lib.h
index 9a4f177..2284f63 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.h
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.h
@@ -1,11 +1,15 @@
 #ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_TEST_LIB_H_
 #define FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_TEST_LIB_H_
 
-#include "aos/events/event-loop.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "aos/events/event_loop.h"
+#include "frc971/control_loops/control_loops_generated.h"
 #include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
 #include "frc971/control_loops/state_feedback_loop.h"
-#include "frc971/queues/gyro.q.h"
+#include "frc971/queues/gyro_generated.h"
 
 namespace frc971 {
 namespace control_loops {
@@ -79,11 +83,11 @@
   ::aos::EventLoop *event_loop_;
   ::aos::Fetcher<::aos::RobotState> robot_state_fetcher_;
 
-  ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Position>
+  ::aos::Sender<::frc971::control_loops::drivetrain::Position>
       drivetrain_position_sender_;
-  ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Output>
+  ::aos::Fetcher<::frc971::control_loops::drivetrain::Output>
       drivetrain_output_fetcher_;
-  ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Status>
+  ::aos::Fetcher<::frc971::control_loops::drivetrain::Status>
       drivetrain_status_fetcher_;
   ::aos::Sender<::frc971::sensors::GyroReading> gyro_reading_sender_;
 
@@ -95,7 +99,8 @@
   ::Eigen::Matrix<double, 5, 1> state_ = ::Eigen::Matrix<double, 5, 1>::Zero();
   ::std::unique_ptr<
       StateFeedbackLoop<2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
-                        HybridKalman<2, 2, 2>>> velocity_drivetrain_;
+                        HybridKalman<2, 2, 2>>>
+      velocity_drivetrain_;
   double last_left_position_;
   double last_right_position_;
 
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
index 8234b1c..7fe5cad 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
@@ -105,9 +105,9 @@
 
 void LineFollowDrivetrain::SetGoal(
     ::aos::monotonic_clock::time_point now,
-    const ::frc971::control_loops::DrivetrainQueue::Goal &goal) {
+    const ::frc971::control_loops::drivetrain::Goal *goal) {
   // TODO(james): More properly calculate goal velocity from throttle.
-  goal_velocity_ = 4.0 * goal.throttle;
+  goal_velocity_ = 4.0 * goal->throttle();
   // The amount of time to freeze the target choice after the driver releases
   // the button. Depending on the current state, we vary how long this timeout
   // is so that we can avoid button glitches causing issues.
@@ -115,7 +115,7 @@
   // Freeze the target once the driver presses the button; if we haven't yet
   // confirmed a target when the driver presses the button, we will not do
   // anything and report not ready until we have a target.
-  if (goal.controller_type == 3) {
+  if (goal->controller_type() == drivetrain::ControllerType_LINE_FOLLOWER) {
     last_enable_ = now;
     // If we already acquired a target, we want to keep track if it.
     if (have_target_) {
@@ -130,11 +130,11 @@
   freeze_target_ = now <= freeze_delay + last_enable_;
   // Set an adjustment that lets the driver tweak the offset for where we place
   // the target left/right.
-  side_adjust_ = -goal.wheel * 0.1;
+  side_adjust_ = -goal->wheel() * 0.1;
 }
 
 bool LineFollowDrivetrain::SetOutput(
-    ::frc971::control_loops::DrivetrainQueue::Output *output) {
+    ::frc971::control_loops::drivetrain::OutputT *output) {
   // TODO(james): Account for voltage error terms, and/or provide driver with
   // ability to influence steering.
   if (output != nullptr && have_target_) {
@@ -257,18 +257,20 @@
   }
 }
 
-void LineFollowDrivetrain::PopulateStatus(
-    ::frc971::control_loops::DrivetrainQueue::Status *status) const {
-  status->line_follow_logging.frozen = freeze_target_;
-  status->line_follow_logging.have_target = have_target_;
-  status->line_follow_logging.x = target_pose_.abs_pos().x();
-  status->line_follow_logging.y = target_pose_.abs_pos().y();
-  status->line_follow_logging.theta = target_pose_.abs_theta();
-  status->line_follow_logging.offset = relative_pose_.rel_pos().y();
-  status->line_follow_logging.distance_to_target =
-      -relative_pose_.rel_pos().x();
-  status->line_follow_logging.goal_theta = controls_goal_(0, 0);
-  status->line_follow_logging.rel_theta = relative_pose_.rel_theta();
+flatbuffers::Offset<LineFollowLogging> LineFollowDrivetrain::PopulateStatus(
+    aos::Sender<drivetrain::Status>::Builder *builder) const {
+  LineFollowLogging::Builder line_follow_logging_builder =
+      builder->MakeBuilder<LineFollowLogging>();
+  line_follow_logging_builder.add_frozen(freeze_target_);
+  line_follow_logging_builder.add_have_target(have_target_);
+  line_follow_logging_builder.add_x(target_pose_.abs_pos().x());
+  line_follow_logging_builder.add_y(target_pose_.abs_pos().y());
+  line_follow_logging_builder.add_theta(target_pose_.abs_theta());
+  line_follow_logging_builder.add_offset(relative_pose_.rel_pos().y());
+  line_follow_logging_builder.add_distance_to_target(-relative_pose_.rel_pos().x());
+  line_follow_logging_builder.add_goal_theta(controls_goal_(0, 0));
+  line_follow_logging_builder.add_rel_theta(relative_pose_.rel_theta());
+  return line_follow_logging_builder.Finish();
 }
 
 }  // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain.h b/frc971/control_loops/drivetrain/line_follow_drivetrain.h
index 09b2080..1ebf64d 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain.h
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain.h
@@ -2,10 +2,15 @@
 #define FRC971_CONTROL_LOOPS_DRIVETRAIN_LINE_FOLLOW_DRIVETRAIN_H_
 #include "Eigen/Dense"
 
-#include "frc971/control_loops/pose.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
 #include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
 #include "frc971/control_loops/drivetrain/localizer.h"
+#include "frc971/control_loops/pose.h"
+#include "frc971/control_loops/profiled_subsystem_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_goal_generated.h"
 
 namespace frc971 {
 namespace control_loops {
@@ -29,7 +34,7 @@
   // a Target; the positive X-axis in the Pose's frame represents the direction
   // we want to go (we approach the pose from the left-half plane).
   void SetGoal(::aos::monotonic_clock::time_point now,
-               const ::frc971::control_loops::DrivetrainQueue::Goal &goal);
+               const ::frc971::control_loops::drivetrain::Goal *goal);
   // State: [x, y, theta, left_vel, right_vel]
   void Update(::aos::monotonic_clock::time_point now,
               const ::Eigen::Matrix<double, 5, 1> &state);
@@ -37,9 +42,11 @@
   // yet have a target to track into and so some other drivetrain should take
   // over.
   bool SetOutput(
-      ::frc971::control_loops::DrivetrainQueue::Output *output);
-  void PopulateStatus(
-      ::frc971::control_loops::DrivetrainQueue::Status *status) const;
+      ::frc971::control_loops::drivetrain::OutputT *output);
+
+  flatbuffers::Offset<LineFollowLogging> PopulateStatus(
+      aos::Sender<drivetrain::Status>::Builder *line_follow_logging_builder)
+      const;
 
  private:
   // Nominal max voltage.
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc b/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
index f4d718e..bd45e92 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
@@ -50,13 +50,21 @@
   }
 
   void Iterate() {
-    ::frc971::control_loops::DrivetrainQueue::Goal goal;
-    goal.throttle = driver_model_(state_);
-    goal.controller_type = freeze_target_ ? 3 : 0;
-    drivetrain_.SetGoal(t_, goal);
+    flatbuffers::FlatBufferBuilder fbb;
+    fbb.ForceDefaults(1);
+    Goal::Builder goal_builder(fbb);
+    goal_builder.add_throttle(driver_model_(state_));
+    goal_builder.add_controller_type(freeze_target_
+                                         ? ControllerType_LINE_FOLLOWER
+                                         : ControllerType_POLYDRIVE);
+    fbb.Finish(goal_builder.Finish());
+
+    aos::FlatbufferDetachedBuffer<Goal> goal(fbb.Release());
+
+    drivetrain_.SetGoal(t_, &goal.message());
     drivetrain_.Update(t_, state_);
 
-    ::frc971::control_loops::DrivetrainQueue::Output output;
+    ::frc971::control_loops::drivetrain::OutputT output;
     EXPECT_EQ(expect_output_, drivetrain_.SetOutput(&output));
     if (!expect_output_) {
       EXPECT_EQ(0.0, output.left_voltage);
@@ -96,9 +104,15 @@
   }
 
   double GoalTheta(double x, double y, double v, double throttle) {
-    ::frc971::control_loops::DrivetrainQueue::Goal goal;
-    goal.throttle = throttle;
-    drivetrain_.SetGoal(t_, goal);
+    flatbuffers::FlatBufferBuilder fbb;
+    fbb.ForceDefaults(1);
+    Goal::Builder goal_builder(fbb);
+    goal_builder.add_throttle(throttle);
+    fbb.Finish(goal_builder.Finish());
+
+    aos::FlatbufferDetachedBuffer<Goal> goal(fbb.Release());
+
+    drivetrain_.SetGoal(t_, &goal.message());
     ::Eigen::Matrix<double, 5, 1> state;
     state << x, y, 0.0, v, v;
     return drivetrain_.GoalTheta(state, y, throttle > 0.0 ? 1.0 : -1.0);
diff --git a/frc971/control_loops/drivetrain/localizer.fbs b/frc971/control_loops/drivetrain/localizer.fbs
new file mode 100644
index 0000000..5450c08
--- /dev/null
+++ b/frc971/control_loops/drivetrain/localizer.fbs
@@ -0,0 +1,13 @@
+namespace frc971.control_loops.drivetrain;
+
+// Allows you to reset the state of the localizer to a specific position on the
+// field.
+table LocalizerControl {
+  x:float;      // X position, meters
+  y:float;      // Y position, meters
+  theta:float;  // heading, radians
+  theta_uncertainty:double; // Uncertainty in theta.
+  keep_current_theta:bool; // Whether to keep the current theta value.
+}
+
+root_type LocalizerControl;
diff --git a/frc971/control_loops/drivetrain/localizer.h b/frc971/control_loops/drivetrain/localizer.h
index 7b2ceae..2589d4b 100644
--- a/frc971/control_loops/drivetrain/localizer.h
+++ b/frc971/control_loops/drivetrain/localizer.h
@@ -1,7 +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 "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"
diff --git a/frc971/control_loops/drivetrain/localizer.q b/frc971/control_loops/drivetrain/localizer.q
deleted file mode 100644
index 323f895..0000000
--- a/frc971/control_loops/drivetrain/localizer.q
+++ /dev/null
@@ -1,12 +0,0 @@
-package frc971.control_loops.drivetrain;
-
-// Allows you to reset the state of the localizer to a specific position on the
-// field.
-// Published on ".frc971.control_loops.drivetrain.localizer_control"
-message LocalizerControl {
-  float x;      // X position, meters
-  float y;      // Y position, meters
-  float theta;  // heading, radians
-  double theta_uncertainty; // Uncertainty in theta.
-  bool keep_current_theta; // Whether to keep the current theta value.
-};
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.cc b/frc971/control_loops/drivetrain/polydrivetrain.cc
index c213b06..a0d374e 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.cc
+++ b/frc971/control_loops/drivetrain/polydrivetrain.cc
@@ -1,14 +1,5 @@
 #include "frc971/control_loops/drivetrain/polydrivetrain.h"
 
-#include "aos/commonmath.h"
-#include "aos/controls/polytope.h"
-#include "frc971/control_loops/coerce_goal.h"
-#ifdef __linux__
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/control_loops/drivetrain/drivetrain_config.h"
-#endif  // __linux__
-#include "frc971/control_loops/state_feedback_loop.h"
-
 namespace frc971 {
 namespace control_loops {
 namespace drivetrain {
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.h b/frc971/control_loops/drivetrain/polydrivetrain.h
index f8f1a39..74fb2eb 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.h
+++ b/frc971/control_loops/drivetrain/polydrivetrain.h
@@ -7,13 +7,18 @@
 #include "frc971/control_loops/coerce_goal.h"
 #include "frc971/control_loops/drivetrain/gear.h"
 #ifdef __linux__
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
 #include "aos/logging/logging.h"
-#include "aos/logging/matrix_logging.h"
-#include "aos/logging/queue_logging.h"
-#include "aos/robot_state/robot_state.q.h"
+#include "aos/robot_state/robot_state_generated.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
 #else
-#include "frc971/control_loops/drivetrain/drivetrain_uc.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_float_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_float_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_position_float_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_float_generated.h"
 #endif  // __linux__
 #include "frc971/control_loops/state_feedback_loop.h"
 #include "frc971/control_loops/drivetrain/drivetrain_config.h"
@@ -35,10 +40,11 @@
   Scalar MotorSpeed(const constants::ShifterHallEffect &hall_effect,
                     Scalar shifter_position, Scalar velocity, Gear gear);
 
-  void SetGoal(const ::frc971::control_loops::DrivetrainQueue::Goal &goal);
+  void SetGoal(const Scalar wheel, const Scalar throttle, const bool quickturn,
+               const bool highgear);
 
   void SetPosition(
-      const ::frc971::control_loops::DrivetrainQueue::Position *position,
+      const ::frc971::control_loops::drivetrain::Position *position,
       Gear left_gear, Gear right_gear);
 
   Scalar FilterVelocity(Scalar throttle) const;
@@ -47,9 +53,10 @@
 
   void Update(Scalar voltage_battery);
 
-  void SetOutput(::frc971::control_loops::DrivetrainQueue::Output *output);
+  void SetOutput(::frc971::control_loops::drivetrain::OutputT *output);
 
-  void PopulateStatus(::frc971::control_loops::DrivetrainQueue::Status *status);
+  flatbuffers::Offset<CIMLogging> PopulateStatus(
+      flatbuffers::FlatBufferBuilder *fbb);
 
   // Computes the next state of a shifter given the current state and the
   // requested state.
@@ -81,8 +88,8 @@
   Gear left_gear_;
   Gear right_gear_;
 
-  ::frc971::control_loops::DrivetrainQueue::Position last_position_;
-  ::frc971::control_loops::DrivetrainQueue::Position position_;
+  ::frc971::control_loops::drivetrain::PositionT last_position_;
+  ::frc971::control_loops::drivetrain::PositionT position_;
   int counter_;
   DrivetrainConfig<Scalar> dt_config_;
 
@@ -123,10 +130,7 @@
       left_gear_(dt_config.default_high_gear ? Gear::HIGH : Gear::LOW),
       right_gear_(dt_config.default_high_gear ? Gear::HIGH : Gear::LOW),
       counter_(0),
-      dt_config_(dt_config) {
-  last_position_.Zero();
-  position_.Zero();
-}
+      dt_config_(dt_config) {}
 
 template <typename Scalar>
 Scalar PolyDrivetrain<Scalar>::MotorSpeed(
@@ -195,13 +199,9 @@
 }
 
 template <typename Scalar>
-void PolyDrivetrain<Scalar>::SetGoal(
-    const ::frc971::control_loops::DrivetrainQueue::Goal &goal) {
-  const Scalar wheel = goal.wheel;
-  const Scalar throttle = goal.throttle;
-  const bool quickturn = goal.quickturn;
-  const bool highgear = goal.highgear;
-
+void PolyDrivetrain<Scalar>::SetGoal(const Scalar wheel, const Scalar throttle,
+                                     const bool quickturn,
+                                     const bool highgear) {
   // Apply a sin function that's scaled to make it feel better.
   const Scalar angular_range =
       static_cast<Scalar>(M_PI_2) * dt_config_.wheel_non_linearity;
@@ -234,12 +234,12 @@
 
 template <typename Scalar>
 void PolyDrivetrain<Scalar>::SetPosition(
-    const ::frc971::control_loops::DrivetrainQueue::Position *position,
+    const ::frc971::control_loops::drivetrain::Position *position,
     Gear left_gear, Gear right_gear) {
   left_gear_ = left_gear;
   right_gear_ = right_gear;
   last_position_ = position_;
-  position_ = *position;
+  position->UnPackTo(&position_);
 }
 
 template <typename Scalar>
@@ -415,8 +415,8 @@
 
 template <typename Scalar>
 void PolyDrivetrain<Scalar>::SetOutput(
-    ::frc971::control_loops::DrivetrainQueue::Output *output) {
-  if (output != NULL) {
+    ::frc971::control_loops::drivetrain::OutputT *output) {
+  if (output != nullptr) {
     output->left_voltage = loop_->U(0, 0);
     output->right_voltage = loop_->U(1, 0);
     output->left_high = MaybeHigh(left_gear_);
@@ -425,19 +425,21 @@
 }
 
 template <typename Scalar>
-void PolyDrivetrain<Scalar>::PopulateStatus(
-    ::frc971::control_loops::DrivetrainQueue::Status *status) {
+flatbuffers::Offset<CIMLogging> PolyDrivetrain<Scalar>::PopulateStatus(
+    flatbuffers::FlatBufferBuilder *fbb) {
+  CIMLogging::Builder builder(*fbb);
 
-  status->cim_logging.left_in_gear = IsInGear(left_gear_);
-  status->cim_logging.left_motor_speed = left_motor_speed_;
-  status->cim_logging.left_velocity = current_left_velocity_;
+  builder.add_left_in_gear(IsInGear(left_gear_));
+  builder.add_left_motor_speed(left_motor_speed_);
+  builder.add_left_velocity(current_left_velocity_);
 
-  status->cim_logging.right_in_gear = IsInGear(right_gear_);
-  status->cim_logging.right_motor_speed = right_motor_speed_;
-  status->cim_logging.right_velocity = current_right_velocity_;
+  builder.add_right_in_gear(IsInGear(right_gear_));
+  builder.add_right_motor_speed(right_motor_speed_);
+  builder.add_right_velocity(current_right_velocity_);
+
+  return builder.Finish();
 }
 
-
 }  // namespace drivetrain
 }  // namespace control_loops
 }  // namespace frc971
diff --git a/frc971/control_loops/drivetrain/replay_drivetrain.cc b/frc971/control_loops/drivetrain/replay_drivetrain.cc
deleted file mode 100644
index 2f63c9e..0000000
--- a/frc971/control_loops/drivetrain/replay_drivetrain.cc
+++ /dev/null
@@ -1,38 +0,0 @@
-#include "aos/controls/replay_control_loop.h"
-#include "aos/init.h"
-
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/queues/gyro.q.h"
-
-// Reads one or more log files and sends out all the queue messages (in the
-// correct order and at the correct time) to feed a "live" drivetrain process.
-
-int main(int argc, char **argv) {
-  if (argc <= 1) {
-    fprintf(stderr, "Need at least one file to replay!\n");
-    return EXIT_FAILURE;
-  }
-
-  ::aos::InitNRT();
-
-  ::frc971::control_loops::DrivetrainQueue 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::controls::ControlLoopReplayer<
-        ::frc971::control_loops::DrivetrainQueue>
-        replayer(&drivetrain_queue, "drivetrain");
-
-    replayer.AddDirectQueueSender<::frc971::sensors::GyroReading>(
-        "wpilib_interface.Gyro", "sending", ".frc971.sensors.gyro_reading");
-    for (int i = 1; i < argc; ++i) {
-      replayer.ProcessFile(argv[i]);
-    }
-  }
-
-  ::aos::Cleanup();
-}
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index de59355..1222ae0 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -2,10 +2,13 @@
 
 #include "Eigen/Dense"
 
-#include "aos/init.h"
+#include "aos/realtime.h"
 #include "aos/util/math.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
 #include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
 
 namespace frc971 {
 namespace control_loops {
@@ -30,7 +33,7 @@
 
   ::aos::MutexLocker locker(&mutex_);
   while (run_) {
-    while (goal_.spline.spline_idx == future_spline_idx_) {
+    while (goal_.spline_idx == future_spline_idx_) {
       AOS_CHECK(!new_goal_.Wait());
       if (!run_) {
         return;
@@ -39,10 +42,10 @@
     past_distance_spline_.reset();
     past_trajectory_.reset();
 
-    plan_state_ = PlanState::kBuildingTrajectory;
-    const ::frc971::MultiSpline &multispline = goal_.spline;
-    future_spline_idx_ = multispline.spline_idx;
+    plan_state_ = PlanningState_BUILDING_TRAJECTORY;
+    future_spline_idx_ = goal_.spline_idx;
     planning_spline_idx_ = future_spline_idx_;
+    const MultiSpline &multispline = goal_.spline;
     auto x = multispline.spline_x;
     auto y = multispline.spline_y;
     ::std::vector<Spline> splines = ::std::vector<Spline>();
@@ -56,7 +59,7 @@
       splines.emplace_back(Spline(points));
     }
 
-    future_drive_spline_backwards_ = goal_.spline.drive_spline_backwards;
+    future_drive_spline_backwards_ = goal_.drive_spline_backwards;
 
     future_distance_spline_ = ::std::unique_ptr<DistanceSpline>(
         new DistanceSpline(::std::move(splines)));
@@ -65,47 +68,79 @@
         new Trajectory(future_distance_spline_.get(), dt_config_));
 
     for (size_t i = 0; i < multispline.constraints.size(); ++i) {
-      const ::frc971::Constraint &constraint = multispline.constraints[i];
+      const ::frc971::ConstraintT &constraint = multispline.constraints[i];
       switch (constraint.constraint_type) {
-        case 0:
+        case frc971::ConstraintType_CONSTRAINT_TYPE_UNDEFINED:
           break;
-        case 1:
+        case frc971::ConstraintType_LONGITUDINAL_ACCELERATION:
           future_trajectory_->set_longitudal_acceleration(constraint.value);
           break;
-        case 2:
+        case frc971::ConstraintType_LATERAL_ACCELERATION:
           future_trajectory_->set_lateral_acceleration(constraint.value);
           break;
-        case 3:
+        case frc971::ConstraintType_VOLTAGE:
           future_trajectory_->set_voltage_limit(constraint.value);
           break;
-        case 4:
+        case frc971::ConstraintType_VELOCITY:
           future_trajectory_->LimitVelocity(constraint.start_distance,
                                             constraint.end_distance,
                                             constraint.value);
           break;
       }
     }
-    plan_state_ = PlanState::kPlanningTrajectory;
+    plan_state_ = PlanningState_PLANNING_TRAJECTORY;
 
     future_trajectory_->Plan();
-    plan_state_ = PlanState::kPlannedTrajectory;
+    plan_state_ = PlanningState_PLANNED;
   }
 }
 
 void SplineDrivetrain::SetGoal(
-    const ::frc971::control_loops::DrivetrainQueue::Goal &goal) {
-  current_spline_handle_ = goal.spline_handle;
+    const ::frc971::control_loops::drivetrain::Goal *goal) {
+  current_spline_handle_ = goal->spline_handle();
+
   // If told to stop, set the executing spline to an invalid index and clear out
   // its plan:
   if (current_spline_handle_ == 0 &&
-      current_spline_idx_ != goal.spline.spline_idx) {
+      (goal->spline() == nullptr ||
+       current_spline_idx_ != CHECK_NOTNULL(goal->spline())->spline_idx())) {
     current_spline_idx_ = -1;
   }
 
   ::aos::Mutex::State mutex_state = mutex_.TryLock();
   if (mutex_state == ::aos::Mutex::State::kLocked) {
-    if (goal.spline.spline_idx && future_spline_idx_ != goal.spline.spline_idx) {
-      goal_ = goal;
+    if (goal->spline() != nullptr && goal->spline()->spline_idx() &&
+        future_spline_idx_ != goal->spline()->spline_idx()) {
+      CHECK_NOTNULL(goal->spline());
+      goal_.spline_idx = goal->spline()->spline_idx();
+      goal_.drive_spline_backwards = goal->spline()->drive_spline_backwards();
+
+      const frc971::MultiSpline *multispline = goal->spline()->spline();
+      CHECK_NOTNULL(multispline);
+
+      goal_.spline.spline_count = multispline->spline_count();
+
+      CHECK_EQ(multispline->spline_x()->size(),
+               static_cast<size_t>(goal_.spline.spline_count * 5 + 1));
+      CHECK_EQ(multispline->spline_y()->size(),
+               static_cast<size_t>(goal_.spline.spline_count * 5 + 1));
+
+      std::copy(multispline->spline_x()->begin(),
+                multispline->spline_x()->end(), goal_.spline.spline_x.begin());
+      std::copy(multispline->spline_y()->begin(),
+                multispline->spline_y()->end(), goal_.spline.spline_y.begin());
+
+      for (size_t i = 0; i < 6; ++i) {
+        if (multispline->constraints() != nullptr &&
+            i < multispline->constraints()->size()) {
+          multispline->constraints()->Get(i)->UnPackTo(
+              &goal_.spline.constraints[i]);
+        } else {
+          goal_.spline.constraints[i].constraint_type =
+              ConstraintType_CONSTRAINT_TYPE_UNDEFINED;
+        }
+      }
+
       new_goal_.Broadcast();
       if (current_spline_handle_ != current_spline_idx_) {
         // If we aren't going to actively execute the current spline, evict it's
@@ -178,7 +213,7 @@
 }
 
 void SplineDrivetrain::SetOutput(
-    ::frc971::control_loops::DrivetrainQueue::Output *output) {
+    ::frc971::control_loops::drivetrain::OutputT *output) {
   if (!output) {
     return;
   }
@@ -193,42 +228,52 @@
   output->right_voltage = next_U_(1);
 }
 
+
 void SplineDrivetrain::PopulateStatus(
-    ::frc971::control_loops::DrivetrainQueue::Status *status) const {
-  if (status && enable_) {
-    status->uncapped_left_voltage = uncapped_U_(0);
-    status->uncapped_right_voltage = uncapped_U_(1);
-    status->robot_speed = current_xva_(1);
-    status->output_was_capped = output_was_capped_;
+  drivetrain::Status::Builder *builder) const {
+  if (builder && enable_) {
+    builder->add_uncapped_left_voltage(uncapped_U_(0));
+    builder->add_uncapped_right_voltage(uncapped_U_(1));
+    builder->add_robot_speed(current_xva_(1));
+    builder->add_output_was_capped(output_was_capped_);
   }
+}
 
-  if (status) {
-    if (current_distance_spline_) {
-      ::Eigen::Matrix<double, 5, 1> goal_state = CurrentGoalState();
-      status->trajectory_logging.x = goal_state(0);
-      status->trajectory_logging.y = goal_state(1);
-      status->trajectory_logging.theta = ::aos::math::NormalizeAngle(
-          goal_state(2) + (current_drive_spline_backwards_ ? M_PI : 0.0));
-      status->trajectory_logging.left_velocity = goal_state(3);
-      status->trajectory_logging.right_velocity = goal_state(4);
-    }
-    status->trajectory_logging.planning_state = static_cast<int8_t>(plan_state_.load());
-    status->trajectory_logging.is_executing = !IsAtEnd() && has_started_execution_;
-    status->trajectory_logging.is_executed =
-        (current_spline_idx_ != -1) && IsAtEnd();
-    status->trajectory_logging.goal_spline_handle = current_spline_handle_;
-    status->trajectory_logging.current_spline_idx = current_spline_idx_;
-    status->trajectory_logging.distance_remaining =
-        current_trajectory_ ? current_trajectory_->length() - current_xva_.x()
-                            : 0.0;
-
-    int32_t planning_spline_idx = planning_spline_idx_;
-    if (current_spline_idx_ == planning_spline_idx) {
-      status->trajectory_logging.planning_spline_idx = 0;
-    } else {
-      status->trajectory_logging.planning_spline_idx = planning_spline_idx_;
-    }
+flatbuffers::Offset<TrajectoryLogging> SplineDrivetrain::MakeTrajectoryLogging(
+    flatbuffers::FlatBufferBuilder *builder) const {
+  drivetrain::TrajectoryLogging::Builder trajectory_logging_builder(*builder);
+  if (current_distance_spline_) {
+    ::Eigen::Matrix<double, 5, 1> goal_state = CurrentGoalState();
+    trajectory_logging_builder.add_x(goal_state(0));
+    trajectory_logging_builder.add_y(goal_state(1));
+    trajectory_logging_builder.add_theta(::aos::math::NormalizeAngle(
+        goal_state(2) + (current_drive_spline_backwards_ ? M_PI : 0.0)));
+    trajectory_logging_builder.add_left_velocity(goal_state(3));
+    trajectory_logging_builder.add_right_velocity(goal_state(4));
   }
+  trajectory_logging_builder.add_planning_state(plan_state_.load());
+  trajectory_logging_builder.add_is_executing(!IsAtEnd() &&
+                                              has_started_execution_);
+  trajectory_logging_builder.add_is_executed((current_spline_idx_ != -1) &&
+                                             IsAtEnd());
+  trajectory_logging_builder.add_goal_spline_handle(current_spline_handle_);
+  trajectory_logging_builder.add_current_spline_idx(current_spline_idx_);
+  trajectory_logging_builder.add_distance_remaining(
+      current_trajectory_ ? current_trajectory_->length() - current_xva_.x()
+                          : 0.0);
+
+  int32_t planning_spline_idx = planning_spline_idx_;
+  if (current_spline_idx_ == planning_spline_idx) {
+    trajectory_logging_builder.add_planning_spline_idx(0);
+  } else {
+    trajectory_logging_builder.add_planning_spline_idx(planning_spline_idx_);
+  }
+  return trajectory_logging_builder.Finish();
+}
+
+flatbuffers::Offset<TrajectoryLogging> SplineDrivetrain::MakeTrajectoryLogging(
+    aos::Sender<drivetrain::Status>::Builder *builder) const {
+  return MakeTrajectoryLogging(builder->fbb());
 }
 
 }  // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
index 98c4e74..4f21b29 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -8,9 +8,12 @@
 
 #include "aos/condition.h"
 #include "aos/mutex/mutex.h"
+#include "frc971/control_loops/control_loops_generated.h"
 #include "frc971/control_loops/drivetrain/distance_spline.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
 #include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
 #include "frc971/control_loops/drivetrain/spline.h"
 #include "frc971/control_loops/drivetrain/trajectory.h"
 
@@ -31,14 +34,18 @@
     worker_thread_.join();
   }
 
-  void SetGoal(const ::frc971::control_loops::DrivetrainQueue::Goal &goal);
+  void SetGoal(const ::frc971::control_loops::drivetrain::Goal *goal);
 
   void Update(bool enabled, const ::Eigen::Matrix<double, 5, 1> &state);
 
-  void SetOutput(
-      ::frc971::control_loops::DrivetrainQueue::Output *output);
+  void SetOutput(::frc971::control_loops::drivetrain::OutputT *output);
+
+  flatbuffers::Offset<TrajectoryLogging> MakeTrajectoryLogging(
+      aos::Sender<drivetrain::Status>::Builder *builder) const;
+  flatbuffers::Offset<TrajectoryLogging> MakeTrajectoryLogging(
+      flatbuffers::FlatBufferBuilder *builder) const;
   void PopulateStatus(
-      ::frc971::control_loops::DrivetrainQueue::Status *status) const;
+      drivetrain::Status::Builder *status) const;
 
   // Accessor for the current goal state, pretty much only present for debugging
   // purposes.
@@ -55,6 +62,9 @@
               true;
   }
 
+  // Returns true if the splinedrivetrain is enabled.
+  bool enable() const { return enable_; }
+
   enum class PlanState : int8_t {
     kNoPlan = 0,
     kBuildingTrajectory = 1,
@@ -85,7 +95,7 @@
   bool enable_ = false;
   bool output_was_capped_ = false;
 
-  std::atomic<PlanState> plan_state_ = {PlanState::kNoPlan};
+  std::atomic<PlanningState> plan_state_ = {PlanningState_NO_PLAN};
 
   ::std::thread worker_thread_;
   // mutex_ is held by the worker thread while it is doing work or by the main
@@ -95,7 +105,26 @@
   ::aos::Condition new_goal_;
   // The following variables are guarded by mutex_.
   bool run_ = true;
-  ::frc971::control_loops::DrivetrainQueue::Goal goal_;
+
+  // These two structures mirror the flatbuffer Multispline.
+  // TODO(austin): copy the goal flatbuffer directly instead of recreating it
+  // like this...
+  struct MultiSpline {
+    int32_t spline_count;
+    std::array<float, 36> spline_x;
+    std::array<float, 36> spline_y;
+    std::array<ConstraintT, 6> constraints;
+  };
+
+  struct SplineGoal {
+    int32_t spline_idx = 0;
+
+    bool drive_spline_backwards;
+
+    MultiSpline spline;
+  };
+
+  SplineGoal goal_;
   ::std::unique_ptr<DistanceSpline> past_distance_spline_;
   ::std::unique_ptr<DistanceSpline> future_distance_spline_;
   ::std::unique_ptr<Trajectory> past_trajectory_;
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.cc b/frc971/control_loops/drivetrain/ssdrivetrain.cc
index 5924eb1..cd29e39 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.cc
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.cc
@@ -2,11 +2,12 @@
 
 #include "aos/commonmath.h"
 #include "aos/controls/polytope.h"
-#include "aos/logging/matrix_logging.h"
 
 #include "frc971/control_loops/coerce_goal.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
 #include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
 #include "frc971/control_loops/state_feedback_loop.h"
 
 namespace frc971 {
@@ -60,8 +61,6 @@
 
   const Eigen::Matrix<double, 7, 1> error = kf_->R() - kf_->X_hat();
 
-  AOS_LOG_MATRIX(DEBUG, "U_uncapped", *U);
-
   Eigen::Matrix<double, 2, 2> position_K;
   position_K << kf_->controller().K(0, 0), kf_->controller().K(0, 2),
       kf_->controller().K(1, 0), kf_->controller().K(1, 2);
@@ -75,7 +74,6 @@
   const auto drive_error = T_inverse_ * position_error;
   Eigen::Matrix<double, 2, 1> velocity_error;
   velocity_error << error(1, 0), error(3, 0);
-  AOS_LOG_MATRIX(DEBUG, "error", error);
 
   Eigen::Matrix<double, 2, 1> U_integral;
   U_integral << kf_->X_hat(4, 0), kf_->X_hat(5, 0);
@@ -144,23 +142,31 @@
 }
 
 void DrivetrainMotorsSS::SetGoal(
-    const ::frc971::control_loops::DrivetrainQueue::Goal &goal) {
-  unprofiled_goal_ << goal.left_goal, 0.0, goal.right_goal, 0.0, 0.0, 0.0, 0.0;
-  if (::std::abs(goal.max_ss_voltage) < 0.01) {
+    const ::frc971::control_loops::drivetrain::Goal *goal) {
+  unprofiled_goal_ << goal->left_goal(), 0.0, goal->right_goal(), 0.0, 0.0, 0.0,
+      0.0;
+  if (!goal->has_max_ss_voltage()) {
     max_voltage_ = kMaxVoltage;
   } else {
-    max_voltage_ = goal.max_ss_voltage;
+    max_voltage_ = goal->has_max_ss_voltage();
   }
 
-  use_profile_ =
-      !kf_->controller().Kff().isZero(0) &&
-      (goal.linear.max_velocity != 0.0 && goal.linear.max_acceleration != 0.0 &&
-       goal.angular.max_velocity != 0.0 &&
-       goal.angular.max_acceleration != 0.0);
-  linear_profile_.set_maximum_velocity(goal.linear.max_velocity);
-  linear_profile_.set_maximum_acceleration(goal.linear.max_acceleration);
-  angular_profile_.set_maximum_velocity(goal.angular.max_velocity);
-  angular_profile_.set_maximum_acceleration(goal.angular.max_acceleration);
+  use_profile_ = !kf_->controller().Kff().isZero(0) &&
+                 (goal->has_linear() && goal->has_angular() &&
+                  goal->linear()->has_max_velocity() &&
+                  goal->linear()->has_max_acceleration() &&
+                  goal->angular()->has_max_velocity() &&
+                  goal->angular()->has_max_acceleration());
+  if (goal->has_linear()) {
+    linear_profile_.set_maximum_velocity(goal->linear()->max_velocity());
+    linear_profile_.set_maximum_acceleration(
+        goal->linear()->max_acceleration());
+  }
+  if (goal->has_angular()) {
+    angular_profile_.set_maximum_velocity(goal->angular()->max_velocity());
+    angular_profile_.set_maximum_acceleration(
+        goal->angular()->max_acceleration());
+  }
 }
 
 void DrivetrainMotorsSS::Update(bool enable_control_loop) {
@@ -255,7 +261,7 @@
 }
 
 void DrivetrainMotorsSS::SetOutput(
-    ::frc971::control_loops::DrivetrainQueue::Output *output) const {
+    ::frc971::control_loops::drivetrain::OutputT *output) const {
   if (output) {
     output->left_voltage = kf_->U(0, 0);
     output->right_voltage = kf_->U(1, 0);
@@ -265,7 +271,7 @@
 }
 
 void DrivetrainMotorsSS::PopulateStatus(
-    ::frc971::control_loops::DrivetrainQueue::Status *status) const {
+    ::frc971::control_loops::drivetrain::StatusBuilder *builder) const {
   Eigen::Matrix<double, 2, 1> profiled_linear =
       dt_config_.LeftRightToLinear(kf_->next_R());
   Eigen::Matrix<double, 2, 1> profiled_angular =
@@ -276,10 +282,10 @@
   Eigen::Matrix<double, 4, 1> profiled_gyro_left_right =
       dt_config_.AngularLinearToLeftRight(profiled_linear, profiled_angular);
 
-  status->profiled_left_position_goal = profiled_gyro_left_right(0, 0);
-  status->profiled_left_velocity_goal = profiled_gyro_left_right(1, 0);
-  status->profiled_right_position_goal = profiled_gyro_left_right(2, 0);
-  status->profiled_right_velocity_goal = profiled_gyro_left_right(3, 0);
+  builder->add_profiled_left_position_goal(profiled_gyro_left_right(0, 0));
+  builder->add_profiled_left_velocity_goal(profiled_gyro_left_right(1, 0));
+  builder->add_profiled_right_position_goal(profiled_gyro_left_right(2, 0));
+  builder->add_profiled_right_velocity_goal(profiled_gyro_left_right(3, 0));
 }
 
 }  // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.h b/frc971/control_loops/drivetrain/ssdrivetrain.h
index 762cbc2..4d91807 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.h
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.h
@@ -4,14 +4,16 @@
 #include "aos/commonmath.h"
 #include "aos/controls/control_loop.h"
 #include "aos/controls/polytope.h"
-#include "aos/logging/matrix_logging.h"
 #include "aos/util/trapezoid_profile.h"
 
-#include "frc971/control_loops/state_feedback_loop.h"
 #include "frc971/control_loops/coerce_goal.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
 #include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
 #include "frc971/control_loops/drivetrain/localizer.h"
+#include "frc971/control_loops/state_feedback_loop.h"
 
 namespace frc971 {
 namespace control_loops {
@@ -23,7 +25,7 @@
                      StateFeedbackLoop<7, 2, 4> *kf,
                      LocalizerInterface *localizer);
 
-  void SetGoal(const ::frc971::control_loops::DrivetrainQueue::Goal &goal);
+  void SetGoal(const ::frc971::control_loops::drivetrain::Goal *goal);
 
   // Computes the power to send out as part of the controller.  Should be called
   // when disabled (with enable_control_loop false) so the profiles get computed
@@ -34,10 +36,9 @@
 
   bool output_was_capped() const { return output_was_capped_; }
 
-  void SetOutput(
-      ::frc971::control_loops::DrivetrainQueue::Output *output) const;
+  void SetOutput(::frc971::control_loops::drivetrain::OutputT *output) const;
   void PopulateStatus(
-      ::frc971::control_loops::DrivetrainQueue::Status *status) const;
+      ::frc971::control_loops::drivetrain::StatusBuilder *builder) const;
 
  private:
   void PolyCapU(Eigen::Matrix<double, 2, 1> *U);
diff --git a/frc971/control_loops/drivetrain/trajectory.cc b/frc971/control_loops/drivetrain/trajectory.cc
index 9d35a5e..ba4c0c2 100644
--- a/frc971/control_loops/drivetrain/trajectory.cc
+++ b/frc971/control_loops/drivetrain/trajectory.cc
@@ -3,7 +3,6 @@
 #include <chrono>
 
 #include "Eigen/Dense"
-#include "aos/logging/matrix_logging.h"
 #include "frc971/control_loops/c2d.h"
 #include "frc971/control_loops/dlqr.h"
 #include "frc971/control_loops/drivetrain/distance_spline.h"
@@ -327,9 +326,7 @@
   ::Eigen::Matrix<double, 2, 5> K = ::Eigen::Matrix<double, 2, 5>::Zero();
 
   int info = ::frc971::controls::dlqr<5, 2>(A, B, Q, R, &K, &S);
-  if (info == 0) {
-    AOS_LOG_MATRIX(INFO, "K", K);
-  } else {
+  if (info != 0) {
     AOS_LOG(ERROR, "Failed to solve %d, controllability: %d\n", info,
             controls::Controllability(A, B));
     // TODO(austin): Can we be more clever here?  Use the last one?  We should
diff --git a/frc971/control_loops/drivetrain/trajectory_plot.cc b/frc971/control_loops/drivetrain/trajectory_plot.cc
index 0b35ee8..2a990f9 100644
--- a/frc971/control_loops/drivetrain/trajectory_plot.cc
+++ b/frc971/control_loops/drivetrain/trajectory_plot.cc
@@ -3,7 +3,6 @@
 #include <chrono>
 
 #include "aos/logging/implementations.h"
-#include "aos/logging/matrix_logging.h"
 #include "aos/network/team_number.h"
 #include "aos/time/time.h"
 #include "frc971/control_loops/dlqr.h"