diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index 3d6c930..17613ab 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -1,6 +1,6 @@
 package(default_visibility = ["//visibility:public"])
 
-load("//aos/build:queues.bzl", "queue_library")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
 load("//tools:environments.bzl", "mcu_cpus")
 
 cc_library(
@@ -34,7 +34,7 @@
     hdrs = ["pose.h"],
     deps = [
         "//aos/util:math",
-        "//third_party/eigen",
+        "@org_tuxfamily_eigen//:eigen",
     ],
 )
 
@@ -53,15 +53,16 @@
         "hall_effect_tracker.h",
     ],
     deps = [
-        ":queues",
+        ":control_loops_fbs",
     ],
 )
 
-queue_library(
-    name = "queues",
+flatbuffer_cc_library(
+    name = "control_loops_fbs",
     srcs = [
-        "control_loops.q",
+        "control_loops.fbs",
     ],
+    compatible_with = mcu_cpus,
 )
 
 cc_test(
@@ -70,8 +71,8 @@
         "position_sensor_sim_test.cc",
     ],
     deps = [
+        ":control_loops_fbs",
         ":position_sensor_sim",
-        ":queues",
         "//aos/logging",
         "//aos/testing:googletest",
     ],
@@ -90,8 +91,8 @@
         "-lm",
     ],
     deps = [
+        ":control_loops_fbs",
         ":gaussian_noise",
-        ":queues",
         "//aos/testing:random_seed",
     ],
 )
@@ -120,7 +121,7 @@
     restricted_to = mcu_cpus,
     deps = [
         "//aos/controls:polytope_uc",
-        "//third_party/eigen",
+        "@org_tuxfamily_eigen//:eigen",
     ],
 )
 
@@ -137,7 +138,7 @@
     ],
     deps = [
         "//aos/controls:polytope",
-        "//third_party/eigen",
+        "@org_tuxfamily_eigen//:eigen",
     ],
 )
 
@@ -151,7 +152,7 @@
     restricted_to = mcu_cpus,
     deps = [
         "//aos:macros",
-        "//third_party/eigen",
+        "@org_tuxfamily_eigen//:eigen",
     ],
 )
 
@@ -163,7 +164,7 @@
     deps = [
         "//aos:macros",
         "//aos/logging",
-        "//third_party/eigen",
+        "@org_tuxfamily_eigen//:eigen",
     ],
 )
 
@@ -178,7 +179,7 @@
         "//aos:macros",
         "//aos/controls:control_loop",
         "//aos/logging",
-        "//third_party/eigen",
+        "@org_tuxfamily_eigen//:eigen",
     ],
 )
 
@@ -189,7 +190,7 @@
     ],
     deps = [
         ":state_feedback_loop",
-        "//third_party/eigen",
+        "@org_tuxfamily_eigen//:eigen",
     ],
 )
 
@@ -199,7 +200,7 @@
         "runge_kutta.h",
     ],
     deps = [
-        "//third_party/eigen",
+        "@org_tuxfamily_eigen//:eigen",
     ],
 )
 
@@ -211,7 +212,7 @@
     deps = [
         ":runge_kutta",
         "//aos/testing:googletest",
-        "//third_party/eigen",
+        "@org_tuxfamily_eigen//:eigen",
     ],
 )
 
@@ -233,24 +234,13 @@
     ],
 )
 
-queue_library(
-    name = "profiled_subsystem_queue",
+flatbuffer_cc_library(
+    name = "profiled_subsystem_fbs",
     srcs = [
-        "profiled_subsystem.q",
+        "profiled_subsystem.fbs",
     ],
-    deps = [
-        ":queues",
-    ],
-)
-
-queue_library(
-    name = "static_zeroing_single_dof_profiled_subsystem_test_queue",
-    srcs = [
-        "static_zeroing_single_dof_profiled_subsystem_test.q",
-    ],
-    deps = [
-        ":profiled_subsystem_queue",
-        ":queues",
+    includes = [
+        ":control_loops_fbs_includes",
     ],
 )
 
@@ -263,7 +253,8 @@
         "profiled_subsystem.h",
     ],
     deps = [
-        ":profiled_subsystem_queue",
+        ":control_loops_fbs",
+        ":profiled_subsystem_fbs",
         ":simple_capped_state_feedback_loop",
         ":state_feedback_loop",
         "//aos/controls:control_loop",
@@ -278,7 +269,7 @@
         "jacobian.h",
     ],
     deps = [
-        "//third_party/eigen",
+        "@org_tuxfamily_eigen//:eigen",
     ],
 )
 
@@ -290,7 +281,7 @@
     deps = [
         ":jacobian",
         "//aos/testing:googletest",
-        "//third_party/eigen",
+        "@org_tuxfamily_eigen//:eigen",
     ],
 )
 
@@ -315,7 +306,7 @@
     visibility = ["//visibility:public"],
     deps = [
         "//aos/time",
-        "//third_party/eigen",
+        "@org_tuxfamily_eigen//:eigen",
     ],
 )
 
@@ -326,7 +317,7 @@
     ],
     visibility = ["//visibility:public"],
     deps = [
-        "//third_party/eigen",
+        "@org_tuxfamily_eigen//:eigen",
         "@slycot_repo//:slicot",
     ],
 )
@@ -408,6 +399,17 @@
     ],
 )
 
+flatbuffer_cc_library(
+    name = "static_zeroing_single_dof_profiled_subsystem_test_fbs",
+    srcs = [
+        "static_zeroing_single_dof_profiled_subsystem_test.fbs",
+    ],
+    includes = [
+        ":control_loops_fbs_includes",
+        ":profiled_subsystem_fbs_includes",
+    ],
+)
+
 cc_test(
     name = "static_zeroing_single_dof_profiled_subsystem_test",
     srcs = [
@@ -417,8 +419,8 @@
         ":capped_test_plant",
         ":position_sensor_sim",
         ":static_zeroing_single_dof_profiled_subsystem",
+        ":static_zeroing_single_dof_profiled_subsystem_test_fbs",
         ":static_zeroing_single_dof_profiled_subsystem_test_plants",
-        ":static_zeroing_single_dof_profiled_subsystem_test_queue",
         "//aos/controls:control_loop_test",
         "//aos/testing:googletest",
     ],
diff --git a/frc971/control_loops/coerce_goal.h b/frc971/control_loops/coerce_goal.h
index 6c1f504..6e927b0 100644
--- a/frc971/control_loops/coerce_goal.h
+++ b/frc971/control_loops/coerce_goal.h
@@ -76,7 +76,7 @@
   } else {
     Eigen::Matrix<Scalar, 2, 4> region_vertices = region.StaticVertices();
 #ifdef __linux__
-    AOS_CHECK_GT(region_vertices.outerSize(), 0);
+    CHECK_GT(reinterpret_cast<ssize_t>(region_vertices.outerSize()), 0);
 #else
     assert(region_vertices.outerSize() > 0);
 #endif
diff --git a/frc971/control_loops/control_loops.q b/frc971/control_loops/control_loops.fbs
similarity index 65%
rename from frc971/control_loops/control_loops.q
rename to frc971/control_loops/control_loops.fbs
index 2359bcd..4f1e888 100644
--- a/frc971/control_loops/control_loops.q
+++ b/frc971/control_loops/control_loops.fbs
@@ -1,4 +1,4 @@
-package frc971;
+namespace frc971;
 
 // Represents all of the data for a single indexed encoder. In other words,
 // just a relative encoder with an index pulse.
@@ -6,14 +6,14 @@
 // All encoder values are relative to where the encoder was at some arbitrary
 // point in time. All potentiometer values are relative to some arbitrary 0
 // position which varies with each robot.
-struct IndexPosition {
+table IndexPosition {
   // Current position read from the encoder.
-  double encoder;
+  encoder:double;
   // Position from the encoder latched at the last index pulse.
-  double latched_encoder;
+  latched_encoder:double;
   // How many index pulses we've seen since startup. Starts at 0.
-  uint32_t index_pulses;
-};
+  index_pulses:uint;
+}
 
 // Represents all of the data for a single potentiometer and indexed encoder
 // pair.
@@ -21,20 +21,20 @@
 // All encoder values are relative to where the encoder was at some arbitrary
 // point in time. All potentiometer values are relative to some arbitrary 0
 // position which varies with each robot.
-struct PotAndIndexPosition {
+table PotAndIndexPosition {
   // Current position read from the encoder.
-  double encoder;
+  encoder:double;
   // Current position read from the potentiometer.
-  double pot;
+  pot:double;
 
   // Position from the encoder latched at the last index pulse.
-  double latched_encoder;
+  latched_encoder:double;
   // Position from the potentiometer latched at the last index pulse.
-  double latched_pot;
+  latched_pot:double;
 
   // How many index pulses we've seen since startup. Starts at 0.
-  uint32_t index_pulses;
-};
+  index_pulses:uint;
+}
 
 // Represents all of the data for a single potentiometer with an absolute and
 // relative encoder pair.
@@ -42,182 +42,183 @@
 // The relative encoder values are relative to where the encoder was at some
 // arbitrary point in time. All potentiometer values are relative to some
 // arbitrary 0 position which varies with each robot.
-struct PotAndAbsolutePosition {
+table PotAndAbsolutePosition {
   // Current position read from each encoder.
-  double encoder;
-  double absolute_encoder;
+  encoder:double;
+  absolute_encoder:double;
 
   // Current position read from the potentiometer.
-  double pot;
-};
+  pot:double;
+}
 
 // Represents all of the data for an absolute and relative encoder pair.
 // The units on all of the positions are the same.
 // The relative encoder values are relative to where the encoder was at some
 // arbitrary point in time.
-struct AbsolutePosition {
+table AbsolutePosition {
   // Current position read from each encoder.
-  double encoder;
-  double absolute_encoder;
-};
+  encoder:double;
+  absolute_encoder:double;
+}
 
 // The internal state of a zeroing estimator.
-struct EstimatorState {
+table EstimatorState {
   // If true, there has been a fatal error for the estimator.
-  bool error;
+  error:bool;
   // If the joint has seen an index pulse and is zeroed.
-  bool zeroed;
+  zeroed:bool;
   // The estimated position of the joint.
-  double position;
+  position:double;
 
   // The estimated position not using the index pulse.
-  double pot_position;
-};
+  pot_position:double;
+}
 
 // The internal state of a zeroing estimator.
-struct PotAndAbsoluteEncoderEstimatorState {
+table PotAndAbsoluteEncoderEstimatorState {
   // If true, there has been a fatal error for the estimator.
-  bool error;
+  error:bool;
   // If the joint has seen an index pulse and is zeroed.
-  bool zeroed;
+  zeroed:bool;
   // The estimated position of the joint.
-  double position;
+  position:double;
 
   // The estimated position not using the index pulse.
-  double pot_position;
+  pot_position:double;
 
   // The estimated absolute position of the encoder.  This is filtered, so it
   // can be easily used when zeroing.
-  double absolute_position;
-};
+  absolute_position:double;
+}
 
 // The internal state of a zeroing estimator.
-struct AbsoluteEncoderEstimatorState {
+table AbsoluteEncoderEstimatorState {
   // If true, there has been a fatal error for the estimator.
-  bool error;
+  error:bool;
   // If the joint has seen an index pulse and is zeroed.
-  bool zeroed;
+  zeroed:bool;
   // The estimated position of the joint.
-  double position;
+  position:double;
 
   // The estimated absolute position of the encoder.  This is filtered, so it
   // can be easily used when zeroing.
-  double absolute_position;
-};
+  absolute_position:double;
+}
 
 // The internal state of a zeroing estimator.
-struct IndexEstimatorState {
+table IndexEstimatorState {
   // If true, there has been a fatal error for the estimator.
-  bool error;
+  error:bool;
   // If the joint has seen an index pulse and is zeroed.
-  bool zeroed;
+  zeroed:bool;
   // The estimated position of the joint. This is just the position relative to
   // where we started if we're not zeroed yet.
-  double position;
+  position:double;
 
   // The positions of the extreme index pulses we've seen.
-  double min_index_position;
-  double max_index_position;
+  min_index_position:double;
+  max_index_position:double;
   // The number of index pulses we've seen.
-  int32_t index_pulses_seen;
-};
+  index_pulses_seen:int;
+}
 
-struct HallEffectAndPositionEstimatorState {
+table HallEffectAndPositionEstimatorState {
   // If error.
-  bool error;
+  error:bool;
   // If we've found a positive edge while moving backwards and is zeroed.
-  bool zeroed;
+  zeroed:bool;
   // Encoder angle relative to where we started.
-  double encoder;
+  encoder:double;
   // The positions of the extreme posedges we've seen.
   // If we've gotten enough samples where the hall effect is high before can be
   // certain it is not a false positive.
-  bool high_long_enough;
-  double offset;
-};
+  high_long_enough:bool;
+  offset:double;
+}
 
 // A left/right pair of PotAndIndexPositions.
-struct PotAndIndexPair {
-  PotAndIndexPosition left;
-  PotAndIndexPosition right;
-};
+table PotAndIndexPair {
+  left:PotAndIndexPosition;
+  right:PotAndIndexPosition;
+}
 
 // Records edges captured on a single hall effect sensor.
-struct HallEffectStruct {
-  bool current;
-  int32_t posedge_count;
-  int32_t negedge_count;
-  double posedge_value;
-  double negedge_value;
-};
+table HallEffectStruct {
+  current:bool;
+  posedge_count:int;
+  negedge_count:int;
+  posedge_value:double;
+  negedge_value:double;
+}
 
 // Records the hall effect sensor and encoder values.
-struct HallEffectAndPosition {
+table HallEffectAndPosition {
   // The current hall effect state.
-  bool current;
+  current:bool;
   // The current encoder position.
-  double encoder;
+  encoder:double;
   // The number of positive and negative edges we've seen on the hall effect
   // sensor.
-  int32_t posedge_count;
-  int32_t negedge_count;
+  posedge_count:int;
+  negedge_count:int;
   // The values corresponding to the last hall effect sensor reading.
-  double posedge_value;
-  double negedge_value;
-};
+  posedge_value:double;
+  negedge_value:double;
+}
 
 // Records the positions for a mechanism with edge-capturing sensors on it.
-struct HallEffectPositions {
-  double current;
-  double posedge;
-  double negedge;
-};
+table HallEventPositions {
+  current:double;
+  posedge:double;
+  negedge:double;
+}
 
 // Records edges captured on a single hall effect sensor.
-struct PosedgeOnlyCountedHallEffectStruct {
-  bool current;
-  int32_t posedge_count;
-  int32_t negedge_count;
-  double posedge_value;
-};
+table PosedgeOnlyCountedHallEffectStruct {
+  current:bool;
+  posedge_count:int;
+  negedge_count:int;
+  posedge_value:double;
+}
 
 // Parameters for the motion profiles.
-struct ProfileParameters {
+table ProfileParameters {
   // Maximum velocity for the profile.
-  float max_velocity;
+  max_velocity:float;
   // Maximum acceleration for the profile.
-  float max_acceleration;
-};
+  max_acceleration:float;
+}
+
+enum ConstraintType : byte {
+  CONSTRAINT_TYPE_UNDEFINED,
+  LONGITUDINAL_ACCELERATION,
+  LATERAL_ACCELERATION,
+  VOLTAGE,
+  VELOCITY,
+}
 
 // Definition of a constraint on a trajectory
-struct Constraint {
-  // Type of constraint
-  //  0: Null constraint. Ignore and all following
-  //  1: longitual acceleration
-  //  2: lateral acceleration
-  //  3: voltage
-  //  4: velocity
-  uint8_t constraint_type;
-  float value;
+table Constraint {
+  constraint_type:ConstraintType;
+
+  value:float;
+
   // start and end distance are only checked for velocity limits.
-  float start_distance;
-  float end_distance;
-};
+  start_distance:float;
+  end_distance:float;
+}
 
 // Parameters for computing a trajectory using a chain of splines and
 // constraints.
-struct MultiSpline {
-  // index of the spline. Zero indicates the spline should not be computed.
-  int32_t spline_idx;
+table MultiSpline {
   // Number of splines. The spline point arrays will be expected to have
   // 6 + 5 * (n - 1) points in them. The endpoints are shared between
   // neighboring splines.
-  uint8_t spline_count;
-  float[36] spline_x;
-  float[36] spline_y;
+  spline_count:byte;
+  // Maximum of 36 spline points (7 splines).
+  spline_x:[float];
+  spline_y:[float];
 
-  // Whether to follow the spline driving forwards or backwards.
-  bool drive_spline_backwards;
-
-  Constraint[6] constraints;
-};
+  // Maximum of 6 constraints;
+  constraints:[Constraint];
+}
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"
diff --git a/frc971/control_loops/hall_effect_tracker.h b/frc971/control_loops/hall_effect_tracker.h
index d1b6efe..084c9a7 100644
--- a/frc971/control_loops/hall_effect_tracker.h
+++ b/frc971/control_loops/hall_effect_tracker.h
@@ -3,7 +3,7 @@
 
 #include <stdint.h>
 
-#include "frc971/control_loops/control_loops.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
 
 namespace frc971 {
 
@@ -27,22 +27,22 @@
   double posedge_value() const { return posedge_value_; }
   double negedge_value() const { return negedge_value_; }
 
-  void Update(const HallEffectStruct &position) {
+  void Update(const HallEffectStruct *position) {
     last_value_ = value_;
-    value_ = position.current;
-    posedge_value_ = position.posedge_value;
-    negedge_value_ = position.negedge_value;
-    posedges_.update(position.posedge_count);
-    negedges_.update(position.negedge_count);
+    value_ = position->current();
+    posedge_value_ = position->posedge_value();
+    negedge_value_ = position->negedge_value();
+    posedges_.update(position->posedge_count());
+    negedges_.update(position->negedge_count());
   }
 
-  void Reset(const HallEffectStruct &position) {
-    posedges_.Reset(position.posedge_count);
-    negedges_.Reset(position.negedge_count);
-    value_ = position.current;
-    last_value_ = position.current;
-    posedge_value_ = position.posedge_value;
-    negedge_value_ = position.negedge_value;
+  void Reset(const HallEffectStruct *position) {
+    posedges_.Reset(position->posedge_count());
+    negedges_.Reset(position->negedge_count());
+    value_ = position->current();
+    last_value_ = position->current();
+    posedge_value_ = position->posedge_value();
+    negedge_value_ = position->negedge_value();
   }
 
  private:
diff --git a/frc971/control_loops/position_sensor_sim.cc b/frc971/control_loops/position_sensor_sim.cc
index c875a0b..612801e 100644
--- a/frc971/control_loops/position_sensor_sim.cc
+++ b/frc971/control_loops/position_sensor_sim.cc
@@ -127,72 +127,95 @@
   current_position_ = new_position;
 }
 
-void PositionSensorSimulator::GetSensorValues(IndexPosition *values) {
-  values->encoder = current_position_ - start_position_;
+template <>
+flatbuffers::Offset<IndexPosition>
+PositionSensorSimulator::GetSensorValues<IndexPositionBuilder>(
+    IndexPositionBuilder *builder) {
+  builder->add_encoder(current_position_ - start_position_);
 
-  values->index_pulses = lower_index_edge_.index_count();
-  if (values->index_pulses == 0) {
-    values->latched_encoder = 0.0;
+  const int index_count = lower_index_edge_.index_count();
+  builder->add_index_pulses(index_count);
+  if (index_count == 0) {
+    builder->add_latched_encoder(0.0);
   } else {
     // Populate the latched encoder samples.
-    values->latched_encoder =
-        lower_index_edge_.IndexPulsePosition() - start_position_;
+    builder->add_latched_encoder(lower_index_edge_.IndexPulsePosition() -
+                                 start_position_);
   }
+  return builder->Finish();
 }
 
-void PositionSensorSimulator::GetSensorValues(PotAndIndexPosition *values) {
-  values->pot = lower_index_edge_.mutable_pot_noise()->AddNoiseToSample(
-      current_position_);
-  values->encoder = current_position_ - start_position_;
+template <>
+flatbuffers::Offset<PotAndIndexPosition>
+PositionSensorSimulator::GetSensorValues<PotAndIndexPositionBuilder>(
+    PotAndIndexPositionBuilder *builder) {
+  builder->add_pot(lower_index_edge_.mutable_pot_noise()->AddNoiseToSample(
+      current_position_));
+  builder->add_encoder(current_position_ - start_position_);
 
   if (lower_index_edge_.index_count() == 0) {
-    values->latched_pot = 0.0;
-    values->latched_encoder = 0.0;
+    builder->add_latched_pot(0.0);
+    builder->add_latched_encoder(0.0);
   } else {
     // Populate the latched pot/encoder samples.
-    values->latched_pot = lower_index_edge_.latched_pot();
-    values->latched_encoder =
-        lower_index_edge_.IndexPulsePosition() - start_position_;
+    builder->add_latched_pot(lower_index_edge_.latched_pot());
+    builder->add_latched_encoder(lower_index_edge_.IndexPulsePosition() -
+                                 start_position_);
   }
 
-  values->index_pulses = lower_index_edge_.index_count();
+  builder->add_index_pulses(lower_index_edge_.index_count());
+  return builder->Finish();
 }
 
-void PositionSensorSimulator::GetSensorValues(PotAndAbsolutePosition *values) {
-  values->pot = lower_index_edge_.mutable_pot_noise()->AddNoiseToSample(
-      current_position_);
-  values->encoder = current_position_ - start_position_;
+template <>
+flatbuffers::Offset<PotAndAbsolutePosition>
+PositionSensorSimulator::GetSensorValues<PotAndAbsolutePositionBuilder>(
+    PotAndAbsolutePositionBuilder *builder) {
+  builder->add_pot(lower_index_edge_.mutable_pot_noise()->AddNoiseToSample(
+      current_position_));
+  builder->add_encoder(current_position_ - start_position_);
   // TODO(phil): Create some lag here since this is a PWM signal it won't be
   // instantaneous like the other signals. Better yet, its lag varies
   // randomly with the distribution varying depending on the reading.
-  values->absolute_encoder = ::std::remainder(
+  double absolute_encoder = ::std::remainder(
       current_position_ + known_absolute_encoder_, distance_per_revolution_);
-  if (values->absolute_encoder < 0) {
-    values->absolute_encoder += distance_per_revolution_;
+  if (absolute_encoder < 0) {
+    absolute_encoder += distance_per_revolution_;
   }
+  builder->add_absolute_encoder(absolute_encoder);
+  return builder->Finish();
 }
 
-void PositionSensorSimulator::GetSensorValues(AbsolutePosition *values) {
-  values->encoder = current_position_ - start_position_;
+template <>
+flatbuffers::Offset<AbsolutePosition>
+PositionSensorSimulator::GetSensorValues<AbsolutePositionBuilder>(
+    AbsolutePositionBuilder *builder) {
+  builder->add_encoder(current_position_ - start_position_);
   // TODO(phil): Create some lag here since this is a PWM signal it won't be
   // instantaneous like the other signals. Better yet, its lag varies
   // randomly with the distribution varying depending on the reading.
-  values->absolute_encoder = ::std::remainder(
+  double absolute_encoder = ::std::remainder(
       current_position_ + known_absolute_encoder_, distance_per_revolution_);
-  if (values->absolute_encoder < 0) {
-    values->absolute_encoder += distance_per_revolution_;
+  if (absolute_encoder < 0) {
+    absolute_encoder += distance_per_revolution_;
   }
+  builder->add_absolute_encoder(absolute_encoder);
+  return builder->Finish();
 }
 
-void PositionSensorSimulator::GetSensorValues(HallEffectAndPosition *values) {
-  values->current = lower_index_edge_.current_index_segment() !=
-                    upper_index_edge_.current_index_segment();
-  values->encoder = current_position_ - start_position_;
+template <>
+flatbuffers::Offset<HallEffectAndPosition>
+PositionSensorSimulator::GetSensorValues<HallEffectAndPositionBuilder>(
+    HallEffectAndPositionBuilder *builder) {
+  builder->add_current(lower_index_edge_.current_index_segment() !=
+                       upper_index_edge_.current_index_segment());
+  builder->add_encoder(current_position_ - start_position_);
 
-  values->posedge_count = posedge_count_;
-  values->negedge_count = negedge_count_;
-  values->posedge_value = posedge_value_ - start_position_;
-  values->negedge_value = negedge_value_ - start_position_;
+  builder->add_posedge_count(posedge_count_);
+  builder->add_negedge_count(negedge_count_);
+  builder->add_posedge_value(posedge_value_ - start_position_);
+  builder->add_negedge_value(negedge_value_ - start_position_);
+  return builder->Finish();
 }
 
 }  // namespace control_loops
diff --git a/frc971/control_loops/position_sensor_sim.h b/frc971/control_loops/position_sensor_sim.h
index 2880429..6be6bbc 100644
--- a/frc971/control_loops/position_sensor_sim.h
+++ b/frc971/control_loops/position_sensor_sim.h
@@ -1,9 +1,11 @@
 #ifndef FRC971_CONTROL_LOOPS_POSITION_SENSOR_SIM_H_
 #define FRC971_CONTROL_LOOPS_POSITION_SENSOR_SIM_H_
 
+#include "flatbuffers/flatbuffers.h"
+
 #include "aos/testing/random_seed.h"
 
-#include "frc971/control_loops/control_loops.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
 #include "frc971/control_loops/gaussian_noise.h"
 
 namespace frc971 {
@@ -58,11 +60,17 @@
   // values: The target structure will be populated with simulated sensor
   //         readings. The readings will be in SI units. For example the units
   //         can be given in radians, meters, etc.
-  void GetSensorValues(IndexPosition *values);
-  void GetSensorValues(PotAndIndexPosition *values);
-  void GetSensorValues(AbsolutePosition *values);
-  void GetSensorValues(PotAndAbsolutePosition *values);
-  void GetSensorValues(HallEffectAndPosition *values);
+  template <typename PositionBuilder>
+  flatbuffers::Offset<typename PositionBuilder::Table> GetSensorValues(
+      PositionBuilder *builder);
+  template <typename Position>
+  const Position *FillSensorValues(flatbuffers::FlatBufferBuilder *fbb) {
+    fbb->Clear();
+    typename Position::Builder values(*fbb);
+
+    fbb->Finish(GetSensorValues(&values));
+    return flatbuffers::GetRoot<Position>(fbb->GetBufferPointer());
+  }
 
  private:
   // It turns out that we can re-use a lot of the same logic to count the index
@@ -84,7 +92,7 @@
       index_count_ = 0;
     }
 
-    double index_count() const { return index_count_; }
+    int index_count() const { return index_count_; }
     double latched_pot() const { return latched_pot_; }
     int current_index_segment() const { return current_index_segment_; }
 
diff --git a/frc971/control_loops/position_sensor_sim_test.cc b/frc971/control_loops/position_sensor_sim_test.cc
index a4436ce..93a648a 100644
--- a/frc971/control_loops/position_sensor_sim_test.cc
+++ b/frc971/control_loops/position_sensor_sim_test.cc
@@ -5,10 +5,12 @@
 #include <random>
 
 #include "gtest/gtest.h"
-#include "frc971/control_loops/control_loops.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
 #include "frc971/control_loops/position_sensor_sim.h"
 #include "aos/die.h"
 
+#include "flatbuffers/flatbuffers.h"
+
 namespace frc971 {
 namespace control_loops {
 
@@ -23,46 +25,51 @@
   // this test is to verify that no false index pulses are generated while the
   // mechanism stays between two index pulses.
   const double index_diff = 0.5;
-  IndexPosition index_position;
-  PotAndIndexPosition pot_and_index_position;
+  flatbuffers::FlatBufferBuilder fbb;
+  flatbuffers::FlatBufferBuilder pot_fbb;
   PositionSensorSimulator sim(index_diff);
   sim.Initialize(3.6 * index_diff, 0);
 
   // Make sure that we don't accidentally hit an index pulse.
   for (int i = 0; i < 30; i++) {
     sim.MoveTo(3.6 * index_diff);
-    sim.GetSensorValues(&index_position);
-    sim.GetSensorValues(&pot_and_index_position);
-    ASSERT_DOUBLE_EQ(3.6 * index_diff, pot_and_index_position.pot);
-    ASSERT_EQ(0u, pot_and_index_position.index_pulses);
-    ASSERT_EQ(0u, index_position.index_pulses);
+    const IndexPosition *index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+    const PotAndIndexPosition *pot_and_index_position =
+        sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+    ASSERT_DOUBLE_EQ(3.6 * index_diff, pot_and_index_position->pot());
+    ASSERT_EQ(0u, pot_and_index_position->index_pulses());
+
+    ASSERT_EQ(0u, index_position->index_pulses());
   }
 
   for (int i = 0; i < 30; i++) {
     sim.MoveTo(3.0 * index_diff);
-    sim.GetSensorValues(&index_position);
-    sim.GetSensorValues(&pot_and_index_position);
-    ASSERT_DOUBLE_EQ(3.0 * index_diff, pot_and_index_position.pot);
-    ASSERT_EQ(0u, pot_and_index_position.index_pulses);
-    ASSERT_EQ(0u, index_position.index_pulses);
+    const IndexPosition *index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+    const PotAndIndexPosition *pot_and_index_position =
+        sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+    ASSERT_DOUBLE_EQ(3.0 * index_diff, pot_and_index_position->pot());
+    ASSERT_EQ(0u, pot_and_index_position->index_pulses());
+    ASSERT_EQ(0u, index_position->index_pulses());
   }
 
   for (int i = 0; i < 30; i++) {
     sim.MoveTo(3.99 * index_diff);
-    sim.GetSensorValues(&index_position);
-    sim.GetSensorValues(&pot_and_index_position);
-    ASSERT_DOUBLE_EQ(3.99 * index_diff, pot_and_index_position.pot);
-    ASSERT_EQ(0u, pot_and_index_position.index_pulses);
-    ASSERT_EQ(0u, index_position.index_pulses);
+    const IndexPosition *index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+    const PotAndIndexPosition *pot_and_index_position =
+        sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+    ASSERT_DOUBLE_EQ(3.99 * index_diff, pot_and_index_position->pot());
+    ASSERT_EQ(0u, pot_and_index_position->index_pulses());
+    ASSERT_EQ(0u, index_position->index_pulses());
   }
 
   for (int i = 0; i < 30; i++) {
     sim.MoveTo(3.0 * index_diff);
-    sim.GetSensorValues(&index_position);
-    sim.GetSensorValues(&pot_and_index_position);
-    ASSERT_DOUBLE_EQ(3.0 * index_diff, pot_and_index_position.pot);
-    ASSERT_EQ(0u, pot_and_index_position.index_pulses);
-    ASSERT_EQ(0u, index_position.index_pulses);
+    const IndexPosition *index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+    const PotAndIndexPosition *pot_and_index_position =
+        sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+    ASSERT_DOUBLE_EQ(3.0 * index_diff, pot_and_index_position->pot());
+    ASSERT_EQ(0u, pot_and_index_position->index_pulses());
+    ASSERT_EQ(0u, index_position->index_pulses());
   }
 }
 
@@ -72,58 +79,59 @@
   // again simulate zero noise on the potentiometer to accurately verify the
   // mechanism's position during the index pulses.
   const double index_diff = 0.8;
-  IndexPosition index_position;
-  PotAndIndexPosition pot_and_index_position;
+  flatbuffers::FlatBufferBuilder fbb;
+  flatbuffers::FlatBufferBuilder pot_fbb;
   PositionSensorSimulator sim(index_diff);
   sim.Initialize(4.6 * index_diff, 0);
 
   // Make sure that we get an index pulse on every transition.
-  sim.GetSensorValues(&index_position);
-  sim.GetSensorValues(&pot_and_index_position);
-  ASSERT_EQ(0u, index_position.index_pulses);
-  ASSERT_EQ(0u, pot_and_index_position.index_pulses);
+  const IndexPosition *index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+  const PotAndIndexPosition *pot_and_index_position =
+      sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+  ASSERT_EQ(0u, index_position->index_pulses());
+  ASSERT_EQ(0u, pot_and_index_position->index_pulses());
 
   sim.MoveTo(3.6 * index_diff);
-  sim.GetSensorValues(&index_position);
-  sim.GetSensorValues(&pot_and_index_position);
-  ASSERT_DOUBLE_EQ(4.0 * index_diff, pot_and_index_position.latched_pot);
-  ASSERT_EQ(1u, index_position.index_pulses);
-  ASSERT_EQ(1u, pot_and_index_position.index_pulses);
+  index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+  pot_and_index_position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+  ASSERT_DOUBLE_EQ(4.0 * index_diff, pot_and_index_position->latched_pot());
+  ASSERT_EQ(1u, index_position->index_pulses());
+  ASSERT_EQ(1u, pot_and_index_position->index_pulses());
 
   sim.MoveTo(4.5 * index_diff);
-  sim.GetSensorValues(&index_position);
-  sim.GetSensorValues(&pot_and_index_position);
-  ASSERT_DOUBLE_EQ(4.0 * index_diff, pot_and_index_position.latched_pot);
-  ASSERT_EQ(2u, index_position.index_pulses);
-  ASSERT_EQ(2u, pot_and_index_position.index_pulses);
+  index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+  pot_and_index_position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+  ASSERT_DOUBLE_EQ(4.0 * index_diff, pot_and_index_position->latched_pot());
+  ASSERT_EQ(2u, index_position->index_pulses());
+  ASSERT_EQ(2u, pot_and_index_position->index_pulses());
 
   sim.MoveTo(5.9 * index_diff);
-  sim.GetSensorValues(&index_position);
-  sim.GetSensorValues(&pot_and_index_position);
-  ASSERT_DOUBLE_EQ(5.0 * index_diff, pot_and_index_position.latched_pot);
-  ASSERT_EQ(3u, index_position.index_pulses);
-  ASSERT_EQ(3u, pot_and_index_position.index_pulses);
+  index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+  pot_and_index_position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+  ASSERT_DOUBLE_EQ(5.0 * index_diff, pot_and_index_position->latched_pot());
+  ASSERT_EQ(3u, index_position->index_pulses());
+  ASSERT_EQ(3u, pot_and_index_position->index_pulses());
 
   sim.MoveTo(6.1 * index_diff);
-  sim.GetSensorValues(&index_position);
-  sim.GetSensorValues(&pot_and_index_position);
-  ASSERT_DOUBLE_EQ(6.0 * index_diff, pot_and_index_position.latched_pot);
-  ASSERT_EQ(4u, index_position.index_pulses);
-  ASSERT_EQ(4u, pot_and_index_position.index_pulses);
+  index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+  pot_and_index_position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+  ASSERT_DOUBLE_EQ(6.0 * index_diff, pot_and_index_position->latched_pot());
+  ASSERT_EQ(4u, index_position->index_pulses());
+  ASSERT_EQ(4u, pot_and_index_position->index_pulses());
 
   sim.MoveTo(8.7 * index_diff);
-  sim.GetSensorValues(&index_position);
-  sim.GetSensorValues(&pot_and_index_position);
-  ASSERT_DOUBLE_EQ(8.0 * index_diff, pot_and_index_position.latched_pot);
-  ASSERT_EQ(5u, index_position.index_pulses);
-  ASSERT_EQ(5u, pot_and_index_position.index_pulses);
+  index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+  pot_and_index_position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+  ASSERT_DOUBLE_EQ(8.0 * index_diff, pot_and_index_position->latched_pot());
+  ASSERT_EQ(5u, index_position->index_pulses());
+  ASSERT_EQ(5u, pot_and_index_position->index_pulses());
 
   sim.MoveTo(7.3 * index_diff);
-  sim.GetSensorValues(&index_position);
-  sim.GetSensorValues(&pot_and_index_position);
-  ASSERT_DOUBLE_EQ(8.0 * index_diff, pot_and_index_position.latched_pot);
-  ASSERT_EQ(6u, index_position.index_pulses);
-  ASSERT_EQ(6u, pot_and_index_position.index_pulses);
+  index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+  pot_and_index_position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+  ASSERT_DOUBLE_EQ(8.0 * index_diff, pot_and_index_position->latched_pot());
+  ASSERT_EQ(6u, index_position->index_pulses());
+  ASSERT_EQ(6u, pot_and_index_position->index_pulses());
 }
 
 // Tests that the simulator handles non-zero specified index pulse locations
@@ -132,65 +140,66 @@
   const double index_diff = 0.5;
   PositionSensorSimulator sim(index_diff);
   sim.Initialize(index_diff * 0.25, 0.0, index_diff * 0.5);
-  IndexPosition index_position;
-  PotAndIndexPosition pot_and_index_position;
+  flatbuffers::FlatBufferBuilder fbb;
+  flatbuffers::FlatBufferBuilder pot_fbb;
 
   sim.MoveTo(0.75 * index_diff);
-  sim.GetSensorValues(&index_position);
-  sim.GetSensorValues(&pot_and_index_position);
-  EXPECT_EQ(1u, index_position.index_pulses);
-  EXPECT_EQ(1u, pot_and_index_position.index_pulses);
-  EXPECT_DOUBLE_EQ(index_diff * 0.5, pot_and_index_position.latched_pot);
-  EXPECT_DOUBLE_EQ(index_diff * 0.25, index_position.latched_encoder);
-  EXPECT_DOUBLE_EQ(index_diff * 0.25, pot_and_index_position.latched_encoder);
+  const IndexPosition *index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+  const PotAndIndexPosition *pot_and_index_position =
+      sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+  EXPECT_EQ(1u, index_position->index_pulses());
+  EXPECT_EQ(1u, pot_and_index_position->index_pulses());
+  EXPECT_DOUBLE_EQ(index_diff * 0.5, pot_and_index_position->latched_pot());
+  EXPECT_DOUBLE_EQ(index_diff * 0.25, index_position->latched_encoder());
+  EXPECT_DOUBLE_EQ(index_diff * 0.25, pot_and_index_position->latched_encoder());
 
   sim.MoveTo(index_diff);
-  sim.GetSensorValues(&index_position);
-  sim.GetSensorValues(&pot_and_index_position);
-  EXPECT_EQ(1u, index_position.index_pulses);
-  EXPECT_EQ(1u, pot_and_index_position.index_pulses);
-  EXPECT_DOUBLE_EQ(index_diff * 0.5, pot_and_index_position.latched_pot);
-  EXPECT_DOUBLE_EQ(index_diff * 0.25, index_position.latched_encoder);
-  EXPECT_DOUBLE_EQ(index_diff * 0.25, pot_and_index_position.latched_encoder);
+  index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+  pot_and_index_position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+  EXPECT_EQ(1u, index_position->index_pulses());
+  EXPECT_EQ(1u, pot_and_index_position->index_pulses());
+  EXPECT_DOUBLE_EQ(index_diff * 0.5, pot_and_index_position->latched_pot());
+  EXPECT_DOUBLE_EQ(index_diff * 0.25, index_position->latched_encoder());
+  EXPECT_DOUBLE_EQ(index_diff * 0.25, pot_and_index_position->latched_encoder());
 
   sim.MoveTo(1.75 * index_diff);
-  sim.GetSensorValues(&index_position);
-  sim.GetSensorValues(&pot_and_index_position);
-  EXPECT_EQ(2u, index_position.index_pulses);
-  EXPECT_EQ(2u, pot_and_index_position.index_pulses);
-  EXPECT_DOUBLE_EQ(index_diff * 1.5, pot_and_index_position.latched_pot);
-  EXPECT_DOUBLE_EQ(index_diff * 1.25, index_position.latched_encoder);
-  EXPECT_DOUBLE_EQ(index_diff * 1.25, pot_and_index_position.latched_encoder);
+  index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+  pot_and_index_position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+  EXPECT_EQ(2u, index_position->index_pulses());
+  EXPECT_EQ(2u, pot_and_index_position->index_pulses());
+  EXPECT_DOUBLE_EQ(index_diff * 1.5, pot_and_index_position->latched_pot());
+  EXPECT_DOUBLE_EQ(index_diff * 1.25, index_position->latched_encoder());
+  EXPECT_DOUBLE_EQ(index_diff * 1.25, pot_and_index_position->latched_encoder());
 
   // Try it with our known index pulse not being our first one.
   sim.Initialize(index_diff * 0.25, 0.0, index_diff * 1.5);
 
   sim.MoveTo(0.75 * index_diff);
-  sim.GetSensorValues(&index_position);
-  sim.GetSensorValues(&pot_and_index_position);
-  EXPECT_EQ(1u, index_position.index_pulses);
-  EXPECT_EQ(1u, pot_and_index_position.index_pulses);
-  EXPECT_DOUBLE_EQ(index_diff * 0.5, pot_and_index_position.latched_pot);
-  EXPECT_DOUBLE_EQ(index_diff * 0.25, index_position.latched_encoder);
-  EXPECT_DOUBLE_EQ(index_diff * 0.25, pot_and_index_position.latched_encoder);
+  index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+  pot_and_index_position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+  EXPECT_EQ(1u, index_position->index_pulses());
+  EXPECT_EQ(1u, pot_and_index_position->index_pulses());
+  EXPECT_DOUBLE_EQ(index_diff * 0.5, pot_and_index_position->latched_pot());
+  EXPECT_DOUBLE_EQ(index_diff * 0.25, index_position->latched_encoder());
+  EXPECT_DOUBLE_EQ(index_diff * 0.25, pot_and_index_position->latched_encoder());
 
   sim.MoveTo(index_diff);
-  sim.GetSensorValues(&index_position);
-  sim.GetSensorValues(&pot_and_index_position);
-  EXPECT_EQ(1u, index_position.index_pulses);
-  EXPECT_EQ(1u, pot_and_index_position.index_pulses);
-  EXPECT_DOUBLE_EQ(index_diff * 0.5, pot_and_index_position.latched_pot);
-  EXPECT_DOUBLE_EQ(index_diff * 0.25, index_position.latched_encoder);
-  EXPECT_DOUBLE_EQ(index_diff * 0.25, pot_and_index_position.latched_encoder);
+  index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+  pot_and_index_position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+  EXPECT_EQ(1u, index_position->index_pulses());
+  EXPECT_EQ(1u, pot_and_index_position->index_pulses());
+  EXPECT_DOUBLE_EQ(index_diff * 0.5, pot_and_index_position->latched_pot());
+  EXPECT_DOUBLE_EQ(index_diff * 0.25, index_position->latched_encoder());
+  EXPECT_DOUBLE_EQ(index_diff * 0.25, pot_and_index_position->latched_encoder());
 
   sim.MoveTo(1.75 * index_diff);
-  sim.GetSensorValues(&index_position);
-  sim.GetSensorValues(&pot_and_index_position);
-  EXPECT_EQ(2u, index_position.index_pulses);
-  EXPECT_EQ(2u, pot_and_index_position.index_pulses);
-  EXPECT_DOUBLE_EQ(index_diff * 1.5, pot_and_index_position.latched_pot);
-  EXPECT_DOUBLE_EQ(index_diff * 1.25, index_position.latched_encoder);
-  EXPECT_DOUBLE_EQ(index_diff * 1.25, pot_and_index_position.latched_encoder);
+  index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+  pot_and_index_position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+  EXPECT_EQ(2u, index_position->index_pulses());
+  EXPECT_EQ(2u, pot_and_index_position->index_pulses());
+  EXPECT_DOUBLE_EQ(index_diff * 1.5, pot_and_index_position->latched_pot());
+  EXPECT_DOUBLE_EQ(index_diff * 1.25, index_position->latched_encoder());
+  EXPECT_DOUBLE_EQ(index_diff * 1.25, pot_and_index_position->latched_encoder());
 }
 
 // Tests that the latched values update correctly.
@@ -198,44 +207,45 @@
   const double index_diff = 0.5;
   PositionSensorSimulator sim(index_diff);
   sim.Initialize(0, 0.25);
-  PotAndIndexPosition position;
+  flatbuffers::FlatBufferBuilder pot_fbb;
 
   sim.MoveTo(0.75 * index_diff);
-  sim.GetSensorValues(&position);
-  EXPECT_EQ(0u, position.index_pulses);
+  const PotAndIndexPosition *position =
+      sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+  EXPECT_EQ(0u, position->index_pulses());
 
   sim.MoveTo(1.75 * index_diff);
-  sim.GetSensorValues(&position);
-  EXPECT_EQ(1u, position.index_pulses);
-  EXPECT_NEAR(index_diff, position.latched_pot, 0.75);
-  EXPECT_DOUBLE_EQ(index_diff, position.latched_encoder);
-  const double first_latched_pot = position.latched_pot;
+  position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+  EXPECT_EQ(1u, position->index_pulses());
+  EXPECT_NEAR(index_diff, position->latched_pot(), 0.75);
+  EXPECT_DOUBLE_EQ(index_diff, position->latched_encoder());
+  const double first_latched_pot = position->latched_pot();
 
   sim.MoveTo(1.95 * index_diff);
-  sim.GetSensorValues(&position);
-  EXPECT_EQ(1u, position.index_pulses);
-  EXPECT_NEAR(index_diff, position.latched_pot, 0.75);
-  EXPECT_DOUBLE_EQ(first_latched_pot, position.latched_pot);
-  EXPECT_DOUBLE_EQ(index_diff, position.latched_encoder);
+  position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+  EXPECT_EQ(1u, position->index_pulses());
+  EXPECT_NEAR(index_diff, position->latched_pot(), 0.75);
+  EXPECT_DOUBLE_EQ(first_latched_pot, position->latched_pot());
+  EXPECT_DOUBLE_EQ(index_diff, position->latched_encoder());
 
   sim.MoveTo(2.05 * index_diff);
-  sim.GetSensorValues(&position);
-  EXPECT_EQ(2u, position.index_pulses);
-  EXPECT_NEAR(index_diff * 2, position.latched_pot, 0.75);
-  EXPECT_DOUBLE_EQ(index_diff * 2, position.latched_encoder);
+  position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+  EXPECT_EQ(2u, position->index_pulses());
+  EXPECT_NEAR(index_diff * 2, position->latched_pot(), 0.75);
+  EXPECT_DOUBLE_EQ(index_diff * 2, position->latched_encoder());
 
   sim.MoveTo(1.95 * index_diff);
-  sim.GetSensorValues(&position);
-  EXPECT_EQ(3u, position.index_pulses);
-  EXPECT_NEAR(index_diff * 2, position.latched_pot, 0.75);
-  EXPECT_DOUBLE_EQ(index_diff * 2, position.latched_encoder);
+  position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+  EXPECT_EQ(3u, position->index_pulses());
+  EXPECT_NEAR(index_diff * 2, position->latched_pot(), 0.75);
+  EXPECT_DOUBLE_EQ(index_diff * 2, position->latched_encoder());
 
   sim.MoveTo(0.95 * index_diff);
-  sim.GetSensorValues(&position);
-  EXPECT_EQ(4u, position.index_pulses);
-  EXPECT_NEAR(index_diff, position.latched_pot, 0.75);
-  EXPECT_GT(::std::abs(first_latched_pot - position.latched_pot), 0.005);
-  EXPECT_DOUBLE_EQ(index_diff, position.latched_encoder);
+  position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+  EXPECT_EQ(4u, position->index_pulses());
+  EXPECT_NEAR(index_diff, position->latched_pot(), 0.75);
+  EXPECT_GT(::std::abs(first_latched_pot - position->latched_pot()), 0.005);
+  EXPECT_DOUBLE_EQ(index_diff, position->latched_encoder());
 }
 
 // This test makes sure that our simulation for an absolute encoder + relative
@@ -263,42 +273,43 @@
   const double index_diff = 0.1;
   PositionSensorSimulator sim(index_diff);
   sim.Initialize(0.20, 0.05, 0.2, 0.07);
-  PotAndAbsolutePosition position;
+  flatbuffers::FlatBufferBuilder pot_fbb;
 
   sim.MoveTo(0.20);
-  sim.GetSensorValues(&position);
-  EXPECT_DOUBLE_EQ(0.00, position.encoder);
-  EXPECT_NEAR(0.07, position.absolute_encoder, 0.00000001);
+  const PotAndAbsolutePosition *position =
+      sim.FillSensorValues<PotAndAbsolutePosition>(&pot_fbb);
+  EXPECT_DOUBLE_EQ(0.00, position->encoder());
+  EXPECT_NEAR(0.07, position->absolute_encoder(), 0.00000001);
 
   sim.MoveTo(0.30);
-  sim.GetSensorValues(&position);
-  EXPECT_DOUBLE_EQ(0.10, position.encoder);
-  EXPECT_NEAR(0.07, position.absolute_encoder, 0.00000001);
+  position = sim.FillSensorValues<PotAndAbsolutePosition>(&pot_fbb);
+  EXPECT_DOUBLE_EQ(0.10, position->encoder());
+  EXPECT_NEAR(0.07, position->absolute_encoder(), 0.00000001);
 
   sim.MoveTo(0.40);
-  sim.GetSensorValues(&position);
-  EXPECT_DOUBLE_EQ(0.20, position.encoder);
-  EXPECT_NEAR(0.07, position.absolute_encoder, 0.00000001);
+  position = sim.FillSensorValues<PotAndAbsolutePosition>(&pot_fbb);
+  EXPECT_DOUBLE_EQ(0.20, position->encoder());
+  EXPECT_NEAR(0.07, position->absolute_encoder(), 0.00000001);
 
   sim.MoveTo(0.34);
-  sim.GetSensorValues(&position);
-  EXPECT_DOUBLE_EQ(0.14, position.encoder);
-  EXPECT_NEAR(0.01, position.absolute_encoder, 0.00000001);
+  position = sim.FillSensorValues<PotAndAbsolutePosition>(&pot_fbb);
+  EXPECT_DOUBLE_EQ(0.14, position->encoder());
+  EXPECT_NEAR(0.01, position->absolute_encoder(), 0.00000001);
 
   sim.MoveTo(0.24);
-  sim.GetSensorValues(&position);
-  EXPECT_DOUBLE_EQ(0.04, position.encoder);
-  EXPECT_NEAR(0.01, position.absolute_encoder, 0.00000001);
+  position = sim.FillSensorValues<PotAndAbsolutePosition>(&pot_fbb);
+  EXPECT_DOUBLE_EQ(0.04, position->encoder());
+  EXPECT_NEAR(0.01, position->absolute_encoder(), 0.00000001);
 
   sim.MoveTo(0.23);
-  sim.GetSensorValues(&position);
-  EXPECT_DOUBLE_EQ(0.03, position.encoder);
-  EXPECT_NEAR(0.00, position.absolute_encoder, 0.00000001);
+  position = sim.FillSensorValues<PotAndAbsolutePosition>(&pot_fbb);
+  EXPECT_DOUBLE_EQ(0.03, position->encoder());
+  EXPECT_NEAR(0.00, position->absolute_encoder(), 0.00000001);
 
   sim.MoveTo(0.13);
-  sim.GetSensorValues(&position);
-  EXPECT_DOUBLE_EQ(-0.07, position.encoder);
-  EXPECT_NEAR(0.00, position.absolute_encoder, 0.00000001);
+  position = sim.FillSensorValues<PotAndAbsolutePosition>(&pot_fbb);
+  EXPECT_DOUBLE_EQ(-0.07, position->encoder());
+  EXPECT_NEAR(0.00, position->absolute_encoder(), 0.00000001);
 
   // TODO(philipp): Test negative values.
 }
@@ -309,78 +320,80 @@
   const double index_diff = 1.0;
   PositionSensorSimulator sim(index_diff);
   sim.InitializeHallEffectAndPosition(-0.25, 0.0, 0.5);
-  HallEffectAndPosition position;
+  //HallEffectAndPosition position;
+  flatbuffers::FlatBufferBuilder fbb;
 
   // Go over only the lower edge rising.
   sim.MoveTo(0.25);
-  sim.GetSensorValues(&position);
-  EXPECT_TRUE(position.current);
-  EXPECT_DOUBLE_EQ(0.50, position.encoder);
-  EXPECT_EQ(1, position.posedge_count);
-  EXPECT_EQ(0.25, position.posedge_value);
-  EXPECT_EQ(0, position.negedge_count);
-  EXPECT_EQ(0, position.negedge_value);
+  const HallEffectAndPosition *position =
+      sim.FillSensorValues<HallEffectAndPosition>(&fbb);
+  EXPECT_TRUE(position->current());
+  EXPECT_DOUBLE_EQ(0.50, position->encoder());
+  EXPECT_EQ(1, position->posedge_count());
+  EXPECT_EQ(0.25, position->posedge_value());
+  EXPECT_EQ(0, position->negedge_count());
+  EXPECT_EQ(0, position->negedge_value());
 
   // Now, go over the upper edge, falling.
   sim.MoveTo(0.75);
-  sim.GetSensorValues(&position);
-  EXPECT_FALSE(position.current);
-  EXPECT_DOUBLE_EQ(1.0, position.encoder);
-  EXPECT_EQ(1, position.posedge_count);
-  EXPECT_DOUBLE_EQ(0.25, position.posedge_value);
-  EXPECT_EQ(1, position.negedge_count);
-  EXPECT_DOUBLE_EQ(0.75, position.negedge_value);
+  position = sim.FillSensorValues<HallEffectAndPosition>(&fbb);
+  EXPECT_FALSE(position->current());
+  EXPECT_DOUBLE_EQ(1.0, position->encoder());
+  EXPECT_EQ(1, position->posedge_count());
+  EXPECT_DOUBLE_EQ(0.25, position->posedge_value());
+  EXPECT_EQ(1, position->negedge_count());
+  EXPECT_DOUBLE_EQ(0.75, position->negedge_value());
 
   // Now, jump a whole cycle.
   sim.MoveTo(1.75);
-  sim.GetSensorValues(&position);
-  EXPECT_FALSE(position.current);
-  EXPECT_DOUBLE_EQ(2.0, position.encoder);
-  EXPECT_EQ(2, position.posedge_count);
-  EXPECT_DOUBLE_EQ(1.25, position.posedge_value);
-  EXPECT_EQ(2, position.negedge_count);
-  EXPECT_DOUBLE_EQ(1.75, position.negedge_value);
+  position = sim.FillSensorValues<HallEffectAndPosition>(&fbb);
+  EXPECT_FALSE(position->current());
+  EXPECT_DOUBLE_EQ(2.0, position->encoder());
+  EXPECT_EQ(2, position->posedge_count());
+  EXPECT_DOUBLE_EQ(1.25, position->posedge_value());
+  EXPECT_EQ(2, position->negedge_count());
+  EXPECT_DOUBLE_EQ(1.75, position->negedge_value());
 
   // Now, jump a whole cycle backwards.
   sim.MoveTo(0.75);
-  sim.GetSensorValues(&position);
-  EXPECT_FALSE(position.current);
-  EXPECT_DOUBLE_EQ(1.0, position.encoder);
-  EXPECT_EQ(3, position.posedge_count);
-  EXPECT_DOUBLE_EQ(1.75, position.posedge_value);
-  EXPECT_EQ(3, position.negedge_count);
-  EXPECT_DOUBLE_EQ(1.25, position.negedge_value);
+  position = sim.FillSensorValues<HallEffectAndPosition>(&fbb);
+  EXPECT_FALSE(position->current());
+  EXPECT_DOUBLE_EQ(1.0, position->encoder());
+  EXPECT_EQ(3, position->posedge_count());
+  EXPECT_DOUBLE_EQ(1.75, position->posedge_value());
+  EXPECT_EQ(3, position->negedge_count());
+  EXPECT_DOUBLE_EQ(1.25, position->negedge_value());
 
   // Now, go over the upper edge, rising.
   sim.MoveTo(0.25);
-  sim.GetSensorValues(&position);
-  EXPECT_TRUE(position.current);
-  EXPECT_DOUBLE_EQ(0.5, position.encoder);
-  EXPECT_EQ(4, position.posedge_count);
-  EXPECT_DOUBLE_EQ(0.75, position.posedge_value);
-  EXPECT_EQ(3, position.negedge_count);
-  EXPECT_DOUBLE_EQ(1.25, position.negedge_value);
+  position = sim.FillSensorValues<HallEffectAndPosition>(&fbb);
+  EXPECT_TRUE(position->current());
+  EXPECT_DOUBLE_EQ(0.5, position->encoder());
+  EXPECT_EQ(4, position->posedge_count());
+  EXPECT_DOUBLE_EQ(0.75, position->posedge_value());
+  EXPECT_EQ(3, position->negedge_count());
+  EXPECT_DOUBLE_EQ(1.25, position->negedge_value());
 
   // Now, go over the lower edge, falling.
   sim.MoveTo(-0.25);
-  sim.GetSensorValues(&position);
-  EXPECT_FALSE(position.current);
-  EXPECT_DOUBLE_EQ(0.0, position.encoder);
-  EXPECT_EQ(4, position.posedge_count);
-  EXPECT_DOUBLE_EQ(0.75, position.posedge_value);
-  EXPECT_EQ(4, position.negedge_count);
-  EXPECT_DOUBLE_EQ(0.25, position.negedge_value);
+  position = sim.FillSensorValues<HallEffectAndPosition>(&fbb);
+  EXPECT_FALSE(position->current());
+  EXPECT_DOUBLE_EQ(0.0, position->encoder());
+  EXPECT_EQ(4, position->posedge_count());
+  EXPECT_DOUBLE_EQ(0.75, position->posedge_value());
+  EXPECT_EQ(4, position->negedge_count());
+  EXPECT_DOUBLE_EQ(0.25, position->negedge_value());
 
   for (int i = 0; i < 10; ++i) {
     // Now, go over the lower edge, falling.
     sim.MoveTo(-0.25 - i * 1.0e-6);
-    sim.GetSensorValues(&position);
-    EXPECT_FALSE(position.current);
-    EXPECT_NEAR(-i * 1.0e-6, position.encoder, 1e-8);
-    EXPECT_EQ(4, position.posedge_count);
-    EXPECT_DOUBLE_EQ(0.75, position.posedge_value);
-    EXPECT_EQ(4, position.negedge_count);
-    EXPECT_DOUBLE_EQ(0.25, position.negedge_value);
+    position = sim.FillSensorValues<HallEffectAndPosition>(&fbb);
+    EXPECT_FALSE(position->current());
+    EXPECT_NEAR(-i * 1.0e-6, position->encoder(), 1e-8);
+    EXPECT_EQ(4, position->posedge_count());
+    EXPECT_DOUBLE_EQ(0.75, position->posedge_value());
+    EXPECT_EQ(4, position->negedge_count());
+    EXPECT_DOUBLE_EQ(0.25, position->negedge_value());
   }
 }
 
diff --git a/frc971/control_loops/profiled_subsystem.fbs b/frc971/control_loops/profiled_subsystem.fbs
new file mode 100644
index 0000000..46bc9fc
--- /dev/null
+++ b/frc971/control_loops/profiled_subsystem.fbs
@@ -0,0 +1,204 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace frc971.control_loops;
+
+table ProfiledJointStatus {
+  // Is the subsystem zeroed?
+  zeroed:bool;
+
+  // The state of the subsystem, if applicable.  -1 otherwise.
+  // TODO(alex): replace with enum.
+  state:int;
+
+  // If true, we have aborted.
+  estopped:bool;
+
+  // Position of the joint.
+  position:float;
+  // Velocity of the joint in units/second.
+  velocity:float;
+  // Profiled goal position of the joint.
+  goal_position:float;
+  // Profiled goal velocity of the joint in units/second.
+  goal_velocity:float;
+  // Unprofiled goal position from absoulte zero  of the joint.
+  unprofiled_goal_position:float;
+  // Unprofiled goal velocity of the joint in units/second.
+  unprofiled_goal_velocity:float;
+
+  // The estimated voltage error.
+  voltage_error:float;
+
+  // The calculated velocity with delta x/delta t
+  calculated_velocity:float;
+
+  // Components of the control loop output
+  position_power:float;
+  velocity_power:float;
+  feedforwards_power:float;
+
+  // State of the estimator.
+  estimator_state:frc971.EstimatorState;
+}
+
+table HallProfiledJointStatus {
+  // Is the subsystem zeroed?
+  zeroed:bool;
+
+  // The state of the subsystem, if applicable.  -1 otherwise.
+  // TODO(alex): replace with enum.
+  state:int;
+
+  // If true, we have aborted.
+  estopped:bool;
+
+  // Position of the joint.
+  position:float;
+  // Velocity of the joint in units/second.
+  velocity:float;
+  // Profiled goal position of the joint.
+  goal_position:float;
+  // Profiled goal velocity of the joint in units/second.
+  goal_velocity:float;
+  // Unprofiled goal position from absoulte zero  of the joint.
+  unprofiled_goal_position:float;
+  // Unprofiled goal velocity of the joint in units/second.
+  unprofiled_goal_velocity:float;
+
+  // The estimated voltage error.
+  voltage_error:float;
+
+  // The calculated velocity with delta x/delta t
+  calculated_velocity:float;
+
+  // Components of the control loop output
+  position_power:float;
+  velocity_power:float;
+  feedforwards_power:float;
+
+  // State of the estimator.
+  estimator_state:frc971.HallEffectAndPositionEstimatorState;
+}
+
+table PotAndAbsoluteEncoderProfiledJointStatus {
+  // Is the subsystem zeroed?
+  zeroed:bool;
+
+  // The state of the subsystem, if applicable.  -1 otherwise.
+  // TODO(alex): replace with enum.
+  state:int;
+
+  // If true, we have aborted.
+  estopped:bool;
+
+  // Position of the joint.
+  position:float;
+  // Velocity of the joint in units/second.
+  velocity:float;
+  // Profiled goal position of the joint.
+  goal_position:float;
+  // Profiled goal velocity of the joint in units/second.
+  goal_velocity:float;
+  // Unprofiled goal position from absoulte zero  of the joint.
+  unprofiled_goal_position:float;
+  // Unprofiled goal velocity of the joint in units/second.
+  unprofiled_goal_velocity:float;
+
+  // The estimated voltage error.
+  voltage_error:float;
+
+  // The calculated velocity with delta x/delta t
+  calculated_velocity:float;
+
+  // Components of the control loop output
+  position_power:float;
+  velocity_power:float;
+  feedforwards_power:float;
+
+  // State of the estimator.
+  estimator_state:frc971.PotAndAbsoluteEncoderEstimatorState;
+}
+
+table IndexProfiledJointStatus {
+  // Is the subsystem zeroed?
+  zeroed:bool;
+
+  // The state of the subsystem, if applicable.  -1 otherwise.
+  // TODO(alex): replace with enum.
+  state:int;
+
+  // If true, we have aborted.
+  estopped:bool;
+
+  // Position of the joint.
+  position:float;
+  // Velocity of the joint in units/second.
+  velocity:float;
+  // Profiled goal position of the joint.
+  goal_position:float;
+  // Profiled goal velocity of the joint in units/second.
+  goal_velocity:float;
+  // Unprofiled goal position from absoulte zero  of the joint.
+  unprofiled_goal_position:float;
+  // Unprofiled goal velocity of the joint in units/second.
+  unprofiled_goal_velocity:float;
+
+  // The estimated voltage error.
+  voltage_error:float;
+
+  // The calculated velocity with delta x/delta t
+  calculated_velocity:float;
+
+  // Components of the control loop output
+  position_power:float;
+  velocity_power:float;
+  feedforwards_power:float;
+
+  // State of the estimator.
+  estimator_state:frc971.IndexEstimatorState;
+}
+
+table AbsoluteEncoderProfiledJointStatus {
+  // Is the subsystem zeroed?
+  zeroed:bool;
+
+  // The state of the subsystem, if applicable.  -1 otherwise.
+  // TODO(alex): replace with enum.
+  state:int;
+
+  // If true, we have aborted.
+  estopped:bool;
+
+  // Position of the joint.
+  position:float;
+  // Velocity of the joint in units/second.
+  velocity:float;
+  // Profiled goal position of the joint.
+  goal_position:float;
+  // Profiled goal velocity of the joint in units/second.
+  goal_velocity:float;
+  // Unprofiled goal position from absoulte zero  of the joint.
+  unprofiled_goal_position:float;
+  // Unprofiled goal velocity of the joint in units/second.
+  unprofiled_goal_velocity:float;
+
+  // The estimated voltage error.
+  voltage_error:float;
+
+  // The calculated velocity with delta x/delta t
+  calculated_velocity:float;
+
+  // Components of the control loop output
+  position_power:float;
+  velocity_power:float;
+  feedforwards_power:float;
+
+  // State of the estimator.
+  estimator_state:frc971.AbsoluteEncoderEstimatorState;
+}
+
+table StaticZeroingSingleDOFProfiledSubsystemGoal {
+  unsafe_goal:double;
+
+  profile_params:frc971.ProfileParameters;
+}
diff --git a/frc971/control_loops/profiled_subsystem.h b/frc971/control_loops/profiled_subsystem.h
index 58058c7..2752dcc 100644
--- a/frc971/control_loops/profiled_subsystem.h
+++ b/frc971/control_loops/profiled_subsystem.h
@@ -11,8 +11,8 @@
 #include "aos/controls/control_loop.h"
 #include "aos/util/trapezoid_profile.h"
 #include "frc971/constants.h"
-#include "frc971/control_loops/control_loops.q.h"
-#include "frc971/control_loops/profiled_subsystem.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/profiled_subsystem_generated.h"
 #include "frc971/control_loops/simple_capped_state_feedback_loop.h"
 #include "frc971/control_loops/state_feedback_loop.h"
 #include "frc971/zeroing/zeroing.h"
@@ -106,8 +106,9 @@
   }
 
   // Returns the current internal estimator state for logging.
-  typename ZeroingEstimator::State EstimatorState(int index) {
-    return estimators_[index].GetEstimatorState();
+  flatbuffers::Offset<typename ZeroingEstimator::State> EstimatorState(
+      flatbuffers::FlatBufferBuilder *fbb, int index) {
+    return estimators_[index].GetEstimatorState(fbb);
   }
 
   // Sets the maximum voltage that will be commanded by the loop.
@@ -152,13 +153,14 @@
       double default_angular_acceleration);
 
   // Updates our estimator with the latest position.
-  void Correct(typename ZeroingEstimator::Position position);
+  void Correct(const typename ZeroingEstimator::Position &position);
   // Runs the controller and profile generator for a cycle.
   void Update(bool disabled);
 
   // Fills out the ProfiledJointStatus structure with the current state.
-  template <class StatusType>
-  void PopulateStatus(StatusType *status);
+  template <class StatusTypeBuilder>
+  StatusTypeBuilder BuildStatus(
+      flatbuffers::FlatBufferBuilder *fbb);
 
   // Forces the current goal to the provided goal, bypassing the profiler.
   void ForceGoal(double goal);
@@ -166,7 +168,7 @@
   // this goal.
   void set_unprofiled_goal(double unprofiled_goal);
   // Limits our profiles to a max velocity and acceleration for proper motion.
-  void AdjustProfile(const ::frc971::ProfileParameters &profile_parameters);
+  void AdjustProfile(const ::frc971::ProfileParameters *profile_parameters);
   void AdjustProfile(double max_angular_velocity,
                      double max_angular_acceleration);
 
@@ -248,37 +250,41 @@
 }
 
 template <class ZeroingEstimator>
-template <class StatusType>
-void SingleDOFProfiledSubsystem<ZeroingEstimator>::PopulateStatus(
-    StatusType *status) {
-  status->zeroed = this->zeroed();
-  status->state = -1;
+template <class StatusTypeBuilder>
+StatusTypeBuilder SingleDOFProfiledSubsystem<ZeroingEstimator>::BuildStatus(
+    flatbuffers::FlatBufferBuilder *fbb) {
+  flatbuffers::Offset<typename ZeroingEstimator::State> estimator_state =
+      this->EstimatorState(fbb, 0);
+
+  StatusTypeBuilder builder(*fbb);
+
+  builder.add_zeroed(this->zeroed());
   // We don't know, so default to the bad case.
-  status->estopped = true;
 
-  status->position = this->X_hat(0, 0);
-  status->velocity = this->X_hat(1, 0);
-  status->goal_position = this->goal(0, 0);
-  status->goal_velocity = this->goal(1, 0);
-  status->unprofiled_goal_position = this->unprofiled_goal(0, 0);
-  status->unprofiled_goal_velocity = this->unprofiled_goal(1, 0);
-  status->voltage_error = this->X_hat(2, 0);
-  status->calculated_velocity =
+  builder.add_position(this->X_hat(0, 0));
+  builder.add_velocity(this->X_hat(1, 0));
+  builder.add_goal_position(this->goal(0, 0));
+  builder.add_goal_velocity(this->goal(1, 0));
+  builder.add_unprofiled_goal_position(this->unprofiled_goal(0, 0));
+  builder.add_unprofiled_goal_velocity(this->unprofiled_goal(1, 0));
+  builder.add_voltage_error(this->X_hat(2, 0));
+  builder.add_calculated_velocity(
       (position() - last_position_) /
-      ::aos::time::DurationInSeconds(::aos::controls::kLoopFrequency);
+      ::aos::time::DurationInSeconds(::aos::controls::kLoopFrequency));
 
-  status->estimator_state = this->EstimatorState(0);
+  builder.add_estimator_state(estimator_state);
 
   Eigen::Matrix<double, 3, 1> error = this->controller().error();
-  status->position_power =
-      this->controller().controller().K(0, 0) * error(0, 0);
-  status->velocity_power =
-      this->controller().controller().K(0, 1) * error(1, 0);
+  builder.add_position_power(
+      this->controller().controller().K(0, 0) * error(0, 0));
+  builder.add_velocity_power(
+      this->controller().controller().K(0, 1) * error(1, 0));
+  return builder;
 }
 
 template <class ZeroingEstimator>
 void SingleDOFProfiledSubsystem<ZeroingEstimator>::Correct(
-    typename ZeroingEstimator::Position new_position) {
+    const typename ZeroingEstimator::Position &new_position) {
   this->estimators_[0].UpdateEstimate(new_position);
 
   if (this->estimators_[0].error()) {
@@ -299,7 +305,7 @@
   }
 
   last_position_ = position();
-  this->Y_ << new_position.encoder;
+  this->Y_ << new_position.encoder();
   this->Y_ += this->offset_;
   this->loop_->Correct(Y_);
 }
@@ -385,9 +391,11 @@
 
 template <class ZeroingEstimator>
 void SingleDOFProfiledSubsystem<ZeroingEstimator>::AdjustProfile(
-    const ::frc971::ProfileParameters &profile_parameters) {
-  AdjustProfile(profile_parameters.max_velocity,
-                profile_parameters.max_acceleration);
+    const ::frc971::ProfileParameters *profile_parameters) {
+  AdjustProfile(
+      profile_parameters != nullptr ? profile_parameters->max_velocity() : 0.0,
+      profile_parameters != nullptr ? profile_parameters->max_acceleration()
+                                    : 0.0);
 }
 
 template <class ZeroingEstimator>
diff --git a/frc971/control_loops/profiled_subsystem.q b/frc971/control_loops/profiled_subsystem.q
deleted file mode 100644
index ac23907..0000000
--- a/frc971/control_loops/profiled_subsystem.q
+++ /dev/null
@@ -1,198 +0,0 @@
-package frc971.control_loops;
-
-import "frc971/control_loops/control_loops.q";
-
-struct ProfiledJointStatus {
-  // Is the subsystem zeroed?
-  bool zeroed;
-
-  // The state of the subsystem, if applicable.  -1 otherwise.
-  int32_t state;
-
-  // If true, we have aborted.
-  bool estopped;
-
-  // Position of the joint.
-  float position;
-  // Velocity of the joint in units/second.
-  float velocity;
-  // Profiled goal position of the joint.
-  float goal_position;
-  // Profiled goal velocity of the joint in units/second.
-  float goal_velocity;
-  // Unprofiled goal position from absoulte zero  of the joint.
-  float unprofiled_goal_position;
-  // Unprofiled goal velocity of the joint in units/second.
-  float unprofiled_goal_velocity;
-
-  // The estimated voltage error.
-  float voltage_error;
-
-  // The calculated velocity with delta x/delta t
-  float calculated_velocity;
-
-  // Components of the control loop output
-  float position_power;
-  float velocity_power;
-  float feedforwards_power;
-
-  // State of the estimator.
-  .frc971.EstimatorState estimator_state;
-};
-
-struct HallProfiledJointStatus {
-  // Is the subsystem zeroed?
-  bool zeroed;
-
-  // The state of the subsystem, if applicable.  -1 otherwise.
-  int32_t state;
-
-  // If true, we have aborted.
-  bool estopped;
-
-  // Position of the joint.
-  float position;
-  // Velocity of the joint in units/second.
-  float velocity;
-  // Profiled goal position of the joint.
-  float goal_position;
-  // Profiled goal velocity of the joint in units/second.
-  float goal_velocity;
-  // Unprofiled goal position from absoulte zero  of the joint.
-  float unprofiled_goal_position;
-  // Unprofiled goal velocity of the joint in units/second.
-  float unprofiled_goal_velocity;
-
-  // The estimated voltage error.
-  float voltage_error;
-
-  // The calculated velocity with delta x/delta t
-  float calculated_velocity;
-
-  // Components of the control loop output
-  float position_power;
-  float velocity_power;
-  float feedforwards_power;
-
-  // State of the estimator.
-  .frc971.HallEffectAndPositionEstimatorState estimator_state;
-};
-
-struct PotAndAbsoluteEncoderProfiledJointStatus {
-  // Is the subsystem zeroed?
-  bool zeroed;
-
-  // The state of the subsystem, if applicable.  -1 otherwise.
-  int32_t state;
-
-  // If true, we have aborted.
-  bool estopped;
-
-  // Position of the joint.
-  float position;
-  // Velocity of the joint in units/second.
-  float velocity;
-  // Profiled goal position of the joint.
-  float goal_position;
-  // Profiled goal velocity of the joint in units/second.
-  float goal_velocity;
-  // Unprofiled goal position from absoulte zero of the joint.
-  float unprofiled_goal_position;
-  // Unprofiled goal velocity of the joint in units/second.
-  float unprofiled_goal_velocity;
-
-  // The estimated voltage error.
-  float voltage_error;
-
-  // The calculated velocity with delta x/delta t
-  float calculated_velocity;
-
-  // Components of the control loop output
-  float position_power;
-  float velocity_power;
-  float feedforwards_power;
-
-  // State of the estimator.
-  .frc971.PotAndAbsoluteEncoderEstimatorState estimator_state;
-};
-
-struct IndexProfiledJointStatus {
-  // Is the subsystem zeroed?
-  bool zeroed;
-
-  // The state of the subsystem, if applicable.  -1 otherwise.
-  int32_t state;
-
-  // If true, we have aborted.
-  bool estopped;
-
-  // Position of the joint.
-  float position;
-  // Velocity of the joint in units/second.
-  float velocity;
-  // Profiled goal position of the joint.
-  float goal_position;
-  // Profiled goal velocity of the joint in units/second.
-  float goal_velocity;
-  // Unprofiled goal position from absoulte zero of the joint.
-  float unprofiled_goal_position;
-  // Unprofiled goal velocity of the joint in units/second.
-  float unprofiled_goal_velocity;
-
-  // The estimated voltage error.
-  float voltage_error;
-
-  // The calculated velocity with delta x/delta t
-  float calculated_velocity;
-
-  // Components of the control loop output
-  float position_power;
-  float velocity_power;
-  float feedforwards_power;
-
-  // State of the estimator.
-  .frc971.IndexEstimatorState estimator_state;
-};
-
-struct AbsoluteEncoderProfiledJointStatus {
-  // Is the subsystem zeroed?
-  bool zeroed;
-
-  // The state of the subsystem, if applicable.  -1 otherwise.
-  int32_t state;
-
-  // If true, we have aborted.
-  bool estopped;
-
-  // Position of the joint.
-  float position;
-  // Velocity of the joint in units/second.
-  float velocity;
-  // Profiled goal position of the joint.
-  float goal_position;
-  // Profiled goal velocity of the joint in units/second.
-  float goal_velocity;
-  // Unprofiled goal position from absoulte zero of the joint.
-  float unprofiled_goal_position;
-  // Unprofiled goal velocity of the joint in units/second.
-  float unprofiled_goal_velocity;
-
-  // The estimated voltage error.
-  float voltage_error;
-
-  // The calculated velocity with delta x/delta t
-  float calculated_velocity;
-
-  // Components of the control loop output
-  float position_power;
-  float velocity_power;
-  float feedforwards_power;
-
-  // State of the estimator.
-  .frc971.AbsoluteEncoderEstimatorState estimator_state;
-};
-
-struct StaticZeroingSingleDOFProfiledSubsystemGoal {
-  double unsafe_goal;
-  .frc971.ProfileParameters profile_params;
-};
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
index 748086f..b220581 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
@@ -6,6 +6,12 @@
 namespace frc971 {
 namespace control_loops {
 
+// TODO(austin): Use ProfileParametersT...
+struct ProfileParametersStruct {
+  float max_velocity;
+  float max_acceleration;
+};
+
 template <typename ZeroingEstimator>
 struct StaticZeroingSingleDOFProfiledSubsystemParams {
   // Maximum voltage while the subsystem is zeroing
@@ -15,11 +21,11 @@
   double operating_voltage;
 
   // Maximum velocity (units/s) and acceleration while State::ZEROING
-  ::frc971::ProfileParameters zeroing_profile_params;
+  ProfileParametersStruct zeroing_profile_params;
 
   // Maximum velocity (units/s) and acceleration while State::RUNNING if max
   // velocity or acceleration in goal profile_params is 0
-  ::frc971::ProfileParameters default_profile_params;
+  ProfileParametersStruct default_profile_params;
 
   // Maximum range of the subsystem in meters
   ::frc971::constants::Range range;
@@ -65,9 +71,10 @@
   // Returns the current position
   double position() const { return profiled_subsystem_.position(); }
 
-  void Iterate(const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
-               const typename ZeroingEstimator::Position *position,
-               double *output, ProfiledJointStatus *status);
+  flatbuffers::Offset<ProfiledJointStatus> Iterate(
+      const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
+      const typename ZeroingEstimator::Position *position, double *output,
+      flatbuffers::FlatBufferBuilder *status_fbb);
 
   // Resets the profiled subsystem and returns to uninitialized
   void Reset();
@@ -125,11 +132,11 @@
 }
 
 template <typename ZeroingEstimator, typename ProfiledJointStatus>
-void StaticZeroingSingleDOFProfiledSubsystem<ZeroingEstimator,
-                                             ProfiledJointStatus>::
+flatbuffers::Offset<ProfiledJointStatus>
+StaticZeroingSingleDOFProfiledSubsystem<ZeroingEstimator, ProfiledJointStatus>::
     Iterate(const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
             const typename ZeroingEstimator::Position *position, double *output,
-            ProfiledJointStatus *status) {
+            flatbuffers::FlatBufferBuilder *status_fbb) {
   bool disabled = output == nullptr;
   profiled_subsystem_.Correct(*position);
 
@@ -159,7 +166,9 @@
       // jump.
       profiled_subsystem_.ForceGoal(profiled_subsystem_.position());
       // Set up the profile to be the zeroing profile.
-      profiled_subsystem_.AdjustProfile(params_.zeroing_profile_params);
+      profiled_subsystem_.AdjustProfile(
+          params_.zeroing_profile_params.max_velocity,
+          params_.zeroing_profile_params.max_acceleration);
 
       // We are not ready to start doing anything yet.
       disabled = true;
@@ -181,9 +190,9 @@
       }
 
       if (goal) {
-        profiled_subsystem_.AdjustProfile(goal->profile_params);
+        profiled_subsystem_.AdjustProfile(goal->profile_params());
 
-        double safe_goal = goal->unsafe_goal;
+        double safe_goal = goal->unsafe_goal();
         if (safe_goal < min_position_) {
           AOS_LOG(DEBUG, "Limiting to %f from %f\n", min_position_, safe_goal);
           safe_goal = min_position_;
@@ -217,11 +226,14 @@
     *output = profiled_subsystem_.voltage();
   }
 
-  status->zeroed = profiled_subsystem_.zeroed();
+  typename ProfiledJointStatus::Builder status_builder =
+      profiled_subsystem_
+          .template BuildStatus<typename ProfiledJointStatus::Builder>(
+              status_fbb);
 
-  profiled_subsystem_.PopulateStatus(status);
-  status->estopped = (state_ == State::ESTOP);
-  status->state = static_cast<int32_t>(state_);
+  status_builder.add_estopped(state_ == State::ESTOP);
+  status_builder.add_state(static_cast<int32_t>(state_));
+  return status_builder.Finish();
 }
 
 }  // namespace control_loops
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
index 96cb782..46e9c76 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
@@ -1,3 +1,4 @@
+#include "flatbuffers/flatbuffers.h"
 #include "gtest/gtest.h"
 
 #include "aos/controls/control_loop.h"
@@ -5,9 +6,10 @@
 #include "frc971/control_loops/capped_test_plant.h"
 #include "frc971/control_loops/position_sensor_sim.h"
 #include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
-#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.q.h"
+#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test_generated.h"
 #include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test_integral_plant.h"
 #include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test_plant.h"
+#include "frc971/zeroing/zeroing.h"
 
 using ::frc971::control_loops::PositionSensorSimulator;
 
@@ -20,20 +22,32 @@
 using ::aos::monotonic_clock;
 
 using SZSDPS_PotAndAbsEncoder = StaticZeroingSingleDOFProfiledSubsystem<
-    zeroing::PotAndAbsoluteEncoderZeroingEstimator,
+    ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator,
     ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>;
 
 using SZSDPS_AbsEncoder = StaticZeroingSingleDOFProfiledSubsystem<
-    zeroing::AbsoluteEncoderZeroingEstimator,
+    ::frc971::zeroing::AbsoluteEncoderZeroingEstimator,
     ::frc971::control_loops::AbsoluteEncoderProfiledJointStatus>;
 
+using FBB = flatbuffers::FlatBufferBuilder;
+
+struct PotAndAbsoluteEncoderQueueGroup {
+  typedef PotAndAbsoluteEncoderProfiledJointStatus Status;
+  typedef PotAndAbsolutePosition Position;
+  typedef ::frc971::control_loops::zeroing::testing::SubsystemGoal Goal;
+  typedef ::frc971::control_loops::zeroing::testing::SubsystemOutput Output;
+};
+
+struct AbsoluteEncoderQueueGroup {
+  typedef AbsoluteEncoderProfiledJointStatus Status;
+  typedef AbsolutePosition Position;
+  typedef zeroing::testing::SubsystemGoal Goal;
+  typedef zeroing::testing::SubsystemOutput Output;
+};
+
 typedef ::testing::Types<
-    ::std::pair<
-        SZSDPS_AbsEncoder,
-        StaticZeroingSingleDOFProfiledSubsystemAbsoluteEncoderTestQueueGroup>,
-    ::std::pair<
-        SZSDPS_PotAndAbsEncoder,
-        StaticZeroingSingleDOFProfiledSubsystemPotAndAbsoluteEncoderTestQueueGroup>>
+    ::std::pair<SZSDPS_AbsEncoder, AbsoluteEncoderQueueGroup>,
+    ::std::pair<SZSDPS_PotAndAbsEncoder, PotAndAbsoluteEncoderQueueGroup>>
     TestTypes;
 
 constexpr ::frc971::constants::Range kRange{
@@ -54,14 +68,15 @@
 };
 
 template <>
-const zeroing::PotAndAbsoluteEncoderZeroingEstimator::ZeroingConstants
+const frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator::ZeroingConstants
     TestIntakeSystemValues<
-        zeroing::PotAndAbsoluteEncoderZeroingEstimator>::kZeroing{
+        frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator>::kZeroing{
         kZeroingSampleSize, kEncoderIndexDifference, 0, 0.0005, 20, 1.9};
 
 template <>
-const zeroing::AbsoluteEncoderZeroingEstimator::ZeroingConstants
-    TestIntakeSystemValues<zeroing::AbsoluteEncoderZeroingEstimator>::kZeroing{
+const frc971::zeroing::AbsoluteEncoderZeroingEstimator::ZeroingConstants
+    TestIntakeSystemValues<
+        frc971::zeroing::AbsoluteEncoderZeroingEstimator>::kZeroing{
         kZeroingSampleSize, kEncoderIndexDifference, 0.0, 0.2, 0.0005, 20, 1.9};
 
 template <typename ZeroingEstimator>
@@ -83,29 +98,21 @@
 template <typename SZSDPS, typename QueueGroup>
 class TestIntakeSystemSimulation {
  public:
-  typedef typename std::remove_reference<decltype(
-      *(static_cast<QueueGroup *>(NULL)->goal.MakeMessage().get()))>::type
-      GoalType;
-  typedef typename std::remove_reference<decltype(
-      *(static_cast<QueueGroup *>(NULL)->position.MakeMessage().get()))>::type
-      PositionType;
-  typedef typename std::remove_reference<decltype(
-      *(static_cast<QueueGroup *>(NULL)->status.MakeMessage().get()))>::type
-      StatusType;
-  typedef typename std::remove_reference<decltype(
-      *(static_cast<QueueGroup *>(NULL)->output.MakeMessage().get()))>::type
-      OutputType;
+  typedef typename QueueGroup::Goal GoalType;
+  typedef typename QueueGroup::Status StatusType;
+  typedef typename QueueGroup::Position PositionType;
+  typedef typename QueueGroup::Output OutputType;
 
   TestIntakeSystemSimulation(::aos::EventLoop *event_loop,
                              chrono::nanoseconds dt)
       : event_loop_(event_loop),
         dt_(dt),
         subsystem_position_sender_(
-            event_loop_->MakeSender<PositionType>(".position")),
+            event_loop_->MakeSender<PositionType>("/loop")),
         subsystem_status_fetcher_(
-            event_loop_->MakeFetcher<StatusType>(".status")),
+            event_loop_->MakeFetcher<StatusType>("/loop")),
         subsystem_output_fetcher_(
-            event_loop_->MakeFetcher<OutputType>(".output")),
+            event_loop_->MakeFetcher<OutputType>("/loop")),
         subsystem_plant_(new CappedTestPlant(
             ::frc971::control_loops::MakeTestIntakeSystemPlant())),
         subsystem_sensor_sim_(kEncoderIndexDifference) {
@@ -133,8 +140,8 @@
 
   void InitializeSensorSim(double start_pos);
 
-  double subsystem_position() const { return this->subsystem_plant_->X(0, 0); }
-  double subsystem_velocity() const { return this->subsystem_plant_->X(1, 0); }
+  double subsystem_position() const { return subsystem_plant_->X(0, 0); }
+  double subsystem_velocity() const { return subsystem_plant_->X(1, 0); }
 
   // Sets the difference between the commanded and applied powers.
   // This lets us test that the integrators work.
@@ -144,12 +151,13 @@
 
   // Sends a queue message with the position.
   void SendPositionMessage() {
-    typename ::aos::Sender<PositionType>::Message position =
-        subsystem_position_sender_.MakeMessage();
+    typename ::aos::Sender<PositionType>::Builder position =
+        subsystem_position_sender_.MakeBuilder();
 
-    this->subsystem_sensor_sim_.GetSensorValues(&position->position);
-
-    position.Send();
+    auto position_builder = position.template MakeBuilder<PositionType>();
+    position.Send(this->subsystem_sensor_sim_
+                      .template GetSensorValues<typename PositionType::Builder>(
+                          &position_builder));
   }
 
   void set_peak_subsystem_acceleration(double value) {
@@ -168,15 +176,15 @@
 
     const double voltage_check_subsystem =
         (static_cast<typename SZSDPS::State>(
-             subsystem_status_fetcher_->status.state) == SZSDPS::State::RUNNING)
+             subsystem_status_fetcher_->state()) == SZSDPS::State::RUNNING)
             ? kOperatingVoltage
             : kZeroingVoltage;
 
-    EXPECT_LE(::std::abs(subsystem_output_fetcher_->output),
+    EXPECT_LE(::std::abs(subsystem_output_fetcher_->output()),
               voltage_check_subsystem);
 
     ::Eigen::Matrix<double, 1, 1> subsystem_U;
-    subsystem_U << subsystem_output_fetcher_->output +
+    subsystem_U << subsystem_output_fetcher_->output() +
                        subsystem_plant_->voltage_offset();
     subsystem_plant_->Update(subsystem_U);
 
@@ -211,13 +219,14 @@
   double peak_subsystem_acceleration_ = 1e10;
   // The velocity limits to check for while moving.
   double peak_subsystem_velocity_ = 1e10;
+
+  flatbuffers::FlatBufferBuilder fbb;
 };
 
 template <>
 void TestIntakeSystemSimulation<
     SZSDPS_PotAndAbsEncoder,
-    StaticZeroingSingleDOFProfiledSubsystemPotAndAbsoluteEncoderTestQueueGroup>::
-    InitializeSensorSim(double start_pos) {
+    PotAndAbsoluteEncoderQueueGroup>::InitializeSensorSim(double start_pos) {
   subsystem_sensor_sim_.Initialize(
       start_pos, kNoiseScalar, 0.0,
       TestIntakeSystemValues<
@@ -226,9 +235,7 @@
 }
 
 template <>
-void TestIntakeSystemSimulation<
-    SZSDPS_AbsEncoder,
-    StaticZeroingSingleDOFProfiledSubsystemAbsoluteEncoderTestQueueGroup>::
+void TestIntakeSystemSimulation<SZSDPS_AbsEncoder, AbsoluteEncoderQueueGroup>::
     InitializeSensorSim(double start_pos) {
   subsystem_sensor_sim_.Initialize(
       start_pos, kNoiseScalar, 0.0,
@@ -240,46 +247,73 @@
 // Class to represent a module using a subsystem.  This lets us use event loops
 // to wrap it.
 template <typename QueueGroup, typename SZSDPS>
-class Subsystem : public ::aos::controls::ControlLoop<QueueGroup> {
+class Subsystem
+    : public ::aos::controls::ControlLoop<
+          typename QueueGroup::Goal, typename QueueGroup::Position,
+          typename QueueGroup::Status, typename QueueGroup::Output> {
  public:
-  typedef typename std::remove_reference<decltype(
-      *(static_cast<QueueGroup *>(NULL)->goal.MakeMessage().get()))>::type
-      GoalType;
-  typedef typename std::remove_reference<decltype(
-      *(static_cast<QueueGroup *>(NULL)->position.MakeMessage().get()))>::type
-      PositionType;
-  typedef typename std::remove_reference<decltype(
-      *(static_cast<QueueGroup *>(NULL)->status.MakeMessage().get()))>::type
-      StatusType;
-  typedef typename std::remove_reference<decltype(
-      *(static_cast<QueueGroup *>(NULL)->output.MakeMessage().get()))>::type
-      OutputType;
+  typedef typename QueueGroup::Goal GoalType;
+  typedef typename QueueGroup::Status StatusType;
+  typedef typename QueueGroup::Position PositionType;
+  typedef typename QueueGroup::Output OutputType;
 
   Subsystem(::aos::EventLoop *event_loop, const ::std::string &name)
-      : aos::controls::ControlLoop<QueueGroup>(event_loop, name),
+      : aos::controls::ControlLoop<
+            typename QueueGroup::Goal, typename QueueGroup::Position,
+            typename QueueGroup::Status, typename QueueGroup::Output>(
+            event_loop, name),
         subsystem_(TestIntakeSystemValues<
                    typename SZSDPS::ZeroingEstimator>::make_params()) {}
 
   void RunIteration(const GoalType *unsafe_goal, const PositionType *position,
-                    OutputType *output, StatusType *status) {
+                    typename ::aos::Sender<OutputType>::Builder *output,
+                    typename ::aos::Sender<StatusType>::Builder *status) {
     if (this->WasReset()) {
       AOS_LOG(ERROR, "WPILib reset, restarting\n");
       subsystem_.Reset();
     }
 
     // Convert one goal type to another...
-    StaticZeroingSingleDOFProfiledSubsystemGoal goal;
+    // TODO(austin): This mallocs...
+    FBB fbb;
+    ProfileParametersBuilder params_builder(fbb);
     if (unsafe_goal != nullptr ) {
-      goal.unsafe_goal = unsafe_goal->unsafe_goal;
-      goal.profile_params.max_velocity =
-          unsafe_goal->profile_params.max_velocity;
-      goal.profile_params.max_acceleration =
-          unsafe_goal->profile_params.max_acceleration;
+      if (unsafe_goal->profile_params() != nullptr) {
+        params_builder.add_max_velocity(
+            unsafe_goal->profile_params()->max_velocity());
+        params_builder.add_max_acceleration(
+            unsafe_goal->profile_params()->max_acceleration());
+      }
+
+      const auto params_builder_offset = params_builder.Finish();
+      StaticZeroingSingleDOFProfiledSubsystemGoalBuilder goal_builder(fbb);
+      goal_builder.add_unsafe_goal(unsafe_goal->unsafe_goal());
+      goal_builder.add_profile_params(params_builder_offset);
+      fbb.Finish(goal_builder.Finish());
+    } else {
+      params_builder.add_max_velocity(0.0);
+      params_builder.add_max_acceleration(0.0);
+      const auto params_builder_offset = params_builder.Finish();
+      StaticZeroingSingleDOFProfiledSubsystemGoalBuilder goal_builder(fbb);
+      goal_builder.add_profile_params(params_builder_offset);
+      fbb.Finish(goal_builder.Finish());
     }
 
-    subsystem_.Iterate(
-        unsafe_goal == nullptr ? nullptr : &goal, &position->position,
-        output == nullptr ? nullptr : &output->output, &status->status);
+    double output_voltage;
+
+    flatbuffers::Offset<StatusType> status_offset = subsystem_.Iterate(
+        unsafe_goal == nullptr
+            ? nullptr
+            : flatbuffers::GetRoot<StaticZeroingSingleDOFProfiledSubsystemGoal>(
+                  fbb.GetBufferPointer()),
+        position, output == nullptr ? nullptr : &output_voltage, status->fbb());
+    status->Send(status_offset);
+    if (output != nullptr) {
+      typename OutputType::Builder output_builder =
+          output->template MakeBuilder<OutputType>();
+      output_builder.add_output(output_voltage);
+      output->Send(output_builder.Finish());
+    }
   }
 
   SZSDPS *subsystem() { return &subsystem_; }
@@ -296,29 +330,58 @@
   using ZeroingEstimator = typename SZSDPS::ZeroingEstimator;
   using ProfiledJointStatus = typename SZSDPS::ProfiledJointStatus;
 
-  typedef typename std::remove_reference<decltype(
-      *(static_cast<QueueGroup *>(NULL)->goal.MakeMessage().get()))>::type
-      GoalType;
-  typedef typename std::remove_reference<decltype(
-      *(static_cast<QueueGroup *>(NULL)->position.MakeMessage().get()))>::type
-      PositionType;
-  typedef typename std::remove_reference<decltype(
-      *(static_cast<QueueGroup *>(NULL)->status.MakeMessage().get()))>::type
-      StatusType;
-  typedef typename std::remove_reference<decltype(
-      *(static_cast<QueueGroup *>(NULL)->output.MakeMessage().get()))>::type
-      OutputType;
+  typedef typename QueueGroup::Goal GoalType;
+  typedef typename QueueGroup::Status StatusType;
+  typedef typename QueueGroup::Position PositionType;
+  typedef typename QueueGroup::Output OutputType;
 
   IntakeSystemTest()
-      : ::aos::testing::ControlLoopTest(chrono::microseconds(5050)),
+      : ::aos::testing::ControlLoopTest(
+            "{\n"
+            "  \"channels\": [ \n"
+            "    {\n"
+            "      \"name\": \"/aos\",\n"
+            "      \"type\": \"aos.JoystickState\"\n"
+            "    },\n"
+            "    {\n"
+            "      \"name\": \"/aos\",\n"
+            "      \"type\": \"aos.RobotState\"\n"
+            "    },\n"
+            "    {\n"
+            "      \"name\": \"/loop\",\n"
+            "      \"type\": \"frc971.control_loops.zeroing.testing.SubsystemGoal\"\n"
+            "    },\n"
+            "    {\n"
+            "      \"name\": \"/loop\",\n"
+            "      \"type\": \"frc971.control_loops.zeroing.testing.SubsystemOutput\"\n"
+            "    },\n"
+            "    {\n"
+            "      \"name\": \"/loop\",\n"
+            "      \"type\": \"frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus\"\n"
+            "    },\n"
+            "    {\n"
+            "      \"name\": \"/loop\",\n"
+            "      \"type\": \"frc971.AbsolutePosition\"\n"
+            "    },\n"
+            "    {\n"
+            "      \"name\": \"/loop\",\n"
+            "      \"type\": \"frc971.PotAndAbsolutePosition\"\n"
+            "    },\n"
+            "    {\n"
+            "      \"name\": \"/loop\",\n"
+            "      \"type\": \"frc971.control_loops.AbsoluteEncoderProfiledJointStatus\"\n"
+            "    }\n"
+            "  ]\n"
+            "}\n",
+            chrono::microseconds(5050)),
         test_event_loop_(MakeEventLoop()),
-        subsystem_goal_sender_(test_event_loop_->MakeSender<GoalType>(".goal")),
+        subsystem_goal_sender_(test_event_loop_->MakeSender<GoalType>("/loop")),
         subsystem_goal_fetcher_(
-            test_event_loop_->MakeFetcher<GoalType>(".goal")),
+            test_event_loop_->MakeFetcher<GoalType>("/loop")),
         subsystem_status_fetcher_(
-            test_event_loop_->MakeFetcher<StatusType>(".status")),
+            test_event_loop_->MakeFetcher<StatusType>("/loop")),
         subsystem_event_loop_(MakeEventLoop()),
-        subsystem_(subsystem_event_loop_.get(), ""),
+        subsystem_(subsystem_event_loop_.get(), "/loop"),
         subsystem_plant_event_loop_(MakeEventLoop()),
         subsystem_plant_(subsystem_plant_event_loop_.get(), dt()) {}
 
@@ -327,20 +390,20 @@
     EXPECT_TRUE(subsystem_goal_fetcher_.get() != nullptr);
     EXPECT_TRUE(subsystem_status_fetcher_.Fetch());
 
-    EXPECT_NEAR(subsystem_goal_fetcher_->unsafe_goal,
-                subsystem_status_fetcher_->status.position, 0.001);
-    EXPECT_NEAR(subsystem_goal_fetcher_->unsafe_goal,
+    EXPECT_NEAR(subsystem_goal_fetcher_->unsafe_goal(),
+                subsystem_status_fetcher_->position(), 0.001);
+    EXPECT_NEAR(subsystem_goal_fetcher_->unsafe_goal(),
                 subsystem_plant_.subsystem_position(), 0.001);
-    EXPECT_NEAR(subsystem_status_fetcher_->status.velocity, 0, 0.001);
+    EXPECT_NEAR(subsystem_status_fetcher_->velocity(), 0, 0.001);
   }
 
   SZSDPS *subsystem() { return subsystem_.subsystem(); }
 
   void set_peak_subsystem_acceleration(double value) {
-    set_peak_subsystem_acceleration(value);
+    subsystem_plant_.set_peak_subsystem_acceleration(value);
   }
   void set_peak_subsystem_velocity(double value) {
-    set_peak_subsystem_velocity(value);
+    subsystem_plant_.set_peak_subsystem_velocity(value);
   }
 
   ::std::unique_ptr<::aos::EventLoop> test_event_loop_;
@@ -363,9 +426,9 @@
   this->SetEnabled(true);
   // Intake system uses 0.05 to test for 0.
   {
-    auto message = this->subsystem_goal_sender_.MakeMessage();
-    message->unsafe_goal = 0.05;
-    EXPECT_TRUE(message.Send());
+    auto message = this->subsystem_goal_sender_.MakeBuilder();
+    EXPECT_TRUE(message.Send(
+        zeroing::testing::CreateSubsystemGoal(*message.fbb(), 0.05)));
   }
   this->RunFor(chrono::seconds(5));
 
@@ -377,11 +440,13 @@
   this->SetEnabled(true);
   // Set a reasonable goal.
   {
-    auto message = this->subsystem_goal_sender_.MakeMessage();
-    message->unsafe_goal = 0.1;
-    message->profile_params.max_velocity = 1;
-    message->profile_params.max_acceleration = 0.5;
-    EXPECT_TRUE(message.Send());
+    auto message = this->subsystem_goal_sender_.MakeBuilder();
+    auto profile_builder =
+        message.template MakeBuilder<frc971::ProfileParameters>();
+    profile_builder.add_max_velocity(1);
+    profile_builder.add_max_acceleration(0.5);
+    EXPECT_TRUE(message.Send(zeroing::testing::CreateSubsystemGoal(
+        *message.fbb(), 0.10, profile_builder.Finish())));
   }
 
   // Give it a lot of time to get there.
@@ -396,9 +461,9 @@
   this->SetEnabled(true);
   // Zero it before we move.
   {
-    auto message = this->subsystem_goal_sender_.MakeMessage();
-    message->unsafe_goal = kRange.upper;
-    EXPECT_TRUE(message.Send());
+    auto message = this->subsystem_goal_sender_.MakeBuilder();
+    EXPECT_TRUE(message.Send(
+        zeroing::testing::CreateSubsystemGoal(*message.fbb(), kRange.upper)));
   }
   this->RunFor(chrono::seconds(8));
   this->VerifyNearGoal();
@@ -406,11 +471,12 @@
   // Try a low acceleration move with a high max velocity and verify the
   // acceleration is capped like expected.
   {
-    auto message = this->subsystem_goal_sender_.MakeMessage();
-    message->unsafe_goal = kRange.lower;
-    message->profile_params.max_velocity = 20.0;
-    message->profile_params.max_acceleration = 0.1;
-    EXPECT_TRUE(message.Send());
+    auto message = this->subsystem_goal_sender_.MakeBuilder();
+    auto profile_builder = message.template MakeBuilder<frc971::ProfileParameters>();
+    profile_builder.add_max_velocity(20.0);
+    profile_builder.add_max_acceleration(0.1);
+    EXPECT_TRUE(message.Send(zeroing::testing::CreateSubsystemGoal(
+        *message.fbb(), kRange.lower, profile_builder.Finish())));
   }
   this->set_peak_subsystem_velocity(23.0);
   this->set_peak_subsystem_acceleration(0.2);
@@ -420,11 +486,13 @@
 
   // Now do a high acceleration move with a low velocity limit.
   {
-    auto message = this->subsystem_goal_sender_.MakeMessage();
-    message->unsafe_goal = kRange.upper;
-    message->profile_params.max_velocity = 0.1;
-    message->profile_params.max_acceleration = 100;
-    EXPECT_TRUE(message.Send());
+    auto message = this->subsystem_goal_sender_.MakeBuilder();
+    auto profile_builder =
+        message.template MakeBuilder<frc971::ProfileParameters>();
+    profile_builder.add_max_velocity(0.1);
+    profile_builder.add_max_acceleration(100.0);
+    EXPECT_TRUE(message.Send(zeroing::testing::CreateSubsystemGoal(
+        *message.fbb(), kRange.upper, profile_builder.Finish())));
   }
 
   this->set_peak_subsystem_velocity(0.2);
@@ -441,34 +509,34 @@
 
   // Set some ridiculous goals to test upper limits.
   {
-    auto message = this->subsystem_goal_sender_.MakeMessage();
-    message->unsafe_goal = 100.0;
-    message->profile_params.max_velocity = 1;
-    message->profile_params.max_acceleration = 0.5;
-    EXPECT_TRUE(message.Send());
+    auto message = this->subsystem_goal_sender_.MakeBuilder();
+    auto profile_builder = message.template MakeBuilder<frc971::ProfileParameters>();
+    profile_builder.add_max_velocity(1.0);
+    profile_builder.add_max_acceleration(0.5);
+    EXPECT_TRUE(message.Send(zeroing::testing::CreateSubsystemGoal(
+        *message.fbb(), 100.0, profile_builder.Finish())));
   }
   this->RunFor(chrono::seconds(10));
 
   // Check that we are near our soft limit.
   EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
-  EXPECT_NEAR(kRange.upper, this->subsystem_status_fetcher_->status.position,
-              0.001);
+  EXPECT_NEAR(kRange.upper, this->subsystem_status_fetcher_->position(), 0.001);
 
   // Set some ridiculous goals to test lower limits.
   {
-    auto message = this->subsystem_goal_sender_.MakeMessage();
-    message->unsafe_goal = -100.0;
-    message->profile_params.max_velocity = 1;
-    message->profile_params.max_acceleration = 0.5;
-    EXPECT_TRUE(message.Send());
+    auto message = this->subsystem_goal_sender_.MakeBuilder();
+    auto profile_builder = message.template MakeBuilder<frc971::ProfileParameters>();
+    profile_builder.add_max_velocity(1.0);
+    profile_builder.add_max_acceleration(0.5);
+    EXPECT_TRUE(message.Send(zeroing::testing::CreateSubsystemGoal(
+        *message.fbb(), -100.0, profile_builder.Finish())));
   }
 
   this->RunFor(chrono::seconds(10));
 
   // Check that we are near our soft limit.
   EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
-  EXPECT_NEAR(kRange.lower, this->subsystem_status_fetcher_->status.position,
-              0.001);
+  EXPECT_NEAR(kRange.lower, this->subsystem_status_fetcher_->position(), 0.001);
 }
 
 // Tests that the subsystem loop zeroes when run for a while.
@@ -476,11 +544,12 @@
   this->SetEnabled(true);
 
   {
-    auto message = this->subsystem_goal_sender_.MakeMessage();
-    message->unsafe_goal = kRange.upper;
-    message->profile_params.max_velocity = 1;
-    message->profile_params.max_acceleration = 0.5;
-    EXPECT_TRUE(message.Send());
+    auto message = this->subsystem_goal_sender_.MakeBuilder();
+    auto profile_builder = message.template MakeBuilder<frc971::ProfileParameters>();
+    profile_builder.add_max_velocity(1.0);
+    profile_builder.add_max_acceleration(0.5);
+    EXPECT_TRUE(message.Send(zeroing::testing::CreateSubsystemGoal(
+        *message.fbb(), kRange.upper, profile_builder.Finish())));
   }
 
   this->RunFor(chrono::seconds(10));
@@ -500,9 +569,9 @@
   this->SetEnabled(true);
   this->subsystem_plant_.InitializeSubsystemPosition(kRange.lower_hard);
   {
-    auto message = this->subsystem_goal_sender_.MakeMessage();
-    message->unsafe_goal = kRange.upper;
-    EXPECT_TRUE(message.Send());
+    auto message = this->subsystem_goal_sender_.MakeBuilder();
+    EXPECT_TRUE(message.Send(
+        zeroing::testing::CreateSubsystemGoal(*message.fbb(), kRange.upper)));
   }
   this->RunFor(chrono::seconds(10));
 
@@ -515,9 +584,9 @@
 
   this->subsystem_plant_.InitializeSubsystemPosition(kRange.upper_hard);
   {
-    auto message = this->subsystem_goal_sender_.MakeMessage();
-    message->unsafe_goal = kRange.upper;
-    EXPECT_TRUE(message.Send());
+    auto message = this->subsystem_goal_sender_.MakeBuilder();
+    EXPECT_TRUE(message.Send(
+        zeroing::testing::CreateSubsystemGoal(*message.fbb(), kRange.upper)));
   }
   this->RunFor(chrono::seconds(10));
 
@@ -530,9 +599,9 @@
 
   this->subsystem_plant_.InitializeSubsystemPosition(kRange.upper);
   {
-    auto message = this->subsystem_goal_sender_.MakeMessage();
-    message->unsafe_goal = kRange.upper - 0.1;
-    EXPECT_TRUE(message.Send());
+    auto message = this->subsystem_goal_sender_.MakeBuilder();
+    EXPECT_TRUE(message.Send(zeroing::testing::CreateSubsystemGoal(
+        *message.fbb(), kRange.upper - 0.1)));
   }
   this->RunFor(chrono::seconds(10));
 
@@ -554,9 +623,9 @@
 // Tests that the internal goals don't change while disabled.
 TYPED_TEST_P(IntakeSystemTest, DisabledGoalTest) {
   {
-    auto message = this->subsystem_goal_sender_.MakeMessage();
-    message->unsafe_goal = kRange.lower + 0.03;
-    EXPECT_TRUE(message.Send());
+    auto message = this->subsystem_goal_sender_.MakeBuilder();
+    EXPECT_TRUE(message.Send(zeroing::testing::CreateSubsystemGoal(
+        *message.fbb(), kRange.lower + 0.03)));
   }
 
   // Checks that the subsystem has not moved from its starting position at 0
@@ -572,9 +641,9 @@
 // Tests that zeroing while disabled works.
 TYPED_TEST_P(IntakeSystemTest, DisabledZeroTest) {
   {
-    auto message = this->subsystem_goal_sender_.MakeMessage();
-    message->unsafe_goal = kRange.lower;
-    EXPECT_TRUE(message.Send());
+    auto message = this->subsystem_goal_sender_.MakeBuilder();
+    EXPECT_TRUE(message.Send(
+        zeroing::testing::CreateSubsystemGoal(*message.fbb(), kRange.lower)));
   }
 
   // Run disabled for 2 seconds
@@ -591,16 +660,16 @@
 TYPED_TEST_P(IntakeSystemTest, MinPositionTest) {
   this->SetEnabled(true);
   {
-    auto message = this->subsystem_goal_sender_.MakeMessage();
-    message->unsafe_goal = kRange.lower_hard;
-    EXPECT_TRUE(message.Send());
+    auto message = this->subsystem_goal_sender_.MakeBuilder();
+    EXPECT_TRUE(message.Send(
+        zeroing::testing::CreateSubsystemGoal(*message.fbb(), kRange.lower_hard)));
   }
   this->RunFor(chrono::seconds(2));
 
   // Check that kRange.lower is used as the default min position
   EXPECT_EQ(this->subsystem()->goal(0), kRange.lower);
   EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
-  EXPECT_NEAR(kRange.lower, this->subsystem_status_fetcher_->status.position,
+  EXPECT_NEAR(kRange.lower, this->subsystem_status_fetcher_->position(),
               0.001);
 
   // Set min position and check that the subsystem increases to that position
@@ -608,7 +677,7 @@
   this->RunFor(chrono::seconds(2));
   EXPECT_EQ(this->subsystem()->goal(0), kRange.lower + 0.05);
   EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
-  EXPECT_NEAR(kRange.lower + 0.05, this->subsystem_status_fetcher_->status.position,
+  EXPECT_NEAR(kRange.lower + 0.05, this->subsystem_status_fetcher_->position(),
               0.001);
 
   // Clear min position and check that the subsystem returns to kRange.lower
@@ -616,7 +685,7 @@
   this->RunFor(chrono::seconds(2));
   EXPECT_EQ(this->subsystem()->goal(0), kRange.lower);
   EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
-  EXPECT_NEAR(kRange.lower, this->subsystem_status_fetcher_->status.position,
+  EXPECT_NEAR(kRange.lower, this->subsystem_status_fetcher_->position(),
               0.001);
 }
 
@@ -625,16 +694,16 @@
   this->SetEnabled(true);
 
   {
-    auto message = this->subsystem_goal_sender_.MakeMessage();
-    message->unsafe_goal = kRange.upper_hard;
-    EXPECT_TRUE(message.Send());
+    auto message = this->subsystem_goal_sender_.MakeBuilder();
+    EXPECT_TRUE(message.Send(zeroing::testing::CreateSubsystemGoal(
+        *message.fbb(), kRange.upper_hard)));
   }
   this->RunFor(chrono::seconds(2));
 
   // Check that kRange.upper is used as the default max position
   EXPECT_EQ(this->subsystem()->goal(0), kRange.upper);
   EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
-  EXPECT_NEAR(kRange.upper, this->subsystem_status_fetcher_->status.position,
+  EXPECT_NEAR(kRange.upper, this->subsystem_status_fetcher_->position(),
               0.001);
 
   // Set max position and check that the subsystem lowers to that position
@@ -642,7 +711,7 @@
   this->RunFor(chrono::seconds(2));
   EXPECT_EQ(this->subsystem()->goal(0), kRange.upper - 0.05);
   EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
-  EXPECT_NEAR(kRange.upper - 0.05, this->subsystem_status_fetcher_->status.position,
+  EXPECT_NEAR(kRange.upper - 0.05, this->subsystem_status_fetcher_->position(),
               0.001);
 
   // Clear max position and check that the subsystem returns to kRange.upper
@@ -650,7 +719,7 @@
   this->RunFor(chrono::seconds(2));
   EXPECT_EQ(this->subsystem()->goal(0), kRange.upper);
   EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
-  EXPECT_NEAR(kRange.upper, this->subsystem_status_fetcher_->status.position,
+  EXPECT_NEAR(kRange.upper, this->subsystem_status_fetcher_->position(),
               0.001);
 }
 
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.fbs b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.fbs
new file mode 100644
index 0000000..e39272a
--- /dev/null
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.fbs
@@ -0,0 +1,13 @@
+include "frc971/control_loops/profiled_subsystem.fbs";
+include "frc971/control_loops/control_loops.fbs";
+
+namespace frc971.control_loops.zeroing.testing;
+
+table SubsystemGoal {
+  unsafe_goal:double;
+  profile_params:frc971.ProfileParameters;
+}
+
+table SubsystemOutput {
+  output:double;
+}
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.q b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.q
deleted file mode 100644
index 3b35837..0000000
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.q
+++ /dev/null
@@ -1,46 +0,0 @@
-package frc971.control_loops;
-
-import "frc971/control_loops/profiled_subsystem.q";
-
-message StaticZeroingSingleDOFProfiledSubsystemTestGoal {
-  double unsafe_goal;
-  .frc971.ProfileParameters profile_params;
-};
-
-message StaticZeroingSingleDOFProfiledSubsystemOutput {
-  double output;
-};
-
-queue_group StaticZeroingSingleDOFProfiledSubsystemPotAndAbsoluteEncoderTestQueueGroup {
-  implements aos.control_loops.ControlLoop;
-
-  message Status {
-    PotAndAbsoluteEncoderProfiledJointStatus status;
-  };
-
-  message Position {
-    PotAndAbsolutePosition position;
-  };
-
-  queue StaticZeroingSingleDOFProfiledSubsystemTestGoal goal;
-  queue StaticZeroingSingleDOFProfiledSubsystemOutput output;
-  queue Status status;
-  queue Position position;
-};
-
-queue_group StaticZeroingSingleDOFProfiledSubsystemAbsoluteEncoderTestQueueGroup {
-  implements aos.control_loops.ControlLoop;
-
-  message Status {
-    AbsoluteEncoderProfiledJointStatus status;
-  };
-
-  message Position {
-    AbsolutePosition position;
-  };
-
-  queue StaticZeroingSingleDOFProfiledSubsystemTestGoal goal;
-  queue StaticZeroingSingleDOFProfiledSubsystemOutput output;
-  queue Status status;
-  queue Position position;
-};
diff --git a/frc971/control_loops/voltage_cap/voltage_cap_test.cc b/frc971/control_loops/voltage_cap/voltage_cap_test.cc
index 9ef4036..73898f9 100644
--- a/frc971/control_loops/voltage_cap/voltage_cap_test.cc
+++ b/frc971/control_loops/voltage_cap/voltage_cap_test.cc
@@ -4,7 +4,6 @@
 
 #include "gtest/gtest.h"
 
-#include "aos/queue.h"
 #include "aos/testing/test_shm.h"
 
 namespace frc971 {