diff --git a/y2020/control_loops/BUILD b/y2020/control_loops/BUILD
new file mode 100644
index 0000000..c0aa1ee
--- /dev/null
+++ b/y2020/control_loops/BUILD
@@ -0,0 +1,6 @@
+py_library(
+    name = "python_init",
+    srcs = ["__init__.py"],
+    visibility = ["//visibility:public"],
+    deps = ["//y2020:python_init"],
+)
diff --git a/y2020/control_loops/__init__.py b/y2020/control_loops/__init__.py
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/y2020/control_loops/__init__.py
diff --git a/y2020/control_loops/drivetrain/BUILD b/y2020/control_loops/drivetrain/BUILD
new file mode 100644
index 0000000..d03c1b9
--- /dev/null
+++ b/y2020/control_loops/drivetrain/BUILD
@@ -0,0 +1,83 @@
+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 //y2020/control_loops/python:drivetrain) $(OUTS)",
+    tools = [
+        "//y2020/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 //y2020/control_loops/python:polydrivetrain) $(OUTS)",
+    tools = [
+        "//y2020/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",
+    ],
+    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",
+    ],
+    visibility = ["//visibility:public"],
+    deps = [
+        ":polydrivetrain_plants",
+        "//frc971:shifter_hall_effect",
+        "//frc971/control_loops/drivetrain:drivetrain_config",
+    ],
+)
+
+cc_binary(
+    name = "drivetrain",
+    srcs = [
+        "drivetrain_main.cc",
+    ],
+    visibility = ["//visibility:public"],
+    deps = [
+        ":drivetrain_base",
+        "//aos:init",
+        "//aos/events:shm_event_loop",
+        "//frc971/control_loops/drivetrain:drivetrain_lib",
+    ],
+)
diff --git a/y2020/control_loops/drivetrain/drivetrain_base.cc b/y2020/control_loops/drivetrain/drivetrain_base.cc
new file mode 100644
index 0000000..c3597f5
--- /dev/null
+++ b/y2020/control_loops/drivetrain/drivetrain_base.cc
@@ -0,0 +1,63 @@
+#include "y2020/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 "y2020/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2020/control_loops/drivetrain/hybrid_velocity_drivetrain.h"
+#include "y2020/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
+#include "y2020/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
+
+using ::frc971::control_loops::drivetrain::DrivetrainConfig;
+
+namespace chrono = ::std::chrono;
+
+namespace y2020 {
+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 y2020
diff --git a/y2020/control_loops/drivetrain/drivetrain_base.h b/y2020/control_loops/drivetrain/drivetrain_base.h
new file mode 100644
index 0000000..c220088
--- /dev/null
+++ b/y2020/control_loops/drivetrain/drivetrain_base.h
@@ -0,0 +1,17 @@
+#ifndef y2020_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
+#define y2020_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
+
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+
+namespace y2020 {
+namespace control_loops {
+namespace drivetrain {
+
+const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
+    &GetDrivetrainConfig();
+
+}  // namespace drivetrain
+}  // namespace control_loops
+}  // namespace y2020
+
+#endif  // y2020_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
diff --git a/y2020/control_loops/drivetrain/drivetrain_main.cc b/y2020/control_loops/drivetrain/drivetrain_main.cc
new file mode 100644
index 0000000..66b9cc7
--- /dev/null
+++ b/y2020/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 "y2020/control_loops/drivetrain/drivetrain_base.h"
+
+using ::frc971::control_loops::drivetrain::DrivetrainLoop;
+
+int main() {
+  ::aos::InitNRT(true);
+
+  aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+      aos::configuration::ReadConfig("config.json");
+
+  ::aos::ShmEventLoop event_loop(&config.message());
+  ::frc971::control_loops::drivetrain::DeadReckonEkf localizer(
+      &event_loop, ::y2020::control_loops::drivetrain::GetDrivetrainConfig());
+  DrivetrainLoop drivetrain(
+      ::y2020::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop,
+      &localizer);
+
+  event_loop.Run();
+
+  ::aos::Cleanup();
+  return 0;
+}
diff --git a/y2020/control_loops/python/BUILD b/y2020/control_loops/python/BUILD
new file mode 100644
index 0000000..edc72f5
--- /dev/null
+++ b/y2020/control_loops/python/BUILD
@@ -0,0 +1,57 @@
+package(default_visibility = ["//y2020:__subpackages__"])
+
+py_binary(
+    name = "drivetrain",
+    srcs = [
+        "drivetrain.py",
+    ],
+    legacy_create_init = False,
+    restricted_to = ["//tools:k8"],
+    deps = [
+        ":python_init",
+        "//external:python-gflags",
+        "//external:python-glog",
+        "//frc971/control_loops/python:drivetrain",
+    ],
+)
+
+py_binary(
+    name = "polydrivetrain",
+    srcs = [
+        "drivetrain.py",
+        "polydrivetrain.py",
+    ],
+    legacy_create_init = False,
+    restricted_to = ["//tools:k8"],
+    deps = [
+        ":python_init",
+        "//external:python-gflags",
+        "//external:python-glog",
+        "//frc971/control_loops/python:polydrivetrain",
+    ],
+)
+
+py_library(
+    name = "polydrivetrain_lib",
+    srcs = [
+        "drivetrain.py",
+        "polydrivetrain.py",
+    ],
+    restricted_to = ["//tools:k8"],
+    visibility = ["//visibility:public"],
+    deps = [
+        ":python_init",
+        "//external:python-gflags",
+        "//external:python-glog",
+        "//frc971/control_loops/python:controls",
+        "//frc971/control_loops/python:drivetrain",
+        "//frc971/control_loops/python:polydrivetrain",
+    ],
+)
+
+py_library(
+    name = "python_init",
+    srcs = ["__init__.py"],
+    visibility = ["//visibility:public"],
+    deps = ["//y2020/control_loops:python_init"],
+)
diff --git a/y2020/control_loops/python/__init__.py b/y2020/control_loops/python/__init__.py
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/y2020/control_loops/python/__init__.py
diff --git a/y2020/control_loops/python/drivetrain.py b/y2020/control_loops/python/drivetrain.py
new file mode 100644
index 0000000..54745dd
--- /dev/null
+++ b/y2020/control_loops/python/drivetrain.py
@@ -0,0 +1,42 @@
+#!/usr/bin/python
+
+from frc971.control_loops.python import drivetrain
+import sys
+
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+
+kDrivetrain = drivetrain.DrivetrainParams(
+    J=1.5,
+    mass=38.5,
+    robot_radius=0.45 / 2.0,
+    wheel_radius=4.0 * 0.0254 / 2.0,
+    G=9.0 / 52.0,
+    q_pos=0.14,
+    q_vel=1.30,
+    efficiency=0.80,
+    has_imu=True,
+    force=True,
+    kf_q_voltage=13.0,
+    controller_poles=[0.82, 0.82],
+    robot_cg_offset=0.0)
+
+
+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], 'y2020', kDrivetrain)
+
+if __name__ == '__main__':
+    sys.exit(main(sys.argv))
diff --git a/y2020/control_loops/python/polydrivetrain.py b/y2020/control_loops/python/polydrivetrain.py
new file mode 100644
index 0000000..6481af1
--- /dev/null
+++ b/y2020/control_loops/python/polydrivetrain.py
@@ -0,0 +1,31 @@
+#!/usr/bin/python
+
+import sys
+from y2020.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], 'y2020',
+                                       drivetrain.kDrivetrain)
+
+if __name__ == '__main__':
+  argv = FLAGS(sys.argv)
+  glog.init()
+  sys.exit(main(argv))
diff --git a/y2020/control_loops/superstructure/BUILD b/y2020/control_loops/superstructure/BUILD
new file mode 100644
index 0000000..533a369
--- /dev/null
+++ b/y2020/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",
+        "//y2020:constants",
+    ],
+)
+
+cc_binary(
+    name = "superstructure",
+    srcs = [
+        "superstructure_main.cc",
+    ],
+    deps = [
+        ":superstructure_lib",
+        "//aos:init",
+        "//aos/events:shm_event_loop",
+    ],
+)
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
new file mode 100644
index 0000000..a3978a4
--- /dev/null
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -0,0 +1,43 @@
+#include "y2020/control_loops/superstructure/superstructure.h"
+
+#include "aos/events/event_loop.h"
+
+namespace y2020 {
+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 control_loops
+}  // namespace y2020
+}  // namespace y2020
diff --git a/y2020/control_loops/superstructure/superstructure.h b/y2020/control_loops/superstructure/superstructure.h
new file mode 100644
index 0000000..9aaca8e
--- /dev/null
+++ b/y2020/control_loops/superstructure/superstructure.h
@@ -0,0 +1,35 @@
+#ifndef y2020_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
+#define y2020_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
+
+#include "aos/controls/control_loop.h"
+#include "aos/events/event_loop.h"
+#include "y2020/constants.h"
+#include "y2020/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2020/control_loops/superstructure/superstructure_output_generated.h"
+#include "y2020/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2020/control_loops/superstructure/superstructure_status_generated.h"
+
+namespace y2020 {
+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 y2020
+
+#endif  // y2020_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
diff --git a/y2020/control_loops/superstructure/superstructure_goal.fbs b/y2020/control_loops/superstructure/superstructure_goal.fbs
new file mode 100644
index 0000000..8a38b95
--- /dev/null
+++ b/y2020/control_loops/superstructure/superstructure_goal.fbs
@@ -0,0 +1,9 @@
+include "frc971/control_loops/profiled_subsystem.fbs";
+
+namespace y2020.control_loops.superstructure;
+
+table Goal {
+
+}
+
+root_type Goal;
diff --git a/y2020/control_loops/superstructure/superstructure_main.cc b/y2020/control_loops/superstructure/superstructure_main.cc
new file mode 100644
index 0000000..101a0c7
--- /dev/null
+++ b/y2020/control_loops/superstructure/superstructure_main.cc
@@ -0,0 +1,20 @@
+#include "y2020/control_loops/superstructure/superstructure.h"
+
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+
+int main(int /*argc*/, char * /*argv*/ []) {
+  ::aos::InitNRT(true);
+
+  aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+      aos::configuration::ReadConfig("config.json");
+
+  ::aos::ShmEventLoop event_loop(&config.message());
+  ::y2020::control_loops::superstructure::Superstructure superstructure(
+      &event_loop);
+
+  event_loop.Run();
+
+  ::aos::Cleanup();
+  return 0;
+}
diff --git a/y2020/control_loops/superstructure/superstructure_output.fbs b/y2020/control_loops/superstructure/superstructure_output.fbs
new file mode 100644
index 0000000..3682db6
--- /dev/null
+++ b/y2020/control_loops/superstructure/superstructure_output.fbs
@@ -0,0 +1,7 @@
+namespace y2020.control_loops.superstructure;
+
+table Output {
+
+}
+
+root_type Output;
diff --git a/y2020/control_loops/superstructure/superstructure_position.fbs b/y2020/control_loops/superstructure/superstructure_position.fbs
new file mode 100644
index 0000000..c4b0a9a
--- /dev/null
+++ b/y2020/control_loops/superstructure/superstructure_position.fbs
@@ -0,0 +1,9 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace y2020.control_loops.superstructure;
+
+table Position {
+
+}
+
+root_type Position;
diff --git a/y2020/control_loops/superstructure/superstructure_status.fbs b/y2020/control_loops/superstructure/superstructure_status.fbs
new file mode 100644
index 0000000..d0e9266
--- /dev/null
+++ b/y2020/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 y2020.control_loops.superstructure;
+
+table Status {
+  // All subsystems know their location.
+  zeroed:bool;
+
+  // If true, we have aborted. This is the or of all subsystem estops.
+  estopped:bool;
+
+}
+
+root_type Status;