diff --git a/y2022/control_loops/superstructure/BUILD b/y2022/control_loops/superstructure/BUILD
index ccfb0ee..9bae7c7 100644
--- a/y2022/control_loops/superstructure/BUILD
+++ b/y2022/control_loops/superstructure/BUILD
@@ -62,6 +62,7 @@
         ":superstructure_status_fbs",
         "//aos/events:event_loop",
         "//frc971/control_loops:control_loop",
+        "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
         "//y2022:constants",
     ],
 )
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index a7af922..bf04774 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -8,13 +8,21 @@
 
 using frc971::control_loops::AbsoluteEncoderProfiledJointStatus;
 using frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus;
-using frc971::control_loops:: RelativeEncoderProfiledJointStatus;
+using frc971::control_loops::RelativeEncoderProfiledJointStatus;
 
 Superstructure::Superstructure(::aos::EventLoop *event_loop,
+                               std::shared_ptr<const constants::Values> values,
                                const ::std::string &name)
     : frc971::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
                                                                     name),
-      climber_(constants::GetValues().climber.subsystem_params) {
+
+      climber_(values->climber.subsystem_params),
+      intake_front_(values->intake_front.subsystem_params),
+      intake_back_(values->intake_back.subsystem_params),
+      turret_(values->turret.subsystem_params),
+      drivetrain_status_fetcher_(
+          event_loop->MakeFetcher<frc971::control_loops::drivetrain::Status>(
+              "/drivetrain")) {
   event_loop->SetRuntimeRealtimePriority(30);
 }
 
@@ -24,6 +32,29 @@
                                   aos::Sender<Status>::Builder *status) {
   if (WasReset()) {
     AOS_LOG(ERROR, "WPILib reset, restarting\n");
+    intake_front_.Reset();
+    intake_back_.Reset();
+    turret_.Reset();
+    climber_.Reset();
+  }
+
+  drivetrain_status_fetcher_.Fetch();
+  const float velocity = robot_velocity();
+
+  double roller_speed_compensated_front = 0;
+  double roller_speed_compensated_back = 0;
+  double transfer_roller_speed = 0;
+
+  if (unsafe_goal != nullptr) {
+    roller_speed_compensated_front =
+        unsafe_goal->roller_speed_front() +
+        std::max(velocity * unsafe_goal->roller_speed_compensation(), 0.0);
+
+    roller_speed_compensated_back =
+        unsafe_goal->roller_speed_back() -
+        std::min(velocity * unsafe_goal->roller_speed_compensation(), 0.0);
+
+    transfer_roller_speed = unsafe_goal->transfer_roller_speed();
   }
 
   OutputT output_struct;
@@ -35,20 +66,59 @@
           output != nullptr ? &(output_struct.climber_voltage) : nullptr,
           status->fbb());
 
+  flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
+      intake_status_offset_front = intake_front_.Iterate(
+          unsafe_goal != nullptr ? unsafe_goal->intake_front() : nullptr,
+          position->intake_front(),
+          output != nullptr ? &(output_struct.intake_voltage_front) : nullptr,
+          status->fbb());
+
+  flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
+      intake_status_offset_back = intake_back_.Iterate(
+          unsafe_goal != nullptr ? unsafe_goal->intake_back() : nullptr,
+          position->intake_back(),
+          output != nullptr ? &(output_struct.intake_voltage_back) : nullptr,
+          status->fbb());
+
+  flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
+      turret_status_offset = turret_.Iterate(
+          unsafe_goal != nullptr ? unsafe_goal->turret() : nullptr,
+          position->turret(),
+          output != nullptr ? &(output_struct.turret_voltage) : nullptr,
+          status->fbb());
+
   if (output != nullptr) {
+    output_struct.roller_voltage_front = roller_speed_compensated_front;
+    output_struct.roller_voltage_back = roller_speed_compensated_back;
+    output_struct.transfer_roller_voltage = transfer_roller_speed;
+
     output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
   }
 
   Status::Builder status_builder = status->MakeBuilder<Status>();
 
-  // Climber is always zeroed; only has a pot
-  status_builder.add_zeroed(climber_.zeroed());
-  status_builder.add_estopped(climber_.estopped());
+  const bool zeroed = intake_front_.zeroed() && intake_back_.zeroed() &&
+                      turret_.zeroed() && climber_.zeroed();
+  const bool estopped = intake_front_.estopped() || intake_back_.estopped() ||
+                        turret_.estopped() || climber_.zeroed();
+
+  status_builder.add_zeroed(zeroed);
+  status_builder.add_estopped(estopped);
+
+  status_builder.add_intake_front(intake_status_offset_front);
+  status_builder.add_intake_back(intake_status_offset_back);
+  status_builder.add_turret(turret_status_offset);
   status_builder.add_climber(climber_status_offset);
 
   (void)status->Send(status_builder.Finish());
 }
 
+double Superstructure::robot_velocity() const {
+  return (drivetrain_status_fetcher_.get() != nullptr
+              ? drivetrain_status_fetcher_->robot_speed()
+              : 0.0);
+}
+
 }  // namespace superstructure
 }  // namespace control_loops
 }  // namespace y2022
diff --git a/y2022/control_loops/superstructure/superstructure.h b/y2022/control_loops/superstructure/superstructure.h
index 4c03a08..d97befc 100644
--- a/y2022/control_loops/superstructure/superstructure.h
+++ b/y2022/control_loops/superstructure/superstructure.h
@@ -3,6 +3,7 @@
 
 #include "aos/events/event_loop.h"
 #include "frc971/control_loops/control_loop.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
 #include "y2022/constants.h"
 #include "y2022/control_loops/superstructure/superstructure_goal_generated.h"
 #include "y2022/control_loops/superstructure/superstructure_output_generated.h"
@@ -21,9 +22,28 @@
           ::frc971::zeroing::RelativeEncoderZeroingEstimator,
           ::frc971::control_loops::RelativeEncoderProfiledJointStatus>;
 
+  using PotAndAbsoluteEncoderSubsystem =
+      ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
+          ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator,
+          ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>;
+
   explicit Superstructure(::aos::EventLoop *event_loop,
+                         std::shared_ptr<const constants::Values> values,
                           const ::std::string &name = "/superstructure");
 
+  inline const PotAndAbsoluteEncoderSubsystem &intake_front() const {
+    return intake_front_;
+  }
+  inline const PotAndAbsoluteEncoderSubsystem &intake_back() const {
+    return intake_back_;
+  }
+  inline const PotAndAbsoluteEncoderSubsystem &turret() const {
+    return turret_;
+  }
+  inline const RelativeEncoderSubsystem &climber() const { return climber_; }
+
+  double robot_velocity() const;
+
  protected:
   virtual void RunIteration(const Goal *unsafe_goal, const Position *position,
                             aos::Sender<Output>::Builder *output,
@@ -31,6 +51,12 @@
 
  private:
   RelativeEncoderSubsystem climber_;
+  PotAndAbsoluteEncoderSubsystem intake_front_;
+  PotAndAbsoluteEncoderSubsystem intake_back_;
+  PotAndAbsoluteEncoderSubsystem turret_;
+
+  aos::Fetcher<frc971::control_loops::drivetrain::Status>
+      drivetrain_status_fetcher_;
 
   DISALLOW_COPY_AND_ASSIGN(Superstructure);
 };
diff --git a/y2022/control_loops/superstructure/superstructure_goal.fbs b/y2022/control_loops/superstructure/superstructure_goal.fbs
index 8107363..37e63b5 100644
--- a/y2022/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2022/control_loops/superstructure/superstructure_goal.fbs
@@ -5,6 +5,22 @@
 table Goal {
   // Height of the climber above rest point
   climber:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 0);
+
+  // Goal angles of intake joints.
+  // Positive is out, 0 is up.
+  intake_front:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 1);
+  intake_back:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 2);
+
+  // Positive is rollers intaking.
+  roller_speed_front:double (id: 3);
+  roller_speed_back:double (id: 4);
+  // One transfer motor for both sides
+  transfer_roller_speed:double (id: 5);
+
+  // Factor to multiply robot velocity by and add to roller voltage.
+  roller_speed_compensation:double (id: 6);
+
+  turret:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 7);
 }
 
 root_type Goal;
diff --git a/y2022/control_loops/superstructure/superstructure_lib_test.cc b/y2022/control_loops/superstructure/superstructure_lib_test.cc
index 82c25b3..493fe95 100644
--- a/y2022/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2022/control_loops/superstructure/superstructure_lib_test.cc
@@ -16,6 +16,237 @@
 namespace control_loops {
 namespace superstructure {
 namespace testing {
+namespace chrono = std::chrono;
+namespace {
+constexpr double kNoiseScalar = 0.01;
+}
+
+using ::aos::monotonic_clock;
+using ::frc971::CreateProfileParameters;
+using ::frc971::control_loops::CappedTestPlant;
+using ::frc971::control_loops::
+    CreateStaticZeroingSingleDOFProfiledSubsystemGoal;
+using ::frc971::control_loops::PositionSensorSimulator;
+using ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
+typedef Superstructure::PotAndAbsoluteEncoderSubsystem
+    PotAndAbsoluteEncoderSubsystem;
+typedef Superstructure::RelativeEncoderSubsystem RelativeEncoderSubsystem;
+using DrivetrainStatus = ::frc971::control_loops::drivetrain::Status;
+
+template <typename SubsystemStatus, typename SubsystemState,
+          typename SubsystemConstants>
+class SubsystemSimulator {
+ public:
+  SubsystemSimulator(CappedTestPlant *plant, PositionSensorSimulator encoder,
+                     const SubsystemConstants subsystem_constants,
+                     const frc971::constants::Range range,
+                     double encoder_offset, const chrono::nanoseconds dt)
+      : plant_(plant),
+        encoder_(encoder),
+        subsystem_constants_(subsystem_constants),
+        range_(range),
+        encoder_offset_(encoder_offset),
+        dt_(dt) {}
+
+  void InitializePosition(double start_pos) {
+    plant_->mutable_X(0, 0) = start_pos;
+    plant_->mutable_X(1, 0) = 0.0;
+
+    encoder_.Initialize(start_pos, kNoiseScalar, 0.0, encoder_offset_);
+  }
+
+  // Simulates the superstructure for a single timestep.
+  void Simulate(double voltage, const SubsystemStatus *status) {
+    double last_velocity = plant_->X(1, 0);
+
+    const double voltage_check =
+        (static_cast<SubsystemState>(status->state()) ==
+         SubsystemState::RUNNING)
+            ? subsystem_constants_.subsystem_params.operating_voltage
+            : subsystem_constants_.subsystem_params.zeroing_voltage;
+
+    EXPECT_NEAR(voltage, 0.0, voltage_check);
+
+    ::Eigen::Matrix<double, 1, 1> U;
+    U << voltage + plant_->voltage_offset();
+    plant_->Update(U);
+
+    const double position = plant_->Y(0, 0);
+
+    encoder_.MoveTo(position);
+
+    EXPECT_GE(position, range_.lower_hard);
+    EXPECT_LE(position, range_.upper_hard);
+
+    const double loop_time = ::aos::time::DurationInSeconds(dt_);
+
+    const double velocity = plant_->X(1, 0);
+    const double acceleration = (velocity - last_velocity) / loop_time;
+
+    EXPECT_GE(peak_acceleration_, acceleration);
+    EXPECT_LE(-peak_acceleration_, acceleration);
+    EXPECT_GE(peak_velocity_, velocity);
+    EXPECT_LE(-peak_velocity_, velocity);
+  }
+
+  void set_peak_acceleration(double value) { peak_acceleration_ = value; }
+  void set_peak_velocity(double value) { peak_velocity_ = value; }
+
+  PositionSensorSimulator *encoder() { return &encoder_; }
+
+ private:
+  std::unique_ptr<CappedTestPlant> plant_;
+  PositionSensorSimulator encoder_;
+  const SubsystemConstants subsystem_constants_;
+  const frc971::constants::Range range_;
+
+  double encoder_offset_ = 0.0;
+
+  double peak_velocity_ = std::numeric_limits<double>::infinity();
+  double peak_acceleration_ = std::numeric_limits<double>::infinity();
+
+  const chrono::nanoseconds dt_;
+};
+
+using PotAndAbsoluteEncoderSimulator = SubsystemSimulator<
+    frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus,
+    PotAndAbsoluteEncoderSubsystem::State,
+    constants::Values::PotAndAbsEncoderConstants>;
+using RelativeEncoderSimulator = SubsystemSimulator<
+    frc971::control_loops::RelativeEncoderProfiledJointStatus,
+    RelativeEncoderSubsystem::State, constants::Values::PotConstants>;
+
+// Class which simulates the superstructure and sends out queue messages with
+// the position.
+class SuperstructureSimulation {
+ public:
+  SuperstructureSimulation(::aos::EventLoop *event_loop,
+                           std::shared_ptr<const constants::Values> values,
+                           chrono::nanoseconds dt)
+      : event_loop_(event_loop),
+        dt_(dt),
+        superstructure_position_sender_(
+            event_loop_->MakeSender<Position>("/superstructure")),
+        superstructure_status_fetcher_(
+            event_loop_->MakeFetcher<Status>("/superstructure")),
+        superstructure_output_fetcher_(
+            event_loop_->MakeFetcher<Output>("/superstructure")),
+        intake_front_(new CappedTestPlant(intake::MakeIntakePlant()),
+                      PositionSensorSimulator(
+                          values->intake_front.subsystem_params.zeroing_constants
+                              .one_revolution_distance),
+                      values->intake_front, constants::Values::kIntakeRange(),
+                      values->intake_front.subsystem_params.zeroing_constants
+                          .measured_absolute_position,
+                      dt_),
+        intake_back_(new CappedTestPlant(intake::MakeIntakePlant()),
+                     PositionSensorSimulator(
+                         values->intake_back.subsystem_params.zeroing_constants
+                             .one_revolution_distance),
+                     values->intake_back, constants::Values::kIntakeRange(),
+                     values->intake_back.subsystem_params.zeroing_constants
+                         .measured_absolute_position,
+                     dt_),
+        turret_(new CappedTestPlant(turret::MakeTurretPlant()),
+                PositionSensorSimulator(
+                    values->turret.subsystem_params.zeroing_constants
+                        .one_revolution_distance),
+                values->turret, constants::Values::kTurretRange(),
+                values->turret.subsystem_params.zeroing_constants
+                    .measured_absolute_position,
+                dt_),
+        climber_(new CappedTestPlant(climber::MakeClimberPlant()),
+                 PositionSensorSimulator(
+                     constants::Values::kClimberPotMetersPerRevolution()),
+                 values->climber, constants::Values::kClimberRange(),
+                 values->climber.potentiometer_offset, dt_) {
+    intake_front_.InitializePosition(
+        constants::Values::kIntakeRange().middle());
+    intake_back_.InitializePosition(constants::Values::kIntakeRange().middle());
+    turret_.InitializePosition(constants::Values::kTurretRange().middle());
+    climber_.InitializePosition(constants::Values::kClimberRange().middle());
+
+    phased_loop_handle_ = event_loop_->AddPhasedLoop(
+        [this](int) {
+          // Skip this the first time.
+          if (!first_) {
+            EXPECT_TRUE(superstructure_output_fetcher_.Fetch());
+            EXPECT_TRUE(superstructure_status_fetcher_.Fetch());
+
+            intake_front_.Simulate(
+                superstructure_output_fetcher_->intake_voltage_front(),
+                superstructure_status_fetcher_->intake_front());
+            intake_back_.Simulate(
+                superstructure_output_fetcher_->intake_voltage_back(),
+                superstructure_status_fetcher_->intake_back());
+            turret_.Simulate(superstructure_output_fetcher_->turret_voltage(),
+                             superstructure_status_fetcher_->turret());
+            climber_.Simulate(superstructure_output_fetcher_->climber_voltage(),
+                              superstructure_status_fetcher_->climber());
+          }
+          first_ = false;
+          SendPositionMessage();
+        },
+        dt);
+  }
+
+  // Sends a queue message with the position of the superstructure.
+  void SendPositionMessage() {
+    ::aos::Sender<Position>::Builder builder =
+        superstructure_position_sender_.MakeBuilder();
+
+    frc971::PotAndAbsolutePosition::Builder turret_builder =
+        builder.MakeBuilder<frc971::PotAndAbsolutePosition>();
+    flatbuffers::Offset<frc971::PotAndAbsolutePosition> turret_offset =
+        turret_.encoder()->GetSensorValues(&turret_builder);
+
+    frc971::PotAndAbsolutePosition::Builder intake_front_builder =
+        builder.MakeBuilder<frc971::PotAndAbsolutePosition>();
+    flatbuffers::Offset<frc971::PotAndAbsolutePosition> intake_front_offset =
+        intake_front_.encoder()->GetSensorValues(&intake_front_builder);
+
+    frc971::PotAndAbsolutePosition::Builder intake_back_builder =
+        builder.MakeBuilder<frc971::PotAndAbsolutePosition>();
+    flatbuffers::Offset<frc971::PotAndAbsolutePosition> intake_back_offset =
+        intake_back_.encoder()->GetSensorValues(&intake_back_builder);
+
+    frc971::RelativePosition::Builder climber_builder =
+        builder.MakeBuilder<frc971::RelativePosition>();
+    flatbuffers::Offset<frc971::RelativePosition> climber_offset =
+        climber_.encoder()->GetSensorValues(&climber_builder);
+
+    Position::Builder position_builder = builder.MakeBuilder<Position>();
+
+    position_builder.add_intake_front(intake_front_offset);
+    position_builder.add_intake_back(intake_back_offset);
+    position_builder.add_turret(turret_offset);
+    position_builder.add_climber(climber_offset);
+
+    CHECK_EQ(builder.Send(position_builder.Finish()),
+             aos::RawSender::Error::kOk);
+  }
+
+  PotAndAbsoluteEncoderSimulator *intake_front() { return &intake_front_; }
+  PotAndAbsoluteEncoderSimulator *intake_back() { return &intake_back_; }
+  PotAndAbsoluteEncoderSimulator *turret() { return &turret_; }
+  RelativeEncoderSimulator *climber() { return &climber_; }
+
+ private:
+  ::aos::EventLoop *event_loop_;
+  const chrono::nanoseconds dt_;
+  ::aos::PhasedLoopHandler *phased_loop_handle_ = nullptr;
+
+  ::aos::Sender<Position> superstructure_position_sender_;
+  ::aos::Fetcher<Status> superstructure_status_fetcher_;
+  ::aos::Fetcher<Output> superstructure_output_fetcher_;
+
+  bool first_ = true;
+
+  PotAndAbsoluteEncoderSimulator intake_front_;
+  PotAndAbsoluteEncoderSimulator intake_back_;
+  PotAndAbsoluteEncoderSimulator turret_;
+  RelativeEncoderSimulator climber_;
+};
 
 class SuperstructureTest : public ::frc971::testing::ControlLoopTest {
  public:
@@ -23,9 +254,12 @@
       : ::frc971::testing::ControlLoopTest(
             aos::configuration::ReadConfig("y2022/config.json"),
             std::chrono::microseconds(5050)),
-        superstructure_event_loop(MakeEventLoop("Superstructure")),
-        superstructure_(superstructure_event_loop.get()),
-        test_event_loop_(MakeEventLoop("test")),
+        values_(std::make_shared<constants::Values>(constants::MakeValues(
+            frc971::control_loops::testing::kTeamNumber))),
+        roborio_(aos::configuration::GetNode(configuration(), "roborio")),
+        superstructure_event_loop(MakeEventLoop("Superstructure", roborio_)),
+        superstructure_(superstructure_event_loop.get(), values_),
+        test_event_loop_(MakeEventLoop("test", roborio_)),
         superstructure_goal_fetcher_(
             test_event_loop_->MakeFetcher<Goal>("/superstructure")),
         superstructure_goal_sender_(
@@ -37,12 +271,15 @@
         superstructure_position_fetcher_(
             test_event_loop_->MakeFetcher<Position>("/superstructure")),
         superstructure_position_sender_(
-            test_event_loop_->MakeSender<Position>("/superstructure")) {
+            test_event_loop_->MakeSender<Position>("/superstructure")),
+        drivetrain_status_sender_(
+            test_event_loop_->MakeSender<DrivetrainStatus>("/drivetrain")),
+        superstructure_plant_event_loop_(MakeEventLoop("plant", roborio_)),
+        superstructure_plant_(superstructure_plant_event_loop_.get(), values_,
+                              dt()) {
     set_team_id(frc971::control_loops::testing::kTeamNumber);
-    SetEnabled(true);
 
-    phased_loop_handle_ = test_event_loop_->AddPhasedLoop(
-        [this](int) { SendPositionMessage(); }, dt());
+    SetEnabled(true);
 
     if (!FLAGS_output_folder.empty()) {
       unlink(FLAGS_output_folder.c_str());
@@ -52,14 +289,119 @@
     }
   }
 
-  void SendPositionMessage() {
-    auto builder = superstructure_position_sender_.MakeBuilder();
-    Position::Builder position_builder = builder.MakeBuilder<Position>();
-    builder.CheckOk(builder.Send(position_builder.Finish()));
+  void VerifyNearGoal() {
+    superstructure_goal_fetcher_.Fetch();
+    superstructure_status_fetcher_.Fetch();
+
+    // Only check the goal if there is one.
+
+    if (superstructure_goal_fetcher_->has_intake_front()) {
+      EXPECT_NEAR(superstructure_goal_fetcher_->intake_front()->unsafe_goal(),
+                  superstructure_status_fetcher_->intake_front()->position(),
+                  0.001);
+    }
+
+    if (superstructure_goal_fetcher_->has_intake_back()) {
+      EXPECT_NEAR(superstructure_goal_fetcher_->intake_back()->unsafe_goal(),
+                  superstructure_status_fetcher_->intake_back()->position(),
+                  0.001);
+    }
+
+    if (superstructure_goal_fetcher_->has_turret()) {
+      EXPECT_NEAR(superstructure_goal_fetcher_->turret()->unsafe_goal(),
+                  superstructure_status_fetcher_->turret()->position(), 0.001);
+    }
+
+    if (superstructure_goal_fetcher_->has_climber()) {
+      EXPECT_NEAR(superstructure_goal_fetcher_->climber()->unsafe_goal(),
+                  superstructure_status_fetcher_->climber()->position(), 0.001);
+    }
   }
 
-  // Because the third robot is single node, the roborio node is nullptr
-  const aos::Node *const roborio_ = nullptr;
+  void CheckIfZeroed() {
+    superstructure_status_fetcher_.Fetch();
+    ASSERT_TRUE(superstructure_status_fetcher_.get()->zeroed());
+  }
+
+  void WaitUntilZeroed() {
+    int i = 0;
+    do {
+      i++;
+      RunFor(dt());
+      superstructure_status_fetcher_.Fetch();
+      // 2 Seconds
+      ASSERT_LE(i, 2.0 / ::aos::time::DurationInSeconds(dt()));
+
+      // Since there is a delay when sending running, make sure we have a
+      // status before checking it.
+    } while (superstructure_status_fetcher_.get() == nullptr ||
+             !superstructure_status_fetcher_.get()->zeroed());
+  }
+
+  void SendRobotVelocity(double robot_velocity) {
+    // Send a robot velocity to test compensation
+    auto builder = drivetrain_status_sender_.MakeBuilder();
+    auto drivetrain_status_builder = builder.MakeBuilder<DrivetrainStatus>();
+    drivetrain_status_builder.add_robot_speed(robot_velocity);
+    builder.CheckOk(builder.Send(drivetrain_status_builder.Finish()));
+  }
+
+  void TestRollerFront(double roller_speed_front,
+                       double roller_speed_compensation) {
+    auto builder = superstructure_goal_sender_.MakeBuilder();
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_roller_speed_front(roller_speed_front);
+    goal_builder.add_roller_speed_compensation(roller_speed_compensation);
+    builder.CheckOk(builder.Send(goal_builder.Finish()));
+    RunFor(dt() * 2);
+    ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
+    EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_front(),
+              roller_speed_front + std::max((superstructure_.robot_velocity() *
+                                             roller_speed_compensation),
+                                            0.0));
+    if (superstructure_.robot_velocity() <= 0) {
+      EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_front(),
+                roller_speed_front);
+    }
+  }
+
+  void TestRollerBack(double roller_speed_back,
+                      double roller_speed_compensation) {
+    auto builder = superstructure_goal_sender_.MakeBuilder();
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_roller_speed_back(roller_speed_back);
+    goal_builder.add_roller_speed_compensation(roller_speed_compensation);
+    builder.CheckOk(builder.Send(goal_builder.Finish()));
+    RunFor(dt() * 2);
+    ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
+    ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+    EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_back(),
+              roller_speed_back - std::min(superstructure_.robot_velocity() *
+                                               roller_speed_compensation,
+                                           0.0));
+    if (superstructure_.robot_velocity() >= 0) {
+      EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_back(),
+                roller_speed_back);
+    }
+  }
+
+  void TestTransferRoller(double transfer_roller_speed,
+                          double roller_speed_compensation) {
+    auto builder = superstructure_goal_sender_.MakeBuilder();
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_transfer_roller_speed(transfer_roller_speed);
+    goal_builder.add_roller_speed_compensation(roller_speed_compensation);
+    builder.CheckOk(builder.Send(goal_builder.Finish()));
+    RunFor(dt() * 2);
+    ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
+    ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+    EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(),
+              transfer_roller_speed);
+  }
+
+  std::shared_ptr<const constants::Values> values_;
+
+  const aos::Node *const roborio_;
 
   ::std::unique_ptr<::aos::EventLoop> superstructure_event_loop;
   ::y2022::control_loops::superstructure::Superstructure superstructure_;
@@ -72,11 +414,306 @@
   ::aos::Fetcher<Output> superstructure_output_fetcher_;
   ::aos::Fetcher<Position> superstructure_position_fetcher_;
   ::aos::Sender<Position> superstructure_position_sender_;
+  ::aos::Sender<DrivetrainStatus> drivetrain_status_sender_;
+
+  ::std::unique_ptr<::aos::EventLoop> superstructure_plant_event_loop_;
+  SuperstructureSimulation superstructure_plant_;
 
   std::unique_ptr<aos::EventLoop> logger_event_loop_;
   std::unique_ptr<aos::logger::Logger> logger_;
 };
 
+// Tests that the superstructure does nothing when the goal is to remain
+// still.
+TEST_F(SuperstructureTest, DoesNothing) {
+  SetEnabled(true);
+  superstructure_plant_.intake_front()->InitializePosition(
+      constants::Values::kIntakeRange().middle());
+  superstructure_plant_.intake_back()->InitializePosition(
+      constants::Values::kIntakeRange().middle());
+  superstructure_plant_.turret()->InitializePosition(
+      constants::Values::kTurretRange().middle());
+  superstructure_plant_.climber()->InitializePosition(
+      constants::Values::kClimberRange().middle());
+
+  WaitUntilZeroed();
+
+  {
+    auto builder = superstructure_goal_sender_.MakeBuilder();
+
+    flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+        intake_offset_front = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+            *builder.fbb(), constants::Values::kIntakeRange().middle());
+
+    flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+        intake_offset_back = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+            *builder.fbb(), constants::Values::kIntakeRange().middle());
+
+    flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+        turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+            *builder.fbb(), constants::Values::kTurretRange().middle());
+
+    flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+        climber_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+            *builder.fbb(), constants::Values::kClimberRange().middle());
+
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+    goal_builder.add_intake_front(intake_offset_front);
+    goal_builder.add_intake_back(intake_offset_back);
+
+    goal_builder.add_turret(turret_offset);
+
+    goal_builder.add_climber(climber_offset);
+
+    ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+  }
+  RunFor(chrono::seconds(10));
+  VerifyNearGoal();
+
+  EXPECT_TRUE(superstructure_output_fetcher_.Fetch());
+}
+
+// Tests that loops can reach a goal.
+TEST_F(SuperstructureTest, ReachesGoal) {
+  SetEnabled(true);
+  // Set a reasonable goal.
+  superstructure_plant_.intake_front()->InitializePosition(
+      constants::Values::kIntakeRange().middle());
+  superstructure_plant_.intake_back()->InitializePosition(
+      constants::Values::kIntakeRange().middle());
+  superstructure_plant_.turret()->InitializePosition(
+      constants::Values::kTurretRange().middle());
+  superstructure_plant_.climber()->InitializePosition(
+      constants::Values::kClimberRange().middle());
+  WaitUntilZeroed();
+  {
+    auto builder = superstructure_goal_sender_.MakeBuilder();
+    flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+        intake_offset_front = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+            *builder.fbb(), constants::Values::kIntakeRange().upper,
+            CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
+
+    flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+        intake_offset_back = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+            *builder.fbb(), constants::Values::kIntakeRange().upper,
+            CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
+
+    flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+        turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+            *builder.fbb(), constants::Values::kTurretRange().upper,
+            CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
+
+    flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+        climber_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+            *builder.fbb(), constants::Values::kClimberRange().upper,
+            CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
+
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+    goal_builder.add_intake_front(intake_offset_front);
+    goal_builder.add_intake_back(intake_offset_back);
+
+    goal_builder.add_turret(turret_offset);
+
+    goal_builder.add_climber(climber_offset);
+
+    ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+  }
+
+  // Give it a lot of time to get there.
+  RunFor(chrono::seconds(15));
+
+  VerifyNearGoal();
+}
+
+// Makes sure that the voltage on a motor is properly pulled back after
+// saturation such that we don't get weird or bad (e.g. oscillating)
+// behaviour.
+TEST_F(SuperstructureTest, SaturationTest) {
+  SetEnabled(true);
+  // Zero it before we move.
+  WaitUntilZeroed();
+  {
+    auto builder = superstructure_goal_sender_.MakeBuilder();
+
+    flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+        intake_offset_front = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+            *builder.fbb(), constants::Values::kIntakeRange().upper);
+
+    flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+        intake_offset_back = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+            *builder.fbb(), constants::Values::kIntakeRange().upper);
+
+    flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+        turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+            *builder.fbb(), constants::Values::kTurretRange().upper);
+
+    flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+        climber_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+            *builder.fbb(), constants::Values::kClimberRange().upper);
+
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+    goal_builder.add_intake_front(intake_offset_front);
+    goal_builder.add_intake_back(intake_offset_back);
+
+    goal_builder.add_turret(turret_offset);
+
+    goal_builder.add_climber(climber_offset);
+
+    ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+  }
+  RunFor(chrono::seconds(10));
+  VerifyNearGoal();
+
+  // Try a low acceleration move with a high max velocity and verify the
+  // acceleration is capped like expected.
+  {
+    auto builder = superstructure_goal_sender_.MakeBuilder();
+
+    flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+        intake_offset_front = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+            *builder.fbb(), constants::Values::kIntakeRange().lower,
+            CreateProfileParameters(*builder.fbb(), 20.0, 0.1));
+
+    flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+        intake_offset_back = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+            *builder.fbb(), constants::Values::kIntakeRange().lower,
+            CreateProfileParameters(*builder.fbb(), 20.0, 0.1));
+
+    flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+        turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+            *builder.fbb(), constants::Values::kTurretRange().lower,
+            CreateProfileParameters(*builder.fbb(), 20.0, 0.1));
+
+    flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+        climber_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+            *builder.fbb(), constants::Values::kClimberRange().lower,
+            CreateProfileParameters(*builder.fbb(), 20.0, 0.1));
+
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+    goal_builder.add_intake_front(intake_offset_front);
+    goal_builder.add_intake_back(intake_offset_back);
+
+    goal_builder.add_turret(turret_offset);
+
+    goal_builder.add_climber(climber_offset);
+
+    ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+  }
+
+  superstructure_plant_.intake_front()->set_peak_velocity(23.0);
+  superstructure_plant_.intake_front()->set_peak_acceleration(0.2);
+  superstructure_plant_.intake_back()->set_peak_velocity(23.0);
+  superstructure_plant_.intake_back()->set_peak_acceleration(0.2);
+
+  superstructure_plant_.turret()->set_peak_velocity(23.0);
+  superstructure_plant_.turret()->set_peak_acceleration(6.0);
+
+  superstructure_plant_.climber()->set_peak_velocity(23.0);
+  superstructure_plant_.climber()->set_peak_acceleration(0.2);
+
+  // Intake needs over 9 seconds to reach the goal
+  // TODO(Milo): Make this a sane time
+  RunFor(chrono::seconds(20));
+  VerifyNearGoal();
+}
+
+// Tests that the loop zeroes when run for a while without a goal.
+TEST_F(SuperstructureTest, ZeroNoGoal) {
+  SetEnabled(true);
+  WaitUntilZeroed();
+  RunFor(chrono::seconds(2));
+
+  EXPECT_EQ(PotAndAbsoluteEncoderSubsystem::State::RUNNING,
+            superstructure_.intake_front().state());
+  EXPECT_EQ(PotAndAbsoluteEncoderSubsystem::State::RUNNING,
+            superstructure_.intake_back().state());
+  EXPECT_EQ(PotAndAbsoluteEncoderSubsystem::State::RUNNING,
+            superstructure_.turret().state());
+  EXPECT_EQ(RelativeEncoderSubsystem::State::RUNNING,
+            superstructure_.climber().state());
+}
+
+// Tests that running disabled works
+TEST_F(SuperstructureTest, DisableTest) {
+  RunFor(chrono::seconds(2));
+  CheckIfZeroed();
+}
+
+// Makes sure the roller speeds are getting compensated correctly
+TEST_F(SuperstructureTest, RunRollers) {
+  SetEnabled(true);
+  WaitUntilZeroed();
+
+  SendRobotVelocity(3.0);
+  TestRollerFront(-12.0, 1.5);
+  TestRollerFront(12.0, 1.5);
+  TestRollerFront(0.0, 1.5);
+
+  SendRobotVelocity(-3.0);
+  TestRollerFront(-12.0, 1.5);
+  TestRollerFront(12.0, 1.5);
+  TestRollerFront(0.0, 1.5);
+
+  SendRobotVelocity(3.0);
+  TestRollerBack(-12.0, 1.5);
+  TestRollerBack(12.0, 1.5);
+  TestRollerBack(0.0, 1.5);
+
+  SendRobotVelocity(-3.0);
+  TestRollerBack(-12.0, 1.5);
+  TestRollerBack(12.0, 1.5);
+  TestRollerBack(0.0, 1.5);
+
+  TestTransferRoller(-12.0, 1.5);
+  TestTransferRoller(12.0, 1.5);
+  TestTransferRoller(0.0, 1.5);
+}
+
+// Make sure that the front and back intakes are never switched
+TEST_F(SuperstructureTest, RunIntakes) {
+  SetEnabled(true);
+  superstructure_plant_.intake_front()->InitializePosition(
+      constants::Values::kIntakeRange().middle());
+  superstructure_plant_.intake_back()->InitializePosition(
+      constants::Values::kIntakeRange().middle());
+
+  WaitUntilZeroed();
+
+  superstructure_plant_.intake_front()->set_peak_velocity(23.0);
+  superstructure_plant_.intake_front()->set_peak_acceleration(0.3);
+  superstructure_plant_.intake_back()->set_peak_velocity(23.0);
+  superstructure_plant_.intake_back()->set_peak_acceleration(0.3);
+
+  auto builder = superstructure_goal_sender_.MakeBuilder();
+  flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+      intake_offset_front = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+          *builder.fbb(), constants::Values::kIntakeRange().lower,
+          CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
+
+  flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+      intake_offset_back = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+          *builder.fbb(), constants::Values::kIntakeRange().upper,
+          CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
+
+  Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+  goal_builder.add_intake_front(intake_offset_front);
+  goal_builder.add_intake_back(intake_offset_back);
+
+  ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+  // TODO(Milo): Make this a sane time
+  RunFor(chrono::seconds(10));
+  ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+  EXPECT_FLOAT_EQ(superstructure_status_fetcher_->intake_front()->position(),
+                  constants::Values::kIntakeRange().lower);
+  EXPECT_FLOAT_EQ(superstructure_status_fetcher_->intake_back()->position(),
+                  constants::Values::kIntakeRange().upper);
+}
+
 }  // namespace testing
 }  // namespace superstructure
 }  // namespace control_loops
diff --git a/y2022/control_loops/superstructure/superstructure_main.cc b/y2022/control_loops/superstructure/superstructure_main.cc
index f53e042..c6c12b7 100644
--- a/y2022/control_loops/superstructure/superstructure_main.cc
+++ b/y2022/control_loops/superstructure/superstructure_main.cc
@@ -1,7 +1,6 @@
-#include "y2022/control_loops/superstructure/superstructure.h"
-
 #include "aos/events/shm_event_loop.h"
 #include "aos/init.h"
+#include "y2022/control_loops/superstructure/superstructure.h"
 
 int main(int argc, char **argv) {
   ::aos::InitGoogle(&argc, &argv);
@@ -10,8 +9,11 @@
       aos::configuration::ReadConfig("config.json");
 
   ::aos::ShmEventLoop event_loop(&config.message());
+  std::shared_ptr<const y2022::constants::Values> values =
+      std::make_shared<const y2022::constants::Values>(
+          y2022::constants::MakeValues());
   ::y2022::control_loops::superstructure::Superstructure superstructure(
-      &event_loop);
+      &event_loop, values);
 
   event_loop.Run();
 
diff --git a/y2022/control_loops/superstructure/superstructure_output.fbs b/y2022/control_loops/superstructure/superstructure_output.fbs
index 7fa390f..aecc090 100644
--- a/y2022/control_loops/superstructure/superstructure_output.fbs
+++ b/y2022/control_loops/superstructure/superstructure_output.fbs
@@ -1,18 +1,28 @@
 namespace y2022.control_loops.superstructure;
 
 table Output {
-    // Voltage of the climber falcon
-    // - is down + is up
-    climber_voltage:double (id: 0);
+  // Voltage of the climber falcon
+  // - is down + is up
+  climber_voltage:double (id: 0);
 
-    // Voltage of the catapult falcon
-    // Positive lifts the catapult to fire.
-    catapult_voltage:double (id: 1);
+  // Voltage of the catapult falcon
+  // Positive lifts the catapult to fire.
+  catapult_voltage:double (id: 1);
 
-    // Voltage of the turret falcon
-    // Positive rotates the turret around the Z axis (up) according to the
-    // right hand rule.
-    turret_voltage:double (id: 2);
+  // Voltage of the turret falcon
+  // Positive rotates the turret around the Z axis (up) according to the
+  // right hand rule.
+  turret_voltage:double (id: 2);
+
+  // Intake joint voltages.
+  intake_voltage_front:double (id: 3);
+  intake_voltage_back:double (id: 4);
+
+  // Intake roller voltages
+  roller_voltage_front:double (id: 5);
+  roller_voltage_back:double (id: 6);
+  // One transfer motor for both sides
+  transfer_roller_voltage:double (id: 7);
 }
 
 root_type Output;
diff --git a/y2022/control_loops/superstructure/superstructure_plotter.ts b/y2022/control_loops/superstructure/superstructure_plotter.ts
index 1f1a4c5..6581758 100644
--- a/y2022/control_loops/superstructure/superstructure_plotter.ts
+++ b/y2022/control_loops/superstructure/superstructure_plotter.ts
@@ -1,7 +1,7 @@
 // Provides a plot for debugging robot state-related issues.
 import {AosPlotter} from 'org_frc971/aos/network/www/aos_plotter';
-import * as proxy from 'org_frc971/aos/network/www/proxy';
 import {BLUE, BROWN, CYAN, GREEN, PINK, RED, WHITE} from 'org_frc971/aos/network/www/colors';
+import * as proxy from 'org_frc971/aos/network/www/proxy';
 
 import Connection = proxy.Connection;
 
@@ -9,11 +9,15 @@
 const DEFAULT_WIDTH = AosPlotter.DEFAULT_WIDTH;
 const DEFAULT_HEIGHT = AosPlotter.DEFAULT_HEIGHT * 3;
 
-export function plotSuperstructure(conn: Connection, element: Element) : void {
+export function plotSuperstructure(conn: Connection, element: Element): void {
   const aosPlotter = new AosPlotter(conn);
-  const goal = aosPlotter.addMessageSource('/superstructure', 'y2022.control_loops.superstructure.Goal');
-  const output = aosPlotter.addMessageSource('/superstructure', 'y2022.control_loops.superstructure.Output');
-  const status = aosPlotter.addMessageSource('/superstructure', 'y2022.control_loops.superstructure.Status');
-  const position = aosPlotter.addMessageSource('/superstructure', 'y2022.control_loops.superstructure.Position');
+  const goal = aosPlotter.addMessageSource(
+      '/superstructure', 'y2022.control_loops.superstructure.Goal');
+  const output = aosPlotter.addMessageSource(
+      '/superstructure', 'y2022.control_loops.superstructure.Output');
+  const status = aosPlotter.addMessageSource(
+      '/superstructure', 'y2022.control_loops.superstructure.Status');
+  const position = aosPlotter.addMessageSource(
+      '/superstructure', 'y2022.control_loops.superstructure.Position');
   const robotState = aosPlotter.addMessageSource('/aos', 'aos.RobotState');
 }
diff --git a/y2022/control_loops/superstructure/superstructure_position.fbs b/y2022/control_loops/superstructure/superstructure_position.fbs
index a36f4f6..38babaf 100644
--- a/y2022/control_loops/superstructure/superstructure_position.fbs
+++ b/y2022/control_loops/superstructure/superstructure_position.fbs
@@ -4,6 +4,11 @@
 
 table Position {
   climber:frc971.RelativePosition (id: 0);
+  // Zero for the intake position value is up, and positive is
+  // down.
+  intake_front:frc971.PotAndAbsolutePosition (id: 1);
+  intake_back:frc971.PotAndAbsolutePosition (id: 2);
+  turret:frc971.PotAndAbsolutePosition (id: 3);
 }
 
 root_type Position;
diff --git a/y2022/control_loops/superstructure/superstructure_status.fbs b/y2022/control_loops/superstructure/superstructure_status.fbs
index 0ee0994..b4bad2a 100644
--- a/y2022/control_loops/superstructure/superstructure_status.fbs
+++ b/y2022/control_loops/superstructure/superstructure_status.fbs
@@ -12,6 +12,12 @@
 
   // Subsystem statuses
   climber:frc971.control_loops.RelativeEncoderProfiledJointStatus (id: 2);
+
+  // Estimated angles and angular velocities of the intakes.
+  intake_front:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 3);
+  intake_back:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 4);
+
+  turret:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 5);
 }
 
 root_type Status;
\ No newline at end of file
diff --git a/y2022/control_loops/superstructure/turret/BUILD b/y2022/control_loops/superstructure/turret/BUILD
new file mode 100644
index 0000000..c2948d7
--- /dev/null
+++ b/y2022/control_loops/superstructure/turret/BUILD
@@ -0,0 +1,34 @@
+package(default_visibility = ["//y2022:__subpackages__"])
+
+genrule(
+    name = "genrule_turret",
+    outs = [
+        "turret_plant.h",
+        "turret_plant.cc",
+        "integral_turret_plant.h",
+        "integral_turret_plant.cc",
+    ],
+    cmd = "$(location //y2022/control_loops/python:turret) $(OUTS)",
+    target_compatible_with = ["@platforms//os:linux"],
+    tools = [
+        "//y2022/control_loops/python:turret",
+    ],
+)
+
+cc_library(
+    name = "turret_plants",
+    srcs = [
+        "integral_turret_plant.cc",
+        "turret_plant.cc",
+    ],
+    hdrs = [
+        "integral_turret_plant.h",
+        "turret_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",
+    ],
+)