diff --git a/y2021_bot3/BUILD b/y2021_bot3/BUILD
new file mode 100644
index 0000000..4e226c0
--- /dev/null
+++ b/y2021_bot3/BUILD
@@ -0,0 +1,119 @@
+load("//frc971:downloader.bzl", "robot_downloader")
+load("//aos:config.bzl", "aos_config")
+load("@com_google_protobuf//:protobuf.bzl", "cc_proto_library")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+
+robot_downloader(
+    data = [
+        ":config",
+    ],
+    start_binaries = [
+        ":joystick_reader",
+        ":wpilib_interface",
+        "//y2021_bot3/control_loops/drivetrain:drivetrain",
+        "//y2021_bot3/control_loops/superstructure:superstructure",
+        "//y2021_bot3/actors:binaries",
+    ],
+)
+
+cc_library(
+    name = "constants",
+    srcs = [
+        "constants.cc",
+    ],
+    hdrs = [
+        "constants.h",
+    ],
+    visibility = ["//visibility:public"],
+    deps = [
+        "//aos/logging",
+        "//aos/mutex",
+        "//aos/network:team_number",
+        "//frc971:constants",
+        "//frc971/control_loops:pose",
+        "//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
+        "//y2021_bot3/control_loops/drivetrain:polydrivetrain_plants",
+        "@com_google_absl//absl/base",
+    ],
+)
+
+cc_binary(
+    name = "wpilib_interface",
+    srcs = [
+        "wpilib_interface.cc",
+    ],
+    target_compatible_with = ["//tools/platforms/hardware:roborio"],
+    deps = [
+        ":constants",
+        "//aos:init",
+        "//aos:make_unique",
+        "//aos:math",
+        "//aos/controls:control_loop",
+        "//aos/events:shm_event_loop",
+        "//aos/logging",
+        "//aos/robot_state:robot_state_fbs",
+        "//aos/stl_mutex",
+        "//aos/time",
+        "//aos/util:log_interval",
+        "//aos/util:phased_loop",
+        "//aos/util:wrapping_counter",
+        "//frc971/autonomous:auto_mode_fbs",
+        "//frc971/control_loops:control_loops_fbs",
+        "//frc971/control_loops/drivetrain:drivetrain_position_fbs",
+        "//frc971/wpilib:ADIS16448",
+        "//frc971/wpilib:buffered_pcm",
+        "//frc971/wpilib:drivetrain_writer",
+        "//frc971/wpilib:encoder_and_potentiometer",
+        "//frc971/wpilib:interrupt_edge_counting",
+        "//frc971/wpilib:joystick_sender",
+        "//frc971/wpilib:logging_fbs",
+        "//frc971/wpilib:loop_output_handler",
+        "//frc971/wpilib:pdp_fetcher",
+        "//frc971/wpilib:sensor_reader",
+        "//frc971/wpilib:wpilib_interface",
+        "//frc971/wpilib:wpilib_robot_base",
+        "//third_party:phoenix",
+        "//third_party:wpilib",
+        "//y2021_bot3/control_loops/superstructure:superstructure_output_fbs",
+        "//y2021_bot3/control_loops/superstructure:superstructure_position_fbs",
+    ],
+)
+
+cc_binary(
+    name = "joystick_reader",
+    srcs = [
+        ":joystick_reader.cc",
+    ],
+    deps = [
+        "//aos:init",
+        "//aos/actions:action_lib",
+        "//aos/input:action_joystick_input",
+        "//aos/input:drivetrain_input",
+        "//aos/input:joystick_input",
+        "//aos/logging",
+        "//frc971/autonomous:auto_fbs",
+        "//frc971/autonomous:base_autonomous_actor",
+        "//frc971/control_loops:profiled_subsystem_fbs",
+        "//y2021_bot3/control_loops/drivetrain:drivetrain_base",
+        "//y2021_bot3/control_loops/superstructure:superstructure_goal_fbs",
+        "//y2021_bot3/control_loops/superstructure:superstructure_status_fbs",
+    ],
+)
+
+aos_config(
+    name = "config",
+    src = "y2021_bot3.json",
+    flatbuffers = [
+        "//y2021_bot3/control_loops/superstructure:superstructure_goal_fbs",
+        "//y2021_bot3/control_loops/superstructure:superstructure_output_fbs",
+        "//y2021_bot3/control_loops/superstructure:superstructure_position_fbs",
+        "//y2021_bot3/control_loops/superstructure:superstructure_status_fbs",
+    ],
+    visibility = ["//visibility:public"],
+    deps = [
+        "//aos/robot_state:config",
+        "//frc971/autonomous:config",
+        "//frc971/control_loops/drivetrain:config",
+        "//frc971/wpilib:config",
+    ],
+)
diff --git a/y2021_bot3/actors/BUILD b/y2021_bot3/actors/BUILD
new file mode 100644
index 0000000..1486c73
--- /dev/null
+++ b/y2021_bot3/actors/BUILD
@@ -0,0 +1,53 @@
+filegroup(
+    name = "binaries.stripped",
+    srcs = [
+        ":autonomous_action.stripped",
+    ],
+    visibility = ["//visibility:public"],
+)
+
+filegroup(
+    name = "binaries",
+    srcs = [
+        ":autonomous_action",
+    ],
+    visibility = ["//visibility:public"],
+)
+
+cc_library(
+    name = "autonomous_action_lib",
+    srcs = [
+        "auto_splines.cc",
+        "autonomous_actor.cc",
+    ],
+    hdrs = [
+        "auto_splines.h",
+        "autonomous_actor.h",
+    ],
+    deps = [
+        "//aos/events:event_loop",
+        "//aos/logging",
+        "//aos/util:phased_loop",
+        "//frc971/autonomous:base_autonomous_actor",
+        "//frc971/control_loops:control_loops_fbs",
+        "//frc971/control_loops:profiled_subsystem_fbs",
+        "//frc971/control_loops/drivetrain:drivetrain_config",
+        "//frc971/control_loops/drivetrain:localizer_fbs",
+        "//y2021_bot3/control_loops/drivetrain:drivetrain_base",
+        "//y2021_bot3/control_loops/superstructure:superstructure_goal_fbs",
+        "//y2021_bot3/control_loops/superstructure:superstructure_status_fbs",
+    ],
+)
+
+cc_binary(
+    name = "autonomous_action",
+    srcs = [
+        "autonomous_actor_main.cc",
+    ],
+    deps = [
+        ":autonomous_action_lib",
+        "//aos:init",
+        "//aos/events:shm_event_loop",
+        "//frc971/autonomous:auto_fbs",
+    ],
+)
diff --git a/y2021_bot3/actors/auto_splines.cc b/y2021_bot3/actors/auto_splines.cc
new file mode 100644
index 0000000..c6a5fd3
--- /dev/null
+++ b/y2021_bot3/actors/auto_splines.cc
@@ -0,0 +1,103 @@
+#include "y2021_bot3/actors/auto_splines.h"
+
+#include "frc971/control_loops/control_loops_generated.h"
+
+namespace y2021_bot3 {
+namespace actors {
+
+void MaybeFlipSpline(
+    aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+    flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset,
+    bool is_left) {
+  flatbuffers::Vector<float> *spline_y =
+      GetMutableTemporaryPointer(*builder->fbb(), spline_y_offset);
+
+  if (!is_left) {
+    for (size_t i = 0; i < spline_y->size(); i++) {
+      spline_y->Mutate(i, -spline_y->Get(i));
+    }
+  }
+}
+
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::BasicSSpline(
+    aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder) {
+  flatbuffers::Offset<frc971::Constraint> longitudinal_constraint_offset;
+  flatbuffers::Offset<frc971::Constraint> lateral_constraint_offset;
+  flatbuffers::Offset<frc971::Constraint> voltage_constraint_offset;
+
+  {
+    frc971::Constraint::Builder longitudinal_constraint_builder =
+        builder->MakeBuilder<frc971::Constraint>();
+    longitudinal_constraint_builder.add_constraint_type(
+        frc971::ConstraintType::LONGITUDINAL_ACCELERATION);
+    longitudinal_constraint_builder.add_value(1.0);
+    longitudinal_constraint_offset = longitudinal_constraint_builder.Finish();
+  }
+
+  {
+    frc971::Constraint::Builder lateral_constraint_builder =
+        builder->MakeBuilder<frc971::Constraint>();
+    lateral_constraint_builder.add_constraint_type(
+        frc971::ConstraintType::LATERAL_ACCELERATION);
+    lateral_constraint_builder.add_value(1.0);
+    lateral_constraint_offset = lateral_constraint_builder.Finish();
+  }
+
+  {
+    frc971::Constraint::Builder voltage_constraint_builder =
+        builder->MakeBuilder<frc971::Constraint>();
+    voltage_constraint_builder.add_constraint_type(
+        frc971::ConstraintType::VOLTAGE);
+    voltage_constraint_builder.add_value(6.0);
+    voltage_constraint_offset = voltage_constraint_builder.Finish();
+  }
+
+  flatbuffers::Offset<
+      flatbuffers::Vector<flatbuffers::Offset<frc971::Constraint>>>
+      constraints_offset =
+          builder->fbb()->CreateVector<flatbuffers::Offset<frc971::Constraint>>(
+              {longitudinal_constraint_offset, lateral_constraint_offset,
+               voltage_constraint_offset});
+
+  const float startx = 0.4;
+  const float starty = 3.4;
+  flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+      builder->fbb()->CreateVector<float>({0.0f + startx, 0.6f + startx,
+                                           0.6f + startx, 0.4f + startx,
+                                           0.4f + startx, 1.0f + startx});
+  flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+      builder->fbb()->CreateVector<float>({starty - 0.0f, starty - 0.0f,
+                                           starty - 0.3f, starty - 0.7f,
+                                           starty - 1.0f, starty - 1.0f});
+
+  frc971::MultiSpline::Builder multispline_builder =
+      builder->MakeBuilder<frc971::MultiSpline>();
+
+  multispline_builder.add_spline_count(1);
+  multispline_builder.add_constraints(constraints_offset);
+  multispline_builder.add_spline_x(spline_x_offset);
+  multispline_builder.add_spline_y(spline_y_offset);
+
+  return multispline_builder.Finish();
+}
+
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::StraightLine(
+    aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder) {
+  flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+      builder->fbb()->CreateVector<float>(
+          {-12.3, -11.9, -11.5, -11.1, -10.6, -10.0});
+  flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+      builder->fbb()->CreateVector<float>({1.25, 1.25, 1.25, 1.25, 1.25, 1.25});
+
+  frc971::MultiSpline::Builder multispline_builder =
+      builder->MakeBuilder<frc971::MultiSpline>();
+
+  multispline_builder.add_spline_count(1);
+  multispline_builder.add_spline_x(spline_x_offset);
+  multispline_builder.add_spline_y(spline_y_offset);
+
+  return multispline_builder.Finish();
+}
+
+}  // namespace actors
+}  // namespace y2021_bot3
diff --git a/y2021_bot3/actors/auto_splines.h b/y2021_bot3/actors/auto_splines.h
new file mode 100644
index 0000000..9984c15
--- /dev/null
+++ b/y2021_bot3/actors/auto_splines.h
@@ -0,0 +1,28 @@
+#ifndef Y2021_BOT3_ACTORS_AUTO_SPLINES_H_
+#define Y2021_BOT3_ACTORS_AUTO_SPLINES_H_
+
+#include "aos/events/event_loop.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+/*
+
+  The cooridinate system for the autonomous splines is the same as the spline
+  python generator and drivetrain spline systems.
+
+*/
+
+namespace y2021_bot3 {
+namespace actors {
+
+class AutonomousSplines {
+ public:
+  static flatbuffers::Offset<frc971::MultiSpline> BasicSSpline(
+      aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder);
+  static flatbuffers::Offset<frc971::MultiSpline> StraightLine(
+      aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder);
+};
+
+}  // namespace actors
+}  // namespace y2021_bot3
+
+#endif  // Y2021_BOT3_ACTORS_AUTO_SPLINES_H_
diff --git a/y2021_bot3/actors/autonomous_actor.cc b/y2021_bot3/actors/autonomous_actor.cc
new file mode 100644
index 0000000..39174d3
--- /dev/null
+++ b/y2021_bot3/actors/autonomous_actor.cc
@@ -0,0 +1,38 @@
+#include "y2021_bot3/actors/autonomous_actor.h"
+
+#include <inttypes.h>
+
+#include <chrono>
+#include <cmath>
+
+#include "aos/logging/logging.h"
+#include "frc971/control_loops/drivetrain/localizer_generated.h"
+#include "y2021_bot3/control_loops/drivetrain/drivetrain_base.h"
+
+namespace y2021_bot3 {
+namespace actors {
+
+using ::aos::monotonic_clock;
+using ::frc971::ProfileParametersT;
+using frc971::control_loops::drivetrain::LocalizerControl;
+namespace chrono = ::std::chrono;
+
+AutonomousActor::AutonomousActor(::aos::EventLoop *event_loop)
+    : frc971::autonomous::BaseAutonomousActor(
+          event_loop, control_loops::drivetrain::GetDrivetrainConfig()) {}
+
+void AutonomousActor::Reset() {
+  InitializeEncoders();
+  ResetDrivetrain();
+}
+
+bool AutonomousActor::RunAction(
+    const ::frc971::autonomous::AutonomousActionParams *params) {
+  Reset();
+
+  AOS_LOG(INFO, "Params are %d\n", params->mode());
+  return true;
+}
+
+}  // namespace actors
+}  // namespace y2021_bot3
diff --git a/y2021_bot3/actors/autonomous_actor.h b/y2021_bot3/actors/autonomous_actor.h
new file mode 100644
index 0000000..5fa509b
--- /dev/null
+++ b/y2021_bot3/actors/autonomous_actor.h
@@ -0,0 +1,27 @@
+#ifndef Y2021_BOT3_ACTORS_AUTONOMOUS_ACTOR_H_
+#define Y2021_BOT3_ACTORS_AUTONOMOUS_ACTOR_H_
+
+#include "aos/actions/actions.h"
+#include "aos/actions/actor.h"
+#include "frc971/autonomous/base_autonomous_actor.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+
+namespace y2021_bot3 {
+namespace actors {
+
+class AutonomousActor : public ::frc971::autonomous::BaseAutonomousActor {
+ public:
+  explicit AutonomousActor(::aos::EventLoop *event_loop);
+
+  bool RunAction(
+      const ::frc971::autonomous::AutonomousActionParams *params) override;
+
+ private:
+  void Reset();
+};
+
+}  // namespace actors
+}  // namespace y2021_bot3
+
+#endif  // Y2021_BOT3_ACTORS_AUTONOMOUS_ACTOR_H_
diff --git a/y2021_bot3/actors/autonomous_actor_main.cc b/y2021_bot3/actors/autonomous_actor_main.cc
new file mode 100644
index 0000000..cd71dc8
--- /dev/null
+++ b/y2021_bot3/actors/autonomous_actor_main.cc
@@ -0,0 +1,19 @@
+#include <stdio.h>
+
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "y2021_bot3/actors/autonomous_actor.h"
+
+int main(int argc, char *argv[]) {
+  ::aos::InitGoogle(&argc, &argv);
+
+  aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+      aos::configuration::ReadConfig("config.json");
+
+  ::aos::ShmEventLoop event_loop(&config.message());
+  ::y2021_bot3::actors::AutonomousActor autonomous(&event_loop);
+
+  event_loop.Run();
+
+  return 0;
+}
diff --git a/y2021_bot3/constants.cc b/y2021_bot3/constants.cc
new file mode 100644
index 0000000..984b844
--- /dev/null
+++ b/y2021_bot3/constants.cc
@@ -0,0 +1,84 @@
+#include "y2021_bot3/constants.h"
+
+#include <inttypes.h>
+
+#include <map>
+
+#if __has_feature(address_sanitizer)
+#include "sanitizer/lsan_interface.h"
+#endif
+
+#include "absl/base/call_once.h"
+#include "aos/logging/logging.h"
+#include "aos/mutex/mutex.h"
+#include "aos/network/team_number.h"
+
+namespace y2021_bot3 {
+namespace constants {
+
+const int Values::kZeroingSampleSize;
+
+namespace {
+
+const uint16_t kCompTeamNumber = 971;
+const uint16_t kPracticeTeamNumber = 9971;
+const uint16_t kCodingRobotTeamNumber = 7971;
+
+const Values *DoGetValuesForTeam(uint16_t team) {
+  Values *const r = new Values();
+
+  switch (team) {
+    // A set of constants for tests.
+    case 1:
+      break;
+
+    case kCompTeamNumber:
+      break;
+
+    case kPracticeTeamNumber:
+      break;
+
+    case kCodingRobotTeamNumber:
+      break;
+
+    default:
+      AOS_LOG(FATAL, "unknown team #%" PRIu16 "\n", team);
+  }
+
+  return r;
+}
+
+void DoGetValues(const Values **result) {
+  uint16_t team = ::aos::network::GetTeamNumber();
+  AOS_LOG(INFO, "creating a Constants for team %" PRIu16 "\n", team);
+  *result = DoGetValuesForTeam(team);
+}
+
+}  // namespace
+
+const Values &GetValues() {
+  static absl::once_flag once;
+  static const Values *result;
+  absl::call_once(once, DoGetValues, &result);
+  return *result;
+}
+
+const Values &GetValuesForTeam(uint16_t team_number) {
+  static ::aos::Mutex mutex;
+  ::aos::MutexLocker locker(&mutex);
+
+  // IMPORTANT: This declaration has to stay after the mutex is locked to avoid
+  // race conditions.
+  static ::std::map<uint16_t, const Values *> values;
+
+  if (values.count(team_number) == 0) {
+    values[team_number] = DoGetValuesForTeam(team_number);
+#if __has_feature(address_sanitizer)
+    __lsan_ignore_object(values[team_number]);
+#endif
+  }
+  return *values[team_number];
+}
+
+}  // namespace constants
+}  // namespace y2021_bot3
diff --git a/y2021_bot3/constants.h b/y2021_bot3/constants.h
new file mode 100644
index 0000000..fa95550
--- /dev/null
+++ b/y2021_bot3/constants.h
@@ -0,0 +1,44 @@
+#ifndef Y2021_BOT3_CONSTANTS_H_
+#define Y2021_BOT3_CONSTANTS_H_
+
+#include <math.h>
+#include <stdint.h>
+
+#include <array>
+
+#include "frc971/constants.h"
+#include "frc971/control_loops/pose.h"
+#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
+#include "y2021_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+
+namespace y2021_bot3 {
+namespace constants {
+
+struct Values {
+  static const int kZeroingSampleSize = 200;
+
+  static constexpr double kDrivetrainCyclesPerRevolution() { return 512.0; }
+  static constexpr double kDrivetrainEncoderCountsPerRevolution() {
+    return kDrivetrainCyclesPerRevolution() * 4;
+  }
+  static constexpr double kDrivetrainEncoderRatio() { return (24.0 / 52.0); }
+  static constexpr double kMaxDrivetrainEncoderPulsesPerSecond() {
+    return control_loops::drivetrain::kFreeSpeed / (2.0 * M_PI) *
+           control_loops::drivetrain::kHighOutputRatio /
+           constants::Values::kDrivetrainEncoderRatio() *
+           kDrivetrainEncoderCountsPerRevolution();
+  }
+};
+
+// Creates (once) a Values instance for ::aos::network::GetTeamNumber() and
+// returns a reference to it.
+const Values &GetValues();
+
+// Creates Values instances for each team number it is called with and returns
+// them.
+const Values &GetValuesForTeam(uint16_t team_number);
+
+}  // namespace constants
+}  // namespace y2021_bot3
+
+#endif  // Y2021_BOT3_CONSTANTS_H_
diff --git a/y2021_bot3/control_loops/drivetrain/BUILD b/y2021_bot3/control_loops/drivetrain/BUILD
new file mode 100644
index 0000000..2741fd2
--- /dev/null
+++ b/y2021_bot3/control_loops/drivetrain/BUILD
@@ -0,0 +1,88 @@
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//tools/build_rules:select.bzl", "compiler_select", "cpu_select")
+
+genrule(
+    name = "genrule_drivetrain",
+    outs = [
+        "drivetrain_dog_motor_plant.h",
+        "drivetrain_dog_motor_plant.cc",
+        "kalman_drivetrain_motor_plant.h",
+        "kalman_drivetrain_motor_plant.cc",
+    ],
+    cmd = "$(location //y2021_bot3/control_loops/python:drivetrain) $(OUTS)",
+    target_compatible_with = ["@platforms//os:linux"],
+    tools = [
+        "//y2021_bot3/control_loops/python:drivetrain",
+    ],
+)
+
+genrule(
+    name = "genrule_polydrivetrain",
+    outs = [
+        "polydrivetrain_dog_motor_plant.h",
+        "polydrivetrain_dog_motor_plant.cc",
+        "polydrivetrain_cim_plant.h",
+        "polydrivetrain_cim_plant.cc",
+        "hybrid_velocity_drivetrain.h",
+        "hybrid_velocity_drivetrain.cc",
+    ],
+    cmd = "$(location //y2021_bot3/control_loops/python:polydrivetrain) $(OUTS)",
+    target_compatible_with = ["@platforms//os:linux"],
+    tools = [
+        "//y2021_bot3/control_loops/python:polydrivetrain",
+    ],
+)
+
+cc_library(
+    name = "polydrivetrain_plants",
+    srcs = [
+        "drivetrain_dog_motor_plant.cc",
+        "hybrid_velocity_drivetrain.cc",
+        "kalman_drivetrain_motor_plant.cc",
+        "polydrivetrain_dog_motor_plant.cc",
+    ],
+    hdrs = [
+        "drivetrain_dog_motor_plant.h",
+        "hybrid_velocity_drivetrain.h",
+        "kalman_drivetrain_motor_plant.h",
+        "polydrivetrain_dog_motor_plant.h",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//visibility:public"],
+    deps = [
+        "//frc971/control_loops:hybrid_state_feedback_loop",
+        "//frc971/control_loops:state_feedback_loop",
+    ],
+)
+
+cc_library(
+    name = "drivetrain_base",
+    srcs = [
+        "drivetrain_base.cc",
+    ],
+    hdrs = [
+        "drivetrain_base.h",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//visibility:public"],
+    deps = [
+        ":polydrivetrain_plants",
+        "//frc971:shifter_hall_effect",
+        "//frc971/control_loops/drivetrain:drivetrain_config",
+    ],
+)
+
+cc_binary(
+    name = "drivetrain",
+    srcs = [
+        "drivetrain_main.cc",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//visibility:public"],
+    deps = [
+        ":drivetrain_base",
+        "//aos:init",
+        "//aos/events:shm_event_loop",
+        "//frc971/control_loops/drivetrain:drivetrain_lib",
+    ],
+)
diff --git a/y2021_bot3/control_loops/drivetrain/drivetrain_base.cc b/y2021_bot3/control_loops/drivetrain/drivetrain_base.cc
new file mode 100644
index 0000000..5b4c120
--- /dev/null
+++ b/y2021_bot3/control_loops/drivetrain/drivetrain_base.cc
@@ -0,0 +1,63 @@
+#include "y2021_bot3/control_loops/drivetrain/drivetrain_base.h"
+
+#include <chrono>
+
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "y2021_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2021_bot3/control_loops/drivetrain/hybrid_velocity_drivetrain.h"
+#include "y2021_bot3/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
+#include "y2021_bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
+
+using ::frc971::control_loops::drivetrain::DrivetrainConfig;
+
+namespace chrono = ::std::chrono;
+
+namespace y2021_bot3 {
+namespace control_loops {
+namespace drivetrain {
+
+using ::frc971::constants::ShifterHallEffect;
+
+const ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.25, 0.75};
+
+const DrivetrainConfig<double> &GetDrivetrainConfig() {
+  static DrivetrainConfig<double> kDrivetrainConfig{
+      ::frc971::control_loops::drivetrain::ShifterType::SIMPLE_SHIFTER,
+      ::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
+      ::frc971::control_loops::drivetrain::GyroType::IMU_Z_GYRO,
+      ::frc971::control_loops::drivetrain::IMUType::IMU_FLIPPED_X,
+
+      drivetrain::MakeDrivetrainLoop,
+      drivetrain::MakeVelocityDrivetrainLoop,
+      drivetrain::MakeKFDrivetrainLoop,
+      drivetrain::MakeHybridVelocityDrivetrainLoop,
+
+      chrono::duration_cast<chrono::nanoseconds>(
+          chrono::duration<double>(drivetrain::kDt)),
+      drivetrain::kRobotRadius,
+      drivetrain::kWheelRadius,
+      drivetrain::kV,
+
+      drivetrain::kHighGearRatio,
+      drivetrain::kLowGearRatio,
+      drivetrain::kJ,
+      drivetrain::kMass,
+      kThreeStateDriveShifter,
+      kThreeStateDriveShifter,
+      true /* default_high_gear */,
+      0 /* down_offset if using constants use
+     constants::GetValues().down_error */
+      ,
+      0.7 /* wheel_non_linearity */,
+      1.2 /* quickturn_wheel_multiplier */,
+      1.2 /* wheel_multiplier */,
+      true /*pistol_grip_shift_enables_line_follow*/,
+  };
+
+  return kDrivetrainConfig;
+};
+
+}  // namespace drivetrain
+}  // namespace control_loops
+}  // namespace y2021_bot3
diff --git a/y2021_bot3/control_loops/drivetrain/drivetrain_base.h b/y2021_bot3/control_loops/drivetrain/drivetrain_base.h
new file mode 100644
index 0000000..7250f93
--- /dev/null
+++ b/y2021_bot3/control_loops/drivetrain/drivetrain_base.h
@@ -0,0 +1,17 @@
+#ifndef Y2021_BOT3_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
+#define Y2021_BOT3_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
+
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+
+namespace y2021_bot3 {
+namespace control_loops {
+namespace drivetrain {
+
+const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
+    &GetDrivetrainConfig();
+
+}  // namespace drivetrain
+}  // namespace control_loops
+}  // namespace y2021_bot3
+
+#endif  // Y2021_BOT3_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
diff --git a/y2021_bot3/control_loops/drivetrain/drivetrain_main.cc b/y2021_bot3/control_loops/drivetrain/drivetrain_main.cc
new file mode 100644
index 0000000..0fef75f
--- /dev/null
+++ b/y2021_bot3/control_loops/drivetrain/drivetrain_main.cc
@@ -0,0 +1,25 @@
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "frc971/control_loops/drivetrain/drivetrain.h"
+#include "y2021_bot3/control_loops/drivetrain/drivetrain_base.h"
+
+using ::frc971::control_loops::drivetrain::DrivetrainLoop;
+
+int main(int argc, char **argv) {
+  ::aos::InitGoogle(&argc, &argv);
+
+  aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+      aos::configuration::ReadConfig("config.json");
+
+  ::aos::ShmEventLoop event_loop(&config.message());
+  ::frc971::control_loops::drivetrain::DeadReckonEkf localizer(
+      &event_loop,
+      ::y2021_bot3::control_loops::drivetrain::GetDrivetrainConfig());
+  DrivetrainLoop drivetrain(
+      ::y2021_bot3::control_loops::drivetrain::GetDrivetrainConfig(),
+      &event_loop, &localizer);
+
+  event_loop.Run();
+
+  return 0;
+}
diff --git a/y2021_bot3/control_loops/python/BUILD b/y2021_bot3/control_loops/python/BUILD
new file mode 100644
index 0000000..7ade878
--- /dev/null
+++ b/y2021_bot3/control_loops/python/BUILD
@@ -0,0 +1,45 @@
+package(default_visibility = ["//y2021_bot3:__subpackages__"])
+
+py_binary(
+    name = "drivetrain",
+    srcs = [
+        "drivetrain.py",
+    ],
+    target_compatible_with = ["@platforms//cpu:x86_64"],
+    deps = [
+        "//external:python-gflags",
+        "//external:python-glog",
+        "//frc971/control_loops/python:drivetrain",
+    ],
+)
+
+py_binary(
+    name = "polydrivetrain",
+    srcs = [
+        "drivetrain.py",
+        "polydrivetrain.py",
+    ],
+    target_compatible_with = ["@platforms//cpu:x86_64"],
+    deps = [
+        "//external:python-gflags",
+        "//external:python-glog",
+        "//frc971/control_loops/python:polydrivetrain",
+    ],
+)
+
+py_library(
+    name = "polydrivetrain_lib",
+    srcs = [
+        "drivetrain.py",
+        "polydrivetrain.py",
+    ],
+    target_compatible_with = ["@platforms//cpu:x86_64"],
+    visibility = ["//visibility:public"],
+    deps = [
+        "//external:python-gflags",
+        "//external:python-glog",
+        "//frc971/control_loops/python:controls",
+        "//frc971/control_loops/python:drivetrain",
+        "//frc971/control_loops/python:polydrivetrain",
+    ],
+)
diff --git a/y2021_bot3/control_loops/python/drivetrain.py b/y2021_bot3/control_loops/python/drivetrain.py
new file mode 100644
index 0000000..e5b001a
--- /dev/null
+++ b/y2021_bot3/control_loops/python/drivetrain.py
@@ -0,0 +1,46 @@
+#!/usr/bin/python3
+
+from __future__ import print_function
+from frc971.control_loops.python import drivetrain
+from frc971.control_loops.python import control_loop
+import sys
+
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+
+kDrivetrain = drivetrain.DrivetrainParams(
+    J=6.0,
+    mass=58.0,
+    # TODO(austin): Measure radius a bit better.
+    robot_radius=0.7 / 2.0,
+    wheel_radius=6.0 * 0.0254 / 2.0,
+    motor_type=control_loop.Falcon(),
+    G=(8.0 / 70.0) * (17.0 / 24.0),
+    q_pos=0.24,
+    q_vel=2.5,
+    efficiency=0.80,
+    has_imu=True,
+    force=True,
+    kf_q_voltage=1.0,
+    controller_poles=[0.82, 0.82])
+
+
+def main(argv):
+    argv = FLAGS(argv)
+    glog.init()
+
+    if FLAGS.plot:
+        drivetrain.PlotDrivetrainMotions(kDrivetrain)
+    elif len(argv) != 5:
+        print("Expected .h file name and .cc file name")
+    else:
+        # Write the generated constants out to a file.
+        drivetrain.WriteDrivetrain(argv[1:3], argv[3:5], 'y2021_bot3', kDrivetrain)
+
+
+if __name__ == '__main__':
+    sys.exit(main(sys.argv))
diff --git a/y2021_bot3/control_loops/python/polydrivetrain.py b/y2021_bot3/control_loops/python/polydrivetrain.py
new file mode 100644
index 0000000..83576cc
--- /dev/null
+++ b/y2021_bot3/control_loops/python/polydrivetrain.py
@@ -0,0 +1,31 @@
+#!/usr/bin/python3
+
+import sys
+from y2021_bot3.control_loops.python import drivetrain
+from frc971.control_loops.python import polydrivetrain
+
+import gflags
+import glog
+
+__author__ = 'Austin Schuh (austin.linux@gmail.com)'
+
+FLAGS = gflags.FLAGS
+
+try:
+  gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+except gflags.DuplicateFlagError:
+  pass
+
+def main(argv):
+  if FLAGS.plot:
+    polydrivetrain.PlotPolyDrivetrainMotions(drivetrain.kDrivetrain)
+  elif len(argv) != 7:
+    glog.fatal('Expected .h file name and .cc file name')
+  else:
+    polydrivetrain.WritePolyDrivetrain(argv[1:3], argv[3:5], argv[5:7], 'y2021_bot3',
+                                       drivetrain.kDrivetrain)
+
+if __name__ == '__main__':
+  argv = FLAGS(sys.argv)
+  glog.init()
+  sys.exit(main(argv))
diff --git a/y2021_bot3/control_loops/superstructure/BUILD b/y2021_bot3/control_loops/superstructure/BUILD
new file mode 100644
index 0000000..6c716a5
--- /dev/null
+++ b/y2021_bot3/control_loops/superstructure/BUILD
@@ -0,0 +1,78 @@
+package(default_visibility = ["//visibility:public"])
+
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+
+flatbuffer_cc_library(
+    name = "superstructure_goal_fbs",
+    srcs = [
+        "superstructure_goal.fbs",
+    ],
+    gen_reflections = 1,
+    includes = [
+        "//frc971/control_loops:control_loops_fbs_includes",
+        "//frc971/control_loops:profiled_subsystem_fbs_includes",
+    ],
+)
+
+flatbuffer_cc_library(
+    name = "superstructure_output_fbs",
+    srcs = [
+        "superstructure_output.fbs",
+    ],
+    gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
+    name = "superstructure_status_fbs",
+    srcs = [
+        "superstructure_status.fbs",
+    ],
+    gen_reflections = 1,
+    includes = [
+        "//frc971/control_loops:control_loops_fbs_includes",
+        "//frc971/control_loops:profiled_subsystem_fbs_includes",
+    ],
+)
+
+flatbuffer_cc_library(
+    name = "superstructure_position_fbs",
+    srcs = [
+        "superstructure_position.fbs",
+    ],
+    gen_reflections = 1,
+    includes = [
+        "//frc971/control_loops:control_loops_fbs_includes",
+        "//frc971/control_loops:profiled_subsystem_fbs_includes",
+    ],
+)
+
+cc_library(
+    name = "superstructure_lib",
+    srcs = [
+        "superstructure.cc",
+    ],
+    hdrs = [
+        "superstructure.h",
+    ],
+    deps = [
+        ":superstructure_goal_fbs",
+        ":superstructure_output_fbs",
+        ":superstructure_position_fbs",
+        ":superstructure_status_fbs",
+        "//aos/controls:control_loop",
+        "//aos/events:event_loop",
+        "//y2021_bot3:constants",
+    ],
+)
+
+cc_binary(
+    name = "superstructure",
+    srcs = [
+        "superstructure_main.cc",
+    ],
+    deps = [
+        ":superstructure_lib",
+        "//aos:init",
+        "//aos/events:shm_event_loop",
+    ],
+)
diff --git a/y2021_bot3/control_loops/superstructure/superstructure.cc b/y2021_bot3/control_loops/superstructure/superstructure.cc
new file mode 100644
index 0000000..ceb479f
--- /dev/null
+++ b/y2021_bot3/control_loops/superstructure/superstructure.cc
@@ -0,0 +1,42 @@
+#include "y2021_bot3/control_loops/superstructure/superstructure.h"
+
+#include "aos/events/event_loop.h"
+
+namespace y2021_bot3 {
+namespace control_loops {
+namespace superstructure {
+
+using frc971::control_loops::AbsoluteEncoderProfiledJointStatus;
+using frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus;
+
+Superstructure::Superstructure(::aos::EventLoop *event_loop,
+                               const ::std::string &name)
+    : aos::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
+                                                                 name) {
+  event_loop->SetRuntimeRealtimePriority(30);
+}
+
+void Superstructure::RunIteration(const Goal * /*unsafe_goal*/,
+                                  const Position * /*position*/,
+                                  aos::Sender<Output>::Builder *output,
+                                  aos::Sender<Status>::Builder *status) {
+  if (WasReset()) {
+    AOS_LOG(ERROR, "WPILib reset, restarting\n");
+  }
+
+  if (output != nullptr) {
+    OutputT output_struct;
+    output->Send(Output::Pack(*output->fbb(), &output_struct));
+  }
+
+  Status::Builder status_builder = status->MakeBuilder<Status>();
+
+  status_builder.add_zeroed(true);
+  status_builder.add_estopped(false);
+
+  status->Send(status_builder.Finish());
+}
+
+}  // namespace superstructure
+}  // namespace control_loops
+}  // namespace y2021_bot3
diff --git a/y2021_bot3/control_loops/superstructure/superstructure.h b/y2021_bot3/control_loops/superstructure/superstructure.h
new file mode 100644
index 0000000..3c25fd0
--- /dev/null
+++ b/y2021_bot3/control_loops/superstructure/superstructure.h
@@ -0,0 +1,35 @@
+#ifndef Y2021_BOT3_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
+#define Y2021_BOT3_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
+
+#include "aos/controls/control_loop.h"
+#include "aos/events/event_loop.h"
+#include "y2021_bot3/constants.h"
+#include "y2021_bot3/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2021_bot3/control_loops/superstructure/superstructure_output_generated.h"
+#include "y2021_bot3/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2021_bot3/control_loops/superstructure/superstructure_status_generated.h"
+
+namespace y2021_bot3 {
+namespace control_loops {
+namespace superstructure {
+
+class Superstructure
+    : public ::aos::controls::ControlLoop<Goal, Position, Status, Output> {
+ public:
+  explicit Superstructure(::aos::EventLoop *event_loop,
+                          const ::std::string &name = "/superstructure");
+
+ protected:
+  virtual void RunIteration(const Goal *unsafe_goal, const Position *position,
+                            aos::Sender<Output>::Builder *output,
+                            aos::Sender<Status>::Builder *status) override;
+
+ private:
+  DISALLOW_COPY_AND_ASSIGN(Superstructure);
+};
+
+}  // namespace superstructure
+}  // namespace control_loops
+}  // namespace y2021_bot3
+
+#endif  // Y2021_BOT3_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
diff --git a/y2021_bot3/control_loops/superstructure/superstructure_goal.fbs b/y2021_bot3/control_loops/superstructure/superstructure_goal.fbs
new file mode 100644
index 0000000..c016cad
--- /dev/null
+++ b/y2021_bot3/control_loops/superstructure/superstructure_goal.fbs
@@ -0,0 +1,9 @@
+include "frc971/control_loops/profiled_subsystem.fbs";
+
+namespace y2021_bot3.control_loops.superstructure;
+
+table Goal {
+
+}
+
+root_type Goal;
diff --git a/y2021_bot3/control_loops/superstructure/superstructure_main.cc b/y2021_bot3/control_loops/superstructure/superstructure_main.cc
new file mode 100644
index 0000000..c366e53
--- /dev/null
+++ b/y2021_bot3/control_loops/superstructure/superstructure_main.cc
@@ -0,0 +1,19 @@
+#include "y2021_bot3/control_loops/superstructure/superstructure.h"
+
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+
+int main(int argc, char **argv) {
+  ::aos::InitGoogle(&argc, &argv);
+
+  aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+      aos::configuration::ReadConfig("config.json");
+
+  ::aos::ShmEventLoop event_loop(&config.message());
+  ::y2021_bot3::control_loops::superstructure::Superstructure superstructure(
+      &event_loop);
+
+  event_loop.Run();
+
+  return 0;
+}
diff --git a/y2021_bot3/control_loops/superstructure/superstructure_output.fbs b/y2021_bot3/control_loops/superstructure/superstructure_output.fbs
new file mode 100644
index 0000000..88bdf2f
--- /dev/null
+++ b/y2021_bot3/control_loops/superstructure/superstructure_output.fbs
@@ -0,0 +1,7 @@
+namespace y2021_bot3.control_loops.superstructure;
+
+table Output {
+
+}
+
+root_type Output;
diff --git a/y2021_bot3/control_loops/superstructure/superstructure_position.fbs b/y2021_bot3/control_loops/superstructure/superstructure_position.fbs
new file mode 100644
index 0000000..ecb2ea4
--- /dev/null
+++ b/y2021_bot3/control_loops/superstructure/superstructure_position.fbs
@@ -0,0 +1,9 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace y2021_bot3.control_loops.superstructure;
+
+table Position {
+
+}
+
+root_type Position;
diff --git a/y2021_bot3/control_loops/superstructure/superstructure_status.fbs b/y2021_bot3/control_loops/superstructure/superstructure_status.fbs
new file mode 100644
index 0000000..e615c62
--- /dev/null
+++ b/y2021_bot3/control_loops/superstructure/superstructure_status.fbs
@@ -0,0 +1,15 @@
+include "frc971/control_loops/control_loops.fbs";
+include "frc971/control_loops/profiled_subsystem.fbs";
+
+namespace y2021_bot3.control_loops.superstructure;
+
+table Status {
+  // All subsystems know their location.
+  zeroed:bool (id: 0);
+
+  // If true, we have aborted. This is the or of all subsystem estops.
+  estopped:bool (id: 1);
+
+}
+
+root_type Status;
diff --git a/y2021_bot3/joystick_reader.cc b/y2021_bot3/joystick_reader.cc
new file mode 100644
index 0000000..accb031
--- /dev/null
+++ b/y2021_bot3/joystick_reader.cc
@@ -0,0 +1,77 @@
+#include <math.h>
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+
+#include "aos/actions/actions.h"
+#include "aos/init.h"
+#include "aos/input/action_joystick_input.h"
+#include "aos/input/driver_station_data.h"
+#include "aos/input/drivetrain_input.h"
+#include "aos/input/joystick_input.h"
+#include "aos/logging/logging.h"
+#include "aos/network/team_number.h"
+#include "aos/util/log_interval.h"
+#include "frc971/autonomous/base_autonomous_actor.h"
+#include "y2021_bot3/control_loops/drivetrain/drivetrain_base.h"
+#include "y2021_bot3/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2021_bot3/control_loops/superstructure/superstructure_status_generated.h"
+
+using aos::input::driver_station::ButtonLocation;
+using aos::input::driver_station::ControlBit;
+using aos::input::driver_station::JoystickAxis;
+using aos::input::driver_station::POVLocation;
+
+namespace y2021_bot3 {
+namespace input {
+namespace joysticks {
+
+namespace superstructure = y2021_bot3::control_loops::superstructure;
+
+class Reader : public ::aos::input::ActionJoystickInput {
+ public:
+  Reader(::aos::EventLoop *event_loop)
+      : ::aos::input::ActionJoystickInput(
+            event_loop,
+            ::y2021_bot3::control_loops::drivetrain::GetDrivetrainConfig(),
+            ::aos::input::DrivetrainInputReader::InputType::kPistol, {}),
+        superstructure_goal_sender_(
+            event_loop->MakeSender<superstructure::Goal>("/superstructure")),
+        superstructure_status_fetcher_(
+            event_loop->MakeFetcher<superstructure::Status>(
+                "/superstructure")) {}
+
+  void AutoEnded() override { AOS_LOG(INFO, "Auto ended.\n"); }
+
+  void HandleTeleop(
+      const ::aos::input::driver_station::Data & /*data*/) override {
+    superstructure_status_fetcher_.Fetch();
+    if (!superstructure_status_fetcher_.get()) {
+      AOS_LOG(ERROR, "Got no superstructure status message.\n");
+      return;
+    }
+  }
+
+ private:
+  ::aos::Sender<superstructure::Goal> superstructure_goal_sender_;
+
+  ::aos::Fetcher<superstructure::Status> superstructure_status_fetcher_;
+};
+
+}  // namespace joysticks
+}  // namespace input
+}  // namespace y2021_bot3
+
+int main(int argc, char **argv) {
+  ::aos::InitGoogle(&argc, &argv);
+
+  aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+      aos::configuration::ReadConfig("config.json");
+
+  ::aos::ShmEventLoop event_loop(&config.message());
+  ::y2021_bot3::input::joysticks::Reader reader(&event_loop);
+
+  event_loop.Run();
+
+  return 0;
+}
diff --git a/y2021_bot3/wpilib_interface.cc b/y2021_bot3/wpilib_interface.cc
new file mode 100644
index 0000000..3dfcfb7
--- /dev/null
+++ b/y2021_bot3/wpilib_interface.cc
@@ -0,0 +1,250 @@
+#include <inttypes.h>
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+
+#include <array>
+#include <chrono>
+#include <cmath>
+#include <functional>
+#include <mutex>
+#include <thread>
+
+#include "ctre/phoenix/CANifier.h"
+#include "frc971/wpilib/ahal/AnalogInput.h"
+#include "frc971/wpilib/ahal/Counter.h"
+#include "frc971/wpilib/ahal/DigitalGlitchFilter.h"
+#include "frc971/wpilib/ahal/DriverStation.h"
+#include "frc971/wpilib/ahal/Encoder.h"
+#include "frc971/wpilib/ahal/TalonFX.h"
+#include "frc971/wpilib/ahal/VictorSP.h"
+#undef ERROR
+
+#include "aos/commonmath.h"
+#include "aos/events/event_loop.h"
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "aos/logging/logging.h"
+#include "aos/make_unique.h"
+#include "aos/realtime.h"
+#include "aos/robot_state/robot_state_generated.h"
+#include "aos/time/time.h"
+#include "aos/util/log_interval.h"
+#include "aos/util/phased_loop.h"
+#include "aos/util/wrapping_counter.h"
+#include "ctre/phoenix/motorcontrol/can/TalonSRX.h"
+#include "frc971/autonomous/auto_mode_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
+#include "frc971/wpilib/ADIS16448.h"
+#include "frc971/wpilib/buffered_pcm.h"
+#include "frc971/wpilib/buffered_solenoid.h"
+#include "frc971/wpilib/dma.h"
+#include "frc971/wpilib/drivetrain_writer.h"
+#include "frc971/wpilib/encoder_and_potentiometer.h"
+#include "frc971/wpilib/joystick_sender.h"
+#include "frc971/wpilib/logging_generated.h"
+#include "frc971/wpilib/loop_output_handler.h"
+#include "frc971/wpilib/pdp_fetcher.h"
+#include "frc971/wpilib/sensor_reader.h"
+#include "frc971/wpilib/wpilib_robot_base.h"
+#include "y2021_bot3/constants.h"
+#include "y2021_bot3/control_loops/superstructure/superstructure_output_generated.h"
+#include "y2021_bot3/control_loops/superstructure/superstructure_position_generated.h"
+
+using ::aos::monotonic_clock;
+using ::y2021_bot3::constants::Values;
+namespace superstructure = ::y2021_bot3::control_loops::superstructure;
+namespace chrono = ::std::chrono;
+using aos::make_unique;
+
+namespace y2021_bot3 {
+namespace wpilib {
+namespace {
+
+constexpr double kMaxBringupPower = 12.0;
+
+// TODO(Brian): Fix the interpretation of the result of GetRaw here and in the
+// DMA stuff and then removing the * 2.0 in *_translate.
+// The low bit is direction.
+
+// TODO(brian): Use ::std::max instead once we have C++14 so that can be
+// constexpr.
+template <typename T>
+constexpr T max(T a, T b) {
+  return (a > b) ? a : b;
+}
+
+template <typename T, typename... Rest>
+constexpr T max(T a, T b, T c, Rest... rest) {
+  return max(max(a, b), c, rest...);
+}
+
+double drivetrain_translate(int32_t in) {
+  return ((static_cast<double>(in) /
+           Values::kDrivetrainEncoderCountsPerRevolution()) *
+          (2.0 * M_PI)) *
+         Values::kDrivetrainEncoderRatio() *
+         control_loops::drivetrain::kWheelRadius;
+}
+
+double drivetrain_velocity_translate(double in) {
+  return (((1.0 / in) / Values::kDrivetrainCyclesPerRevolution()) *
+          (2.0 * M_PI)) *
+         Values::kDrivetrainEncoderRatio() *
+         control_loops::drivetrain::kWheelRadius;
+}
+
+constexpr double kMaxFastEncoderPulsesPerSecond =
+    Values::kMaxDrivetrainEncoderPulsesPerSecond();
+static_assert(kMaxFastEncoderPulsesPerSecond <= 1300000,
+              "fast encoders are too fast");
+constexpr double kMaxMediumEncoderPulsesPerSecond =
+    kMaxFastEncoderPulsesPerSecond;
+
+static_assert(kMaxMediumEncoderPulsesPerSecond <= 400000,
+              "medium encoders are too fast");
+
+}  // namespace
+
+// Class to send position messages with sensor readings to our loops.
+class SensorReader : public ::frc971::wpilib::SensorReader {
+ public:
+  SensorReader(::aos::ShmEventLoop *event_loop)
+      : ::frc971::wpilib::SensorReader(event_loop),
+        auto_mode_sender_(
+            event_loop->MakeSender<::frc971::autonomous::AutonomousMode>(
+                "/autonomous")),
+        superstructure_position_sender_(
+            event_loop->MakeSender<superstructure::Position>(
+                "/superstructure")),
+        drivetrain_position_sender_(
+            event_loop
+                ->MakeSender<::frc971::control_loops::drivetrain::Position>(
+                    "/drivetrain")) {
+    // Set to filter out anything shorter than 1/4 of the minimum pulse width
+    // we should ever see.
+    UpdateFastEncoderFilterHz(kMaxFastEncoderPulsesPerSecond);
+    UpdateMediumEncoderFilterHz(kMaxMediumEncoderPulsesPerSecond);
+  }
+
+  // Auto mode switches.
+  void set_autonomous_mode(int i, ::std::unique_ptr<frc::DigitalInput> sensor) {
+    autonomous_modes_.at(i) = ::std::move(sensor);
+  }
+
+  void RunIteration() override {
+    {
+      auto builder = drivetrain_position_sender_.MakeBuilder();
+      frc971::control_loops::drivetrain::Position::Builder drivetrain_builder =
+          builder.MakeBuilder<frc971::control_loops::drivetrain::Position>();
+      drivetrain_builder.add_left_encoder(
+          drivetrain_translate(drivetrain_left_encoder_->GetRaw()));
+      drivetrain_builder.add_left_speed(
+          drivetrain_velocity_translate(drivetrain_left_encoder_->GetPeriod()));
+
+      drivetrain_builder.add_right_encoder(
+          -drivetrain_translate(drivetrain_right_encoder_->GetRaw()));
+      drivetrain_builder.add_right_speed(-drivetrain_velocity_translate(
+          drivetrain_right_encoder_->GetPeriod()));
+
+      builder.Send(drivetrain_builder.Finish());
+    }
+
+    {
+      auto builder = superstructure_position_sender_.MakeBuilder();
+      superstructure::Position::Builder position_builder =
+          builder.MakeBuilder<superstructure::Position>();
+      builder.Send(position_builder.Finish());
+    }
+
+    {
+      auto builder = auto_mode_sender_.MakeBuilder();
+
+      uint32_t mode = 0;
+      for (size_t i = 0; i < autonomous_modes_.size(); ++i) {
+        if (autonomous_modes_[i] && autonomous_modes_[i]->Get()) {
+          mode |= 1 << i;
+        }
+      }
+
+      auto auto_mode_builder =
+          builder.MakeBuilder<frc971::autonomous::AutonomousMode>();
+
+      auto_mode_builder.add_mode(mode);
+
+      builder.Send(auto_mode_builder.Finish());
+    }
+  }
+
+ private:
+  ::aos::Sender<::frc971::autonomous::AutonomousMode> auto_mode_sender_;
+  ::aos::Sender<superstructure::Position> superstructure_position_sender_;
+  ::aos::Sender<::frc971::control_loops::drivetrain::Position>
+      drivetrain_position_sender_;
+
+  ::std::array<::std::unique_ptr<frc::DigitalInput>, 2> autonomous_modes_;
+};
+
+class SuperstructureWriter
+    : public ::frc971::wpilib::LoopOutputHandler<superstructure::Output> {
+ public:
+  SuperstructureWriter(::aos::EventLoop *event_loop)
+      : ::frc971::wpilib::LoopOutputHandler<superstructure::Output>(
+            event_loop, "/superstructure") {}
+
+ private:
+  void Write(const superstructure::Output & /*output*/) override {}
+
+  void Stop() override { AOS_LOG(WARNING, "Superstructure output too old.\n"); }
+};
+
+class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
+ public:
+  ::std::unique_ptr<frc::Encoder> make_encoder(int index) {
+    return make_unique<frc::Encoder>(10 + index * 2, 11 + index * 2, false,
+                                     frc::Encoder::k4X);
+  }
+
+  void Run() override {
+    aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+        aos::configuration::ReadConfig("config.json");
+
+    // Thread 1.
+    ::aos::ShmEventLoop joystick_sender_event_loop(&config.message());
+    ::frc971::wpilib::JoystickSender joystick_sender(
+        &joystick_sender_event_loop);
+    AddLoop(&joystick_sender_event_loop);
+
+    // Thread 2.
+    ::aos::ShmEventLoop pdp_fetcher_event_loop(&config.message());
+    ::frc971::wpilib::PDPFetcher pdp_fetcher(&pdp_fetcher_event_loop);
+    AddLoop(&pdp_fetcher_event_loop);
+
+    // Thread 3.
+    ::aos::ShmEventLoop sensor_reader_event_loop(&config.message());
+    SensorReader sensor_reader(&sensor_reader_event_loop);
+    sensor_reader.set_drivetrain_left_encoder(make_encoder(0));
+    sensor_reader.set_drivetrain_right_encoder(make_encoder(1));
+
+    AddLoop(&sensor_reader_event_loop);
+
+    // Thread 4.
+    ::aos::ShmEventLoop output_event_loop(&config.message());
+    ::frc971::wpilib::DrivetrainWriter drivetrain_writer(&output_event_loop);
+    drivetrain_writer.set_left_controller0(
+        ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(0)), true);
+    drivetrain_writer.set_right_controller0(
+        ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(1)), false);
+
+    SuperstructureWriter superstructure_writer(&output_event_loop);
+
+    AddLoop(&output_event_loop);
+
+    RunLoops();
+  }
+};
+
+}  // namespace wpilib
+}  // namespace y2021_bot3
+
+AOS_ROBOT_CLASS(::y2021_bot3::wpilib::WPILibRobot);
diff --git a/y2021_bot3/y2021_bot3.json b/y2021_bot3/y2021_bot3.json
new file mode 100644
index 0000000..d96ca09
--- /dev/null
+++ b/y2021_bot3/y2021_bot3.json
@@ -0,0 +1,39 @@
+{
+  "channels":
+  [
+    {
+      "name": "/superstructure",
+      "type": "y2021_bot3.control_loops.superstructure.Goal",
+      "frequency": 200
+    },
+    {
+      "name": "/superstructure",
+      "type": "y2021_bot3.control_loops.superstructure.Status",
+      "frequency": 200
+    },
+    {
+      "name": "/superstructure",
+      "type": "y2021_bot3.control_loops.superstructure.Output",
+      "frequency": 200
+    },
+    {
+      "name": "/superstructure",
+      "type": "y2021_bot3.control_loops.superstructure.Position",
+      "frequency": 200
+    }
+  ],
+  "applications": [
+    {
+      "name": "drivetrain"
+    },
+    {
+      "name": "superstructure"
+    }
+  ],
+  "imports": [
+    "../aos/robot_state/robot_state_config.json",
+    "../frc971/control_loops/drivetrain/drivetrain_config.json",
+    "../frc971/autonomous/autonomous_config.json",
+    "../frc971/wpilib/wpilib_config.json"
+  ]
+}