diff --git a/y2022/control_loops/superstructure/BUILD b/y2022/control_loops/superstructure/BUILD
index ac91668..2efb322 100644
--- a/y2022/control_loops/superstructure/BUILD
+++ b/y2022/control_loops/superstructure/BUILD
@@ -77,6 +77,7 @@
     ],
     deps = [
         ":collision_avoidance_lib",
+        ":superstructure_can_position_fbs",
         ":superstructure_goal_fbs",
         ":superstructure_output_fbs",
         ":superstructure_position_fbs",
diff --git a/y2022/control_loops/superstructure/catapult/catapult.cc b/y2022/control_loops/superstructure/catapult/catapult.cc
index e4d76d3..adc13c7 100644
--- a/y2022/control_loops/superstructure/catapult/catapult.cc
+++ b/y2022/control_loops/superstructure/catapult/catapult.cc
@@ -23,7 +23,7 @@
 
   instance.constraint_matrix =
       Eigen::SparseMatrix<double, Eigen::ColMajor, osqp::c_int>(horizon,
-                                                                 horizon);
+                                                                horizon);
   instance.constraint_matrix.setIdentity();
 
   instance.lower_bounds =
@@ -313,7 +313,7 @@
 const flatbuffers::Offset<
     frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>
 Catapult::Iterate(const Goal *unsafe_goal, const Position *position,
-                  double *catapult_voltage,
+                  double *catapult_voltage, bool fire,
                   flatbuffers::FlatBufferBuilder *fbb) {
   const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
       *catapult_goal = unsafe_goal != nullptr && unsafe_goal->has_catapult()
@@ -326,13 +326,12 @@
   if (catapult_disabled) {
     catapult_state_ = CatapultState::PROFILE;
   } else if (catapult_.running() && unsafe_goal &&
-             unsafe_goal->has_catapult() && unsafe_goal->catapult()->fire() &&
-             !last_firing_) {
+             unsafe_goal->has_catapult() && fire && !last_firing_) {
     catapult_state_ = CatapultState::FIRING;
   }
 
   if (catapult_.running() && unsafe_goal && unsafe_goal->has_catapult()) {
-    last_firing_ = unsafe_goal->catapult()->fire();
+    last_firing_ = fire;
   }
 
   use_profile_ = true;
@@ -379,8 +378,7 @@
           use_profile_ = false;
         }
       } else {
-        if (unsafe_goal && unsafe_goal->has_catapult() &&
-            !unsafe_goal->catapult()->fire()) {
+        if (unsafe_goal && unsafe_goal->has_catapult() && !fire) {
           // Eh, didn't manage to solve before it was time to fire.  Give up.
           catapult_state_ = CatapultState::PROFILE;
         }
@@ -417,7 +415,8 @@
     }
   }
 
-  catapult_.UpdateObserver(catapult_voltage != nullptr ? *catapult_voltage : 0.0);
+  catapult_.UpdateObserver(catapult_voltage != nullptr ? *catapult_voltage
+                                                       : 0.0);
 
   return catapult_.MakeStatus(fbb);
 }
diff --git a/y2022/control_loops/superstructure/catapult/catapult.h b/y2022/control_loops/superstructure/catapult/catapult.h
index a8141a0..ccd4b06 100644
--- a/y2022/control_loops/superstructure/catapult/catapult.h
+++ b/y2022/control_loops/superstructure/catapult/catapult.h
@@ -192,6 +192,8 @@
   // Resets all state for when WPILib restarts.
   void Reset() { catapult_.Reset(); }
 
+  void Estop() { catapult_.Estop(); }
+
   bool zeroed() const { return catapult_.zeroed(); }
   bool estopped() const { return catapult_.estopped(); }
   double solve_time() const { return catapult_mpc_.solve_time(); }
@@ -201,12 +203,16 @@
   // Returns the number of shots taken.
   int shot_count() const { return shot_count_; }
 
+  // Returns the estimated position
+  double estimated_position() const { return catapult_.estimated_position(); }
+
   // Runs either the MPC or the profiled subsystem depending on if we are
   // shooting or not.  Returns the status.
   const flatbuffers::Offset<
       frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>
   Iterate(const Goal *unsafe_goal, const Position *position,
-          double *catapult_voltage, flatbuffers::FlatBufferBuilder *fbb);
+          double *catapult_voltage, bool fire,
+          flatbuffers::FlatBufferBuilder *fbb);
 
  private:
   // TODO(austin): Prototype is just an encoder.  Catapult has both an encoder
diff --git a/y2022/control_loops/superstructure/collision_avoidance.cc b/y2022/control_loops/superstructure/collision_avoidance.cc
index 5d0fe27..b39b48f 100644
--- a/y2022/control_loops/superstructure/collision_avoidance.cc
+++ b/y2022/control_loops/superstructure/collision_avoidance.cc
@@ -67,8 +67,10 @@
   return false;
 }
 
-void CollisionAvoidance::UpdateGoal(const CollisionAvoidance::Status &status,
-                                    const Goal *unsafe_goal) {
+void CollisionAvoidance::UpdateGoal(
+    const CollisionAvoidance::Status &status,
+    const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
+        *unsafe_turret_goal) {
   // Start with our constraints being wide open.
   clear_max_turret_goal();
   clear_min_turret_goal();
@@ -81,10 +83,9 @@
   const double intake_back_position = status.intake_back_position;
   const double turret_position = status.turret_position;
 
-  const double turret_goal =
-      (unsafe_goal != nullptr && unsafe_goal->turret() != nullptr
-           ? unsafe_goal->turret()->unsafe_goal()
-           : std::numeric_limits<double>::quiet_NaN());
+  const double turret_goal = (unsafe_turret_goal != nullptr
+                                  ? unsafe_turret_goal->unsafe_goal()
+                                  : std::numeric_limits<double>::quiet_NaN());
 
   // Calculating the avoidance with either intake, and when the turret is
   // wrapped.
diff --git a/y2022/control_loops/superstructure/collision_avoidance.h b/y2022/control_loops/superstructure/collision_avoidance.h
index 94b454c..7c25964 100644
--- a/y2022/control_loops/superstructure/collision_avoidance.h
+++ b/y2022/control_loops/superstructure/collision_avoidance.h
@@ -69,7 +69,10 @@
                       double min_turret_collision_position,
                       double max_turret_collision_position);
   // Checks and alters goals to make sure they're safe.
-  void UpdateGoal(const Status &status, const Goal *unsafe_goal);
+  void UpdateGoal(
+      const Status &status,
+      const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
+          *unsafe_turret_goal);
   // Limits if goal is in collision spots.
   void CalculateAvoidance(bool intake_front, double intake_position,
                           double turret_goal, double mix_turret_collision_goal,
diff --git a/y2022/control_loops/superstructure/collision_avoidance_test.cc b/y2022/control_loops/superstructure/collision_avoidance_test.cc
index 616a10b..43695d1 100644
--- a/y2022/control_loops/superstructure/collision_avoidance_test.cc
+++ b/y2022/control_loops/superstructure/collision_avoidance_test.cc
@@ -2,8 +2,8 @@
 
 #include "aos/commonmath.h"
 #include "aos/flatbuffers.h"
-#include "gtest/gtest.h"
 #include "gmock/gmock.h"
+#include "gtest/gtest.h"
 #include "y2022/control_loops/superstructure/superstructure_goal_generated.h"
 #include "y2022/control_loops/superstructure/superstructure_status_generated.h"
 
@@ -84,22 +84,18 @@
         status_({0.0, 0.0, 0.0}),
         prev_status_({0.0, 0.0, 0.0}) {}
 
-  int t = 0;
-  int d = 0;
   void Simulate() {
     FlatbufferDetachedBuffer<Goal> safe_goal = MakeZeroGoal();
 
-    t++;
     // Don't simulate if already collided
     if (avoidance_.IsCollided(status_)) {
-      d++;
       return;
     }
 
     bool moving = true;
     while (moving) {
       // Compute the safe goal
-      avoidance_.UpdateGoal(status_, &unsafe_goal_.message());
+      avoidance_.UpdateGoal(status_, unsafe_goal_.message().turret());
 
       // The system should never be collided
       ASSERT_FALSE(avoidance_.IsCollided(status_));
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index bf94b08..aa5eee8 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -21,10 +21,12 @@
       intake_front_(values_->intake_front.subsystem_params),
       intake_back_(values_->intake_back.subsystem_params),
       turret_(values_->turret.subsystem_params),
+      catapult_(*values_),
       drivetrain_status_fetcher_(
           event_loop->MakeFetcher<frc971::control_loops::drivetrain::Status>(
               "/drivetrain")),
-      catapult_(*values_) {
+      can_position_fetcher_(
+          event_loop->MakeFetcher<CANPosition>("/superstructure")) {
   event_loop->SetRuntimeRealtimePriority(30);
 }
 
@@ -32,8 +34,6 @@
                                   const Position *position,
                                   aos::Sender<Output>::Builder *output,
                                   aos::Sender<Status>::Builder *status) {
-  OutputT output_struct;
-
   if (WasReset()) {
     AOS_LOG(ERROR, "WPILib reset, restarting\n");
     intake_front_.Reset();
@@ -43,27 +43,28 @@
     catapult_.Reset();
   }
 
-  collision_avoidance_.UpdateGoal(
-      {.intake_front_position = intake_front_.estimated_position(),
-       .intake_back_position = intake_back_.estimated_position(),
-       .turret_position = turret_.estimated_position()},
-      unsafe_goal);
+  OutputT output_struct;
 
-  turret_.set_min_position(collision_avoidance_.min_turret_goal());
-  turret_.set_max_position(collision_avoidance_.max_turret_goal());
-  intake_front_.set_min_position(collision_avoidance_.min_intake_front_goal());
-  intake_front_.set_max_position(collision_avoidance_.max_intake_front_goal());
-  intake_back_.set_min_position(collision_avoidance_.min_intake_back_goal());
-  intake_back_.set_max_position(collision_avoidance_.max_intake_back_goal());
+  aos::FlatbufferFixedAllocatorArray<
+      frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal, 64>
+      turret_goal_buffer;
+
+  const aos::monotonic_clock::time_point timestamp =
+      event_loop()->context().monotonic_event_time;
 
   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;
+  const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
+      *turret_goal = nullptr;
+  double roller_speed_compensated_front = 0.0;
+  double roller_speed_compensated_back = 0.0;
+  double transfer_roller_speed = 0.0;
+  double flipper_arms_voltage = 0.0;
 
   if (unsafe_goal != nullptr) {
+    turret_goal = unsafe_goal->turret();
+
     roller_speed_compensated_front =
         unsafe_goal->roller_speed_front() +
         std::max(velocity * unsafe_goal->roller_speed_compensation(), 0.0);
@@ -75,45 +76,303 @@
     transfer_roller_speed = unsafe_goal->transfer_roller_speed();
   }
 
+  // TODO: Aimer sets turret_goal here
 
+  // Supersturcture state machine:
+  // 1. IDLE: Wait until an intake beambreak is triggerred, meaning that a ball
+  // is being intaked. This means that the transfer rollers have a ball. If
+  // we've been waiting here for too long without any beambreak triggered, the
+  // ball got lost, so reset.
+  // 2. TRANSFERRING: Until the turret reaches the loading position where the
+  // ball can be transferred into the catapult, wiggle the ball in place.
+  // Once the turret reaches the loading position, send the ball forward with
+  // the transfer rollers until the turret beambreak is triggered.
+  // If we have been in this state for too long, the ball probably got lost so
+  // reset back to IDLE.
+  // 3. LOADING: To load the ball into the catapult, put the flippers at the
+  // feeding speed. Wait for a timeout, and then wait until the ball has gone
+  // past the turret beambreak and the flippers have stopped moving, meaning
+  // that the ball is fully loaded in the catapult.
+  // 4. LOADED: Wait until the user asks us to fire to transition to the
+  // shooting stage. If asked to cancel the shot, reset back to the IDLE state.
+  // 5. SHOOTING: Open the flippers to get ready for the shot. If they don't
+  // open quickly enough, try reseating the ball and going back to the LOADING
+  // stage, which moves the flippers in the opposite direction first.
+  // Now, hold the flippers open and wait until the turret has reached its
+  // aiming goal. Once the turret is ready, tell the catapult to fire.
+  // If the flippers move back for some reason now, it could damage the
+  // catapult, so estop it. Otherwise, wait until the catapult shoots a ball and
+  // goes back to its return position. We have now finished the shot, so return
+  // to IDLE.
+
+  const bool is_spitting = ((intake_state_ == IntakeState::INTAKE_FRONT_BALL &&
+                             transfer_roller_speed < 0) ||
+                            (intake_state_ == IntakeState::INTAKE_BACK_BALL &&
+                             transfer_roller_speed > 0));
+
+  // Intake handling should happen regardless of the turret state
+  if (position->intake_beambreak_front() || position->intake_beambreak_back()) {
+    if (intake_state_ == IntakeState::NO_BALL) {
+      if (position->intake_beambreak_front()) {
+        intake_state_ = IntakeState::INTAKE_FRONT_BALL;
+      } else if (position->intake_beambreak_back()) {
+        intake_state_ = IntakeState::INTAKE_BACK_BALL;
+      }
+    }
+
+    intake_beambreak_timer_ = timestamp;
+  }
+
+  if (intake_state_ != IntakeState::NO_BALL) {
+    // Block intaking in
+    roller_speed_compensated_front = 0.0;
+    roller_speed_compensated_back = 0.0;
+
+    const double wiggle_voltage =
+        (intake_state_ == IntakeState::INTAKE_FRONT_BALL
+             ? constants::Values::kTransferRollerFrontWiggleVoltage()
+             : constants::Values::kTransferRollerBackWiggleVoltage());
+    // Wiggle transfer rollers: send the ball back and forth while waiting
+    // for the turret or waiting for another shot to be completed
+    if ((intake_state_ == IntakeState::INTAKE_FRONT_BALL &&
+         position->intake_beambreak_front()) ||
+        (intake_state_ == IntakeState::INTAKE_BACK_BALL &&
+         position->intake_beambreak_back())) {
+      transfer_roller_speed = -wiggle_voltage;
+    } else {
+      transfer_roller_speed = wiggle_voltage;
+    }
+  }
+
+  switch (state_) {
+    case SuperstructureState::IDLE: {
+      if (timestamp >
+          intake_beambreak_timer_ + constants::Values::kBallLostTime()) {
+        intake_state_ = IntakeState::NO_BALL;
+      }
+
+      if (is_spitting) {
+        intake_state_ = IntakeState::NO_BALL;
+      }
+
+      if (intake_state_ == IntakeState::NO_BALL ||
+          !(position->intake_beambreak_front() ||
+            position->intake_beambreak_back())) {
+        break;
+      }
+
+      state_ = SuperstructureState::TRANSFERRING;
+      transferring_timer_ = timestamp;
+
+      // Save the side the ball is on for later
+
+      break;
+    }
+    case SuperstructureState::TRANSFERRING: {
+      // If we've been transferring for too long, the ball probably got lost
+      if (timestamp >
+          transferring_timer_ + constants::Values::kBallLostTime()) {
+        intake_state_ = IntakeState::NO_BALL;
+        break;
+      }
+
+      if (intake_state_ == IntakeState::NO_BALL) {
+        state_ = SuperstructureState::IDLE;
+        break;
+      }
+
+      double turret_loading_position =
+          (intake_state_ == IntakeState::INTAKE_FRONT_BALL
+               ? constants::Values::kTurretFrontIntakePos()
+               : constants::Values::kTurretBackIntakePos());
+
+      turret_goal_buffer.Finish(
+          frc971::control_loops::
+              CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+                  *turret_goal_buffer.fbb(), turret_loading_position));
+      turret_goal = &turret_goal_buffer.message();
+
+      const bool turret_near_goal =
+          std::abs(turret_.estimated_position() - turret_loading_position) <
+          kTurretGoalThreshold;
+      if (!turret_near_goal) {
+        break;  // Wait for turret to reach the chosen intake
+      }
+
+      // Transfer rollers and flipper arm belt on
+      transfer_roller_speed =
+          (intake_state_ == IntakeState::INTAKE_FRONT_BALL
+               ? constants::Values::kTransferRollerFrontVoltage()
+               : constants::Values::kTransferRollerBackVoltage());
+      flipper_arms_voltage = constants::Values::kFlipperFeedVoltage();
+
+      // Ball is in catapult
+      if (position->turret_beambreak()) {
+        intake_state_ = IntakeState::NO_BALL;
+        state_ = SuperstructureState::LOADING;
+        loading_timer_ = timestamp;
+      }
+      break;
+    }
+    case SuperstructureState::LOADING: {
+      flipper_arms_voltage = constants::Values::kFlipperFeedVoltage();
+
+      // Keep feeding for kExtraLoadingTime
+
+      can_position_fetcher_.Fetch();
+      const bool flipper_arm_roller_is_stopped =
+          can_position_fetcher_.get() != nullptr &&
+          std::abs(
+              can_position_fetcher_->flipper_arm_integrated_sensor_velocity()) <
+              0.01;
+
+      const bool reading_is_recent =
+          can_position_fetcher_.get() != nullptr &&
+          (timestamp < can_position_fetcher_.context().monotonic_event_time +
+                           constants::Values::kFlipperVelocityValidTime());
+
+      // The ball should go past the turret beambreak to be loaded.
+      // If we got a CAN reading not too long ago, the flippers should have also
+      // stopped.
+      // TODO(milind): maybe it's better to update loading_timer_ as long as the
+      // turret beambreak is triggered.
+      if (timestamp > loading_timer_ + constants::Values::kExtraLoadingTime() &&
+          !position->turret_beambreak() &&
+          (flipper_arm_roller_is_stopped || !reading_is_recent)) {
+        state_ = SuperstructureState::LOADED;
+        reseating_in_catapult_ = false;
+      }
+      break;
+    }
+    case SuperstructureState::LOADED: {
+      if (unsafe_goal != nullptr) {
+        if (unsafe_goal->cancel_shot()) {
+          // Cancel the shot process
+          state_ = SuperstructureState::IDLE;
+        } else if (unsafe_goal->fire()) {
+          // Start if we were asked to and the turret is at goal
+          state_ = SuperstructureState::SHOOTING;
+          prev_shot_count_ = catapult_.shot_count();
+
+          // Reset opening timeout
+          flipper_opening_start_time_ = timestamp;
+        }
+      }
+      break;
+    }
+    case SuperstructureState::SHOOTING: {
+      // Opening flipper arms could fail, wait until they are open using their
+      // potentiometers (the member below is just named encoder).
+      // Be a little more lenient if the flippers were already open in case of
+      // noise or collisions.
+      const double flipper_open_position =
+          (flippers_open_ ? constants::Values::kReseatFlipperPosition()
+                          : constants::Values::kFlipperOpenPosition());
+      flippers_open_ =
+          position->flipper_arm_left()->encoder() >= flipper_open_position &&
+          position->flipper_arm_right()->encoder() >= flipper_open_position;
+
+      if (flippers_open_) {
+        // Hold at kFlipperHoldVoltage
+        flipper_arms_voltage = constants::Values::kFlipperHoldVoltage();
+      } else {
+        // Open at kFlipperOpenVoltage
+        flipper_arms_voltage = constants::Values::kFlipperOpenVoltage();
+      }
+
+      if (!flippers_open_ &&
+          timestamp >
+              loading_timer_ + constants::Values::kFlipperOpeningTimeout()) {
+        // Reseat the ball and try again
+        state_ = SuperstructureState::LOADING;
+        loading_timer_ = timestamp;
+        reseating_in_catapult_ = true;
+        break;
+      }
+
+      const bool turret_near_goal =
+          turret_goal != nullptr &&
+          std::abs(turret_goal->unsafe_goal() - turret_.position()) <
+              kTurretGoalThreshold;
+
+      // If the turret reached the aiming goal, fire!
+      if (flippers_open_ && turret_near_goal) {
+        fire_ = true;
+      }
+
+      // If we started firing and the flippers closed a bit, estop to prevent
+      // damage
+      if (fire_ && !flippers_open_) {
+        catapult_.Estop();
+      }
+
+      const bool near_return_position =
+          (unsafe_goal != nullptr && unsafe_goal->has_catapult() &&
+           unsafe_goal->catapult()->has_return_position() &&
+           std::abs(unsafe_goal->catapult()->return_position()->unsafe_goal() -
+                    catapult_.estimated_position()) < kCatapultGoalThreshold);
+
+      // Once the shot is complete and the catapult is back to its return
+      // position, go back to IDLE
+      if (catapult_.shot_count() > prev_shot_count_ && near_return_position) {
+        prev_shot_count_ = catapult_.shot_count();
+        fire_ = false;
+        state_ = SuperstructureState::IDLE;
+      }
+
+      break;
+    }
+  }
+
+  collision_avoidance_.UpdateGoal(
+      {.intake_front_position = intake_front_.estimated_position(),
+       .intake_back_position = intake_back_.estimated_position(),
+       .turret_position = turret_.estimated_position()},
+      turret_goal);
+
+  turret_.set_min_position(collision_avoidance_.min_turret_goal());
+  turret_.set_max_position(collision_avoidance_.max_turret_goal());
+  intake_front_.set_min_position(collision_avoidance_.min_intake_front_goal());
+  intake_front_.set_max_position(collision_avoidance_.max_intake_front_goal());
+  intake_back_.set_min_position(collision_avoidance_.min_intake_back_goal());
+  intake_back_.set_max_position(collision_avoidance_.max_intake_back_goal());
+
+  // Disable the catapult if we want to restart to prevent damage with flippers
   const flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
-      catapult_status_offset = catapult_.Iterate(
-          unsafe_goal, position,
-          output != nullptr ? &(output_struct.catapult_voltage) : nullptr,
-          status->fbb());
+      catapult_status_offset =
+          catapult_.Iterate(unsafe_goal, position,
+                            output != nullptr && !catapult_.estopped()
+                                ? &(output_struct.catapult_voltage)
+                                : nullptr,
+                            fire_, status->fbb());
 
-  flatbuffers::Offset<RelativeEncoderProfiledJointStatus>
+  const flatbuffers::Offset<RelativeEncoderProfiledJointStatus>
       climber_status_offset = climber_.Iterate(
           unsafe_goal != nullptr ? unsafe_goal->climber() : nullptr,
-          position->climber(),
-          output != nullptr ? &(output_struct.climber_voltage) : nullptr,
-          status->fbb());
+          position->climber(), &output_struct.climber_voltage, status->fbb());
 
-  flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
+  const 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,
+          position->intake_front(), &output_struct.intake_voltage_front,
           status->fbb());
 
-  flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
+  const 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,
+          position->intake_back(), &output_struct.intake_voltage_back,
           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());
+  const flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
+      turret_status_offset =
+          turret_.Iterate(turret_goal, position->turret(),
+                          &output_struct.turret_voltage, 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_struct.flipper_arms_voltage = flipper_arms_voltage;
 
     output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
   }
@@ -121,9 +380,11 @@
   Status::Builder status_builder = status->MakeBuilder<Status>();
 
   const bool zeroed = intake_front_.zeroed() && intake_back_.zeroed() &&
-                      turret_.zeroed() && climber_.zeroed() && catapult_.zeroed();
+                      turret_.zeroed() && climber_.zeroed() &&
+                      catapult_.zeroed();
   const bool estopped = intake_front_.estopped() || intake_back_.estopped() ||
-                        turret_.estopped() || climber_.estopped() || catapult_.estopped();
+                        turret_.estopped() || climber_.estopped() ||
+                        catapult_.estopped();
 
   status_builder.add_zeroed(zeroed);
   status_builder.add_estopped(estopped);
@@ -132,10 +393,17 @@
   status_builder.add_intake_back(intake_status_offset_back);
   status_builder.add_turret(turret_status_offset);
   status_builder.add_climber(climber_status_offset);
+
   status_builder.add_catapult(catapult_status_offset);
   status_builder.add_solve_time(catapult_.solve_time());
-  status_builder.add_mpc_active(catapult_.mpc_active());
   status_builder.add_shot_count(catapult_.shot_count());
+  status_builder.add_mpc_active(catapult_.mpc_active());
+
+  status_builder.add_flippers_open(flippers_open_);
+  status_builder.add_reseating_in_catapult(reseating_in_catapult_);
+  status_builder.add_fire(fire_);
+  status_builder.add_state(state_);
+  status_builder.add_intake_state(intake_state_);
 
   (void)status->Send(status_builder.Finish());
 }
diff --git a/y2022/control_loops/superstructure/superstructure.h b/y2022/control_loops/superstructure/superstructure.h
index dfd4265..13a790a 100644
--- a/y2022/control_loops/superstructure/superstructure.h
+++ b/y2022/control_loops/superstructure/superstructure.h
@@ -7,6 +7,7 @@
 #include "y2022/constants.h"
 #include "y2022/control_loops/superstructure/catapult/catapult.h"
 #include "y2022/control_loops/superstructure/collision_avoidance.h"
+#include "y2022/control_loops/superstructure/superstructure_can_position_generated.h"
 #include "y2022/control_loops/superstructure/superstructure_goal_generated.h"
 #include "y2022/control_loops/superstructure/superstructure_output_generated.h"
 #include "y2022/control_loops/superstructure/superstructure_position_generated.h"
@@ -29,6 +30,11 @@
           ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator,
           ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>;
 
+  static constexpr double kTurretGoalThreshold = 0.01;
+  static constexpr double kCatapultGoalThreshold = 0.01;
+  // potentiometer will be more noisy
+  static constexpr double kFlipperGoalThreshold = 0.05;
+
   explicit Superstructure(::aos::EventLoop *event_loop,
                           std::shared_ptr<const constants::Values> values,
                           const ::std::string &name = "/superstructure");
@@ -58,15 +64,32 @@
   PotAndAbsoluteEncoderSubsystem intake_front_;
   PotAndAbsoluteEncoderSubsystem intake_back_;
   PotAndAbsoluteEncoderSubsystem turret_;
+  catapult::Catapult catapult_;
 
   CollisionAvoidance collision_avoidance_;
 
   aos::Fetcher<frc971::control_loops::drivetrain::Status>
       drivetrain_status_fetcher_;
+  aos::Fetcher<CANPosition> can_position_fetcher_;
+
+  int prev_shot_count_ = 0;
+
+  bool flippers_open_ = false;
+  bool reseating_in_catapult_ = false;
+  bool fire_ = false;
+
+  aos::monotonic_clock::time_point intake_beambreak_timer_ =
+      aos::monotonic_clock::min_time;
+  aos::monotonic_clock::time_point transferring_timer_ =
+      aos::monotonic_clock::min_time;
+  aos::monotonic_clock::time_point loading_timer_ =
+      aos::monotonic_clock::min_time;
+  aos::monotonic_clock::time_point flipper_opening_start_time_ =
+      aos::monotonic_clock::min_time;
+  SuperstructureState state_ = SuperstructureState::IDLE;
+  IntakeState intake_state_ = IntakeState::NO_BALL;
 
   DISALLOW_COPY_AND_ASSIGN(Superstructure);
-
-  catapult::Catapult catapult_;
 };
 
 }  // namespace superstructure
diff --git a/y2022/control_loops/superstructure/superstructure_goal.fbs b/y2022/control_loops/superstructure/superstructure_goal.fbs
index ada39a2..379f6ba 100644
--- a/y2022/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2022/control_loops/superstructure/superstructure_goal.fbs
@@ -3,8 +3,9 @@
 namespace y2022.control_loops.superstructure;
 
 table CatapultGoal {
-  // If true, fire!  The robot will only fire when ready.
-  fire:bool (id: 0);
+  // Old fire flag, only kept for backwards-compatability with logs.
+  // Use the fire flag in the root Goal instead
+  fire:bool (id: 0, deprecated);
 
   // The target shot position and velocity.  If these are provided before fire
   // is called, the optimizer can pre-compute the trajectory.
@@ -37,6 +38,15 @@
 
   // Catapult goal state.
   catapult:CatapultGoal (id: 8);
+
+  // If true, fire!  The robot will only fire when ready.
+  fire:bool (id: 9);
+
+  // Aborts the shooting process if the ball has been loaded into the catapult
+  // and the superstructure is in the LOADED state.
+  cancel_shot:bool (id: 10);
 }
 
+
+
 root_type Goal;
diff --git a/y2022/control_loops/superstructure/superstructure_lib_test.cc b/y2022/control_loops/superstructure/superstructure_lib_test.cc
index 06dee7b..ebd0bd3 100644
--- a/y2022/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2022/control_loops/superstructure/superstructure_lib_test.cc
@@ -244,6 +244,18 @@
     flatbuffers::Offset<frc971::RelativePosition> climber_offset =
         climber_.encoder()->GetSensorValues(&climber_builder);
 
+    frc971::RelativePosition::Builder flipper_arm_left_builder =
+        builder.MakeBuilder<frc971::RelativePosition>();
+    flipper_arm_left_builder.add_encoder(flipper_arm_left_);
+    flatbuffers::Offset<frc971::RelativePosition> flipper_arm_left_offset =
+        flipper_arm_left_builder.Finish();
+
+    frc971::RelativePosition::Builder flipper_arm_right_builder =
+        builder.MakeBuilder<frc971::RelativePosition>();
+    flipper_arm_right_builder.add_encoder(flipper_arm_right_);
+    flatbuffers::Offset<frc971::RelativePosition> flipper_arm_right_offset =
+        flipper_arm_left_builder.Finish();
+
     Position::Builder position_builder = builder.MakeBuilder<Position>();
 
     position_builder.add_intake_front(intake_front_offset);
@@ -251,6 +263,11 @@
     position_builder.add_turret(turret_offset);
     position_builder.add_catapult(catapult_offset);
     position_builder.add_climber(climber_offset);
+    position_builder.add_intake_beambreak_front(intake_beambreak_front_);
+    position_builder.add_intake_beambreak_back(intake_beambreak_back_);
+    position_builder.add_turret_beambreak(turret_beambreak_);
+    position_builder.add_flipper_arm_left(flipper_arm_left_offset);
+    position_builder.add_flipper_arm_right(flipper_arm_right_offset);
 
     CHECK_EQ(builder.Send(position_builder.Finish()),
              aos::RawSender::Error::kOk);
@@ -262,6 +279,19 @@
   PotAndAbsoluteEncoderSimulator *catapult() { return &catapult_; }
   RelativeEncoderSimulator *climber() { return &climber_; }
 
+  void set_intake_beambreak_front(bool triggered) {
+    intake_beambreak_front_ = triggered;
+  }
+
+  void set_intake_beambreak_back(bool triggered) {
+    intake_beambreak_back_ = triggered;
+  }
+
+  void set_turret_beambreak(bool triggered) { turret_beambreak_ = triggered; }
+
+  void set_flipper_arm_left(double pos) { flipper_arm_left_ = pos; }
+  void set_flipper_arm_right(double pos) { flipper_arm_right_ = pos; }
+
  private:
   ::aos::EventLoop *event_loop_;
   const chrono::nanoseconds dt_;
@@ -273,6 +303,11 @@
 
   bool first_ = true;
 
+  bool intake_beambreak_front_ = false;
+  bool intake_beambreak_back_ = false;
+  bool turret_beambreak_ = false;
+  double flipper_arm_left_ = 0.0;
+  double flipper_arm_right_ = 0.0;
   PotAndAbsoluteEncoderSimulator intake_front_;
   PotAndAbsoluteEncoderSimulator intake_back_;
   PotAndAbsoluteEncoderSimulator turret_;
@@ -343,11 +378,6 @@
                   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_catapult() &&
         superstructure_goal_fetcher_->catapult()->has_return_position()) {
       EXPECT_NEAR(superstructure_goal_fetcher_->catapult()
@@ -361,7 +391,23 @@
       EXPECT_NEAR(superstructure_goal_fetcher_->climber()->unsafe_goal(),
                   superstructure_status_fetcher_->climber()->position(), 0.001);
     }
-  }
+
+    if (superstructure_status_fetcher_->intake_state() !=
+        IntakeState::NO_BALL) {
+      EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_front(), 0.0);
+      EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_back(), 0.0);
+    }
+
+    EXPECT_NEAR(superstructure_goal_fetcher_->climber()->unsafe_goal(),
+                superstructure_status_fetcher_->climber()->position(), 0.001);
+
+    if (superstructure_goal_fetcher_->has_turret() &&
+        superstructure_status_fetcher_->state() !=
+            SuperstructureState::TRANSFERRING) {
+      EXPECT_NEAR(superstructure_goal_fetcher_->turret()->unsafe_goal(),
+                  superstructure_status_fetcher_->turret()->position(), 0.001);
+    }
+  }  // namespace testing
 
   void CheckIfZeroed() {
     superstructure_status_fetcher_.Fetch();
@@ -392,7 +438,7 @@
   }
 
   void TestRollerFront(double roller_speed_front,
-                       double roller_speed_compensation) {
+                       double roller_speed_compensation, double expected) {
     auto builder = superstructure_goal_sender_.MakeBuilder();
     Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
     goal_builder.add_roller_speed_front(roller_speed_front);
@@ -400,18 +446,11 @@
     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);
-    }
+    EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_front(), expected);
   }
 
   void TestRollerBack(double roller_speed_back,
-                      double roller_speed_compensation) {
+                      double roller_speed_compensation, double expected) {
     auto builder = superstructure_goal_sender_.MakeBuilder();
     Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
     goal_builder.add_roller_speed_back(roller_speed_back);
@@ -420,18 +459,12 @@
     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);
-    }
+
+    EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_back(), expected);
   }
 
   void TestTransferRoller(double transfer_roller_speed,
-                          double roller_speed_compensation) {
+                          double roller_speed_compensation, double expected) {
     auto builder = superstructure_goal_sender_.MakeBuilder();
     Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
     goal_builder.add_transfer_roller_speed(transfer_roller_speed);
@@ -441,7 +474,7 @@
     ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
     ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
     EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(),
-              transfer_roller_speed);
+              expected);
   }
 
   std::shared_ptr<const constants::Values> values_;
@@ -466,7 +499,7 @@
 
   std::unique_ptr<aos::EventLoop> logger_event_loop_;
   std::unique_ptr<aos::logger::Logger> logger_;
-};
+};  // namespace testing
 
 // Tests that the superstructure does nothing when the goal is to remain
 // still.
@@ -699,28 +732,265 @@
   WaitUntilZeroed();
 
   SendRobotVelocity(3.0);
-  TestRollerFront(-12.0, 1.5);
-  TestRollerFront(12.0, 1.5);
-  TestRollerFront(0.0, 1.5);
+  TestRollerFront(-12.0, 1.5, -7.5);
+  TestRollerFront(12.0, 1.5, 16.5);
+  TestRollerFront(0.0, 1.5, 4.5);
 
   SendRobotVelocity(-3.0);
-  TestRollerFront(-12.0, 1.5);
-  TestRollerFront(12.0, 1.5);
-  TestRollerFront(0.0, 1.5);
+  TestRollerFront(-12.0, 1.5, -12.0);
+  TestRollerFront(12.0, 1.5, 12.0);
+  TestRollerFront(0.0, 1.5, 0.0);
 
   SendRobotVelocity(3.0);
-  TestRollerBack(-12.0, 1.5);
-  TestRollerBack(12.0, 1.5);
-  TestRollerBack(0.0, 1.5);
+  TestRollerBack(-12.0, 1.5, -12.0);
+  TestRollerBack(12.0, 1.5, 12.0);
+  TestRollerBack(0.0, 1.5, 0.0);
 
   SendRobotVelocity(-3.0);
-  TestRollerBack(-12.0, 1.5);
-  TestRollerBack(12.0, 1.5);
-  TestRollerBack(0.0, 1.5);
+  TestRollerBack(-12.0, 1.5, -7.5);
+  TestRollerBack(12.0, 1.5, 16.5);
+  TestRollerBack(0.0, 1.5, 4.5);
 
-  TestTransferRoller(-12.0, 1.5);
-  TestTransferRoller(12.0, 1.5);
-  TestTransferRoller(0.0, 1.5);
+  TestTransferRoller(-12.0, 1.5, -12.0);
+  TestTransferRoller(12.0, 1.5, 12.0);
+  TestTransferRoller(0.0, 1.5, 0.0);
+}
+
+// Tests the whole shooting statemachine - from loading to shooting
+TEST_F(SuperstructureTest, LoadingToShooting) {
+  SetEnabled(true);
+  WaitUntilZeroed();
+
+  SendRobotVelocity(3.0);
+
+  constexpr double kTurretGoal = 3.0;
+  {
+    auto builder = superstructure_goal_sender_.MakeBuilder();
+    flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+        turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+            *builder.fbb(), kTurretGoal);
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_roller_speed_front(12.0);
+    goal_builder.add_roller_speed_back(12.0);
+    goal_builder.add_roller_speed_compensation(0.0);
+    goal_builder.add_turret(turret_offset);
+    builder.CheckOk(builder.Send(goal_builder.Finish()));
+  }
+  RunFor(std::chrono::seconds(2));
+
+  // Make sure that the rollers are spinning, but the superstructure hasn't
+  // transitioned away from idle because the beambreaks haven't been triggered.
+  ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
+  ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+  EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_front(), 12.0);
+  EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_back(), 12.0);
+  EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(), 0.0);
+  EXPECT_EQ(superstructure_status_fetcher_->state(), SuperstructureState::IDLE);
+  EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
+            IntakeState::NO_BALL);
+  EXPECT_NEAR(superstructure_status_fetcher_->turret()->position(), kTurretGoal,
+              0.001);
+  EXPECT_EQ(superstructure_status_fetcher_->shot_count(), 0);
+
+  superstructure_plant_.set_intake_beambreak_front(true);
+  superstructure_plant_.set_intake_beambreak_back(false);
+  RunFor(dt());
+
+  // Make sure that the turret goal is set to be loading from the front intake
+  // and the supersturcture is transferring from the front intake, since that
+  // beambreak was trigerred. Also, the outside rollers should be stopped
+  ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+  ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
+  EXPECT_EQ(superstructure_status_fetcher_->state(),
+            SuperstructureState::TRANSFERRING);
+  EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
+            IntakeState::INTAKE_FRONT_BALL);
+  EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_front(), 0.0);
+  EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_back(), 0.0);
+
+  RunFor(chrono::seconds(1));
+
+  // Make sure that we are still transferring and the front transfer rollers
+  // still have a ball. The turret should now be at the loading position and the
+  // flippers should be feeding the ball.
+  ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
+  ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+  EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_front(), 0.0);
+  EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_back(), 0.0);
+  EXPECT_EQ(superstructure_status_fetcher_->state(),
+            SuperstructureState::TRANSFERRING);
+  EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
+            IntakeState::INTAKE_FRONT_BALL);
+  EXPECT_EQ(superstructure_output_fetcher_->flipper_arms_voltage(),
+            constants::Values::kFlipperFeedVoltage());
+  EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(),
+            constants::Values::kTransferRollerFrontVoltage());
+  EXPECT_NEAR(superstructure_status_fetcher_->turret()->position(),
+              constants::Values::kTurretFrontIntakePos(), 0.001);
+  EXPECT_EQ(superstructure_status_fetcher_->shot_count(), 0);
+
+  superstructure_plant_.set_intake_beambreak_front(false);
+  superstructure_plant_.set_intake_beambreak_back(false);
+  superstructure_plant_.set_turret_beambreak(true);
+  RunFor(dt() * 2);
+
+  // Now that the turret beambreak has been triggered, we should be loading the
+  // ball. The outside rollers shouldn't be limited anymore, and the transfer
+  // rollers should be off. The flippers should still be feeding the ball, and
+  // the intake state should reflect that the ball has been transferred away
+  ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
+  ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+  EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_front(), 12.0);
+  EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_back(), 12.0);
+  EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(), 0.0);
+  EXPECT_EQ(superstructure_status_fetcher_->state(),
+            SuperstructureState::LOADING);
+  EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
+            IntakeState::NO_BALL);
+  EXPECT_EQ(superstructure_output_fetcher_->flipper_arms_voltage(),
+            constants::Values::kFlipperFeedVoltage());
+  EXPECT_EQ(superstructure_status_fetcher_->shot_count(), 0);
+
+  superstructure_plant_.set_turret_beambreak(false);
+  RunFor(constants::Values::kExtraLoadingTime() + dt());
+
+  // Now that the ball has gone past the turret beambreak,
+  // it should be loaded in the catapult and ready for firing.
+  // The flippers should be off.
+  ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
+  ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+  EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_front(), 12.0);
+  EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_back(), 12.0);
+  EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(), 0.0);
+  EXPECT_EQ(superstructure_status_fetcher_->state(),
+            SuperstructureState::LOADED);
+  EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
+            IntakeState::NO_BALL);
+  EXPECT_EQ(superstructure_output_fetcher_->flipper_arms_voltage(), 0.0);
+  EXPECT_EQ(superstructure_status_fetcher_->shot_count(), 0);
+
+  RunFor(std::chrono::seconds(2));
+
+  // After a few seconds, the turret should be at it's aiming goal. The flippers
+  // should still be off and we should still be loaded and ready to fire.
+  ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
+  ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+  EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_front(), 12.0);
+  EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_back(), 12.0);
+  EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(), 0.0);
+  EXPECT_EQ(superstructure_status_fetcher_->state(),
+            SuperstructureState::LOADED);
+  EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
+            IntakeState::NO_BALL);
+  EXPECT_EQ(superstructure_output_fetcher_->flipper_arms_voltage(), 0.0);
+  EXPECT_NEAR(superstructure_status_fetcher_->turret()->position(), kTurretGoal,
+              0.001);
+  EXPECT_EQ(superstructure_status_fetcher_->shot_count(), 0);
+
+  superstructure_plant_.set_intake_beambreak_front(false);
+  superstructure_plant_.set_intake_beambreak_back(true);
+  RunFor(dt() * 2);
+
+  // A ball being intaked from the back should be held by wiggling the transfer
+  // rollers, but we shound't abort the shot from the front intake for it and
+  // move the turret.
+  ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+  ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
+  EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_front(), 0.0);
+  EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_back(), 0.0);
+  LOG(INFO) << superstructure_output_fetcher_->transfer_roller_voltage();
+  EXPECT_TRUE(superstructure_output_fetcher_->transfer_roller_voltage() !=
+                  0.0 &&
+              superstructure_output_fetcher_->transfer_roller_voltage() <=
+                  constants::Values::kTransferRollerFrontWiggleVoltage() &&
+              superstructure_output_fetcher_->transfer_roller_voltage() >=
+                  -constants::Values::kTransferRollerFrontWiggleVoltage());
+  EXPECT_EQ(superstructure_status_fetcher_->state(),
+            SuperstructureState::LOADED);
+  EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
+            IntakeState::INTAKE_BACK_BALL);
+  EXPECT_NEAR(superstructure_status_fetcher_->turret()->position(), kTurretGoal,
+              0.001);
+
+  {
+    auto builder = superstructure_goal_sender_.MakeBuilder();
+    flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+        turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+            *builder.fbb(), kTurretGoal);
+
+    const auto catapult_return_offset =
+        CreateStaticZeroingSingleDOFProfiledSubsystemGoal(*builder.fbb(),
+                                                          -0.87);
+    auto catapult_builder = builder.MakeBuilder<CatapultGoal>();
+    catapult_builder.add_shot_position(0.3);
+    catapult_builder.add_shot_velocity(15.0);
+    catapult_builder.add_return_position(catapult_return_offset);
+    auto catapult_offset = catapult_builder.Finish();
+
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_roller_speed_front(12.0);
+    goal_builder.add_roller_speed_back(12.0);
+    goal_builder.add_roller_speed_compensation(0.0);
+    goal_builder.add_catapult(catapult_offset);
+    goal_builder.add_fire(true);
+    goal_builder.add_turret(turret_offset);
+    builder.CheckOk(builder.Send(goal_builder.Finish()));
+  }
+  superstructure_plant_.set_flipper_arm_left(
+      constants::Values::kFlipperArmRange().upper);
+  superstructure_plant_.set_flipper_arm_right(
+      constants::Values::kFlipperArmRange().upper);
+  RunFor(dt() * 2);
+
+  // Now that we were asked to fire and the flippers are open,
+  // we should be shooting the ball and holding the flippers open.
+  // The turret should still be at its goal, and we should still be wiggling the
+  // transfer rollers to keep the ball in the back intake
+  ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
+  ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+  EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_front(), 0.0);
+  EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_back(), 0.0);
+  EXPECT_TRUE(superstructure_output_fetcher_->transfer_roller_voltage() !=
+                  0.0 &&
+              superstructure_output_fetcher_->transfer_roller_voltage() <=
+                  constants::Values::kTransferRollerFrontWiggleVoltage() &&
+              superstructure_output_fetcher_->transfer_roller_voltage() >=
+                  -constants::Values::kTransferRollerFrontWiggleVoltage());
+  EXPECT_EQ(superstructure_status_fetcher_->state(),
+            SuperstructureState::SHOOTING);
+  EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
+            IntakeState::INTAKE_BACK_BALL);
+  EXPECT_TRUE(superstructure_status_fetcher_->flippers_open());
+  EXPECT_EQ(superstructure_output_fetcher_->flipper_arms_voltage(),
+            constants::Values::kFlipperHoldVoltage());
+  EXPECT_NEAR(superstructure_status_fetcher_->turret()->position(), kTurretGoal,
+              0.001);
+  EXPECT_EQ(superstructure_status_fetcher_->shot_count(), 0);
+
+  superstructure_plant_.set_flipper_arm_left(
+      constants::Values::kFlipperArmRange().upper);
+  superstructure_plant_.set_flipper_arm_right(
+      constants::Values::kFlipperArmRange().upper);
+  superstructure_plant_.set_intake_beambreak_back(false);
+  RunFor(std::chrono::seconds(2));
+
+  // After a bit, we should have completed the shot and be idle.
+  // Since the beambreak was triggered a bit ago, it should still think a ball
+  // is there
+  ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+  EXPECT_EQ(superstructure_status_fetcher_->shot_count(), 1);
+  EXPECT_EQ(superstructure_status_fetcher_->state(), SuperstructureState::IDLE);
+  EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
+            IntakeState::INTAKE_BACK_BALL);
+
+  // Since the intake beambreak hasn't triggered in a while, it should realize
+  // the ball was lost
+  RunFor(std::chrono::seconds(1));
+  ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+  EXPECT_EQ(superstructure_status_fetcher_->shot_count(), 1);
+  EXPECT_EQ(superstructure_status_fetcher_->state(), SuperstructureState::IDLE);
+  EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
+            IntakeState::NO_BALL);
 }
 
 // Make sure that the front and back intakes are never switched
@@ -776,9 +1046,11 @@
 TEST_F(SuperstructureTest, ShootCatapult) {
   SetEnabled(true);
   superstructure_plant_.intake_front()->InitializePosition(
-      constants::Values::kIntakeRange().middle());
+      constants::Values::kIntakeRange().upper);
   superstructure_plant_.intake_back()->InitializePosition(
-      constants::Values::kIntakeRange().middle());
+      constants::Values::kIntakeRange().upper);
+  superstructure_plant_.turret()->InitializePosition(
+      constants::Values::kTurretFrontIntakePos());
 
   WaitUntilZeroed();
 
@@ -794,7 +1066,6 @@
     CatapultGoal::Builder catapult_goal_builder =
         builder.MakeBuilder<CatapultGoal>();
 
-    catapult_goal_builder.add_fire(false);
     catapult_goal_builder.add_shot_position(0.3);
     catapult_goal_builder.add_shot_velocity(15.0);
     catapult_goal_builder.add_return_position(catapult_return_position_offset);
@@ -802,6 +1073,7 @@
         catapult_goal_builder.Finish();
 
     Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_fire(false);
     goal_builder.add_catapult(catapult_goal_offset);
     ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
   }
@@ -817,6 +1089,10 @@
     auto builder = superstructure_goal_sender_.MakeBuilder();
 
     flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+        turret_goal_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+            *builder.fbb(), constants::Values::kTurretFrontIntakePos());
+
+    flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
         catapult_return_position_offset =
             CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
                 *builder.fbb(), constants::Values::kCatapultRange().lower,
@@ -825,7 +1101,6 @@
     CatapultGoal::Builder catapult_goal_builder =
         builder.MakeBuilder<CatapultGoal>();
 
-    catapult_goal_builder.add_fire(true);
     catapult_goal_builder.add_shot_position(0.5);
     catapult_goal_builder.add_shot_velocity(20.0);
     catapult_goal_builder.add_return_position(catapult_return_position_offset);
@@ -834,11 +1109,28 @@
 
     Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
 
+    goal_builder.add_fire(true);
     goal_builder.add_catapult(catapult_goal_offset);
+    goal_builder.add_turret(turret_goal_offset);
     ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
   }
 
-  RunFor(chrono::milliseconds(100));
+  // Make the superstructure statemachine progress to SHOOTING
+  superstructure_plant_.set_intake_beambreak_front(true);
+  superstructure_plant_.set_turret_beambreak(true);
+  superstructure_plant_.set_flipper_arm_left(
+      constants::Values::kFlipperArmRange().upper);
+  superstructure_plant_.set_flipper_arm_right(
+      constants::Values::kFlipperArmRange().upper);
+
+  RunFor(dt() * 4);
+
+  ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+  EXPECT_EQ(superstructure_status_fetcher_->state(),
+            SuperstructureState::LOADING);
+  superstructure_plant_.set_turret_beambreak(false);
+
+  RunFor(chrono::milliseconds(200));
 
   ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
   EXPECT_TRUE(superstructure_status_fetcher_->mpc_active());
@@ -846,12 +1138,17 @@
 
   EXPECT_GT(superstructure_status_fetcher_->catapult()->position(),
             constants::Values::kCatapultRange().lower + 0.1);
+  EXPECT_EQ(superstructure_status_fetcher_->state(),
+            SuperstructureState::SHOOTING);
+  superstructure_plant_.set_intake_beambreak_front(false);
+
   RunFor(chrono::milliseconds(1950));
 
   ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
   EXPECT_NEAR(superstructure_status_fetcher_->catapult()->position(),
               constants::Values::kCatapultRange().lower, 1e-3);
   EXPECT_EQ(superstructure_status_fetcher_->shot_count(), 1);
+  EXPECT_EQ(superstructure_status_fetcher_->state(), SuperstructureState::IDLE);
 }
 
 }  // namespace testing
diff --git a/y2022/control_loops/superstructure/superstructure_output.fbs b/y2022/control_loops/superstructure/superstructure_output.fbs
index a673361..8fa992e 100644
--- a/y2022/control_loops/superstructure/superstructure_output.fbs
+++ b/y2022/control_loops/superstructure/superstructure_output.fbs
@@ -23,6 +23,7 @@
   intake_voltage_back:double (id: 5);
 
   // Intake roller voltages
+  // positive is pulling into the robot
   roller_voltage_front:double (id: 6);
   roller_voltage_back:double (id: 7);
   // One transfer motor for both sides
diff --git a/y2022/control_loops/superstructure/superstructure_status.fbs b/y2022/control_loops/superstructure/superstructure_status.fbs
index 4487f1d..c74cbab 100644
--- a/y2022/control_loops/superstructure/superstructure_status.fbs
+++ b/y2022/control_loops/superstructure/superstructure_status.fbs
@@ -3,12 +3,47 @@
 
 namespace y2022.control_loops.superstructure;
 
+// Contains which intake has a ball
+enum IntakeState : ubyte {
+  NO_BALL,
+  INTAKE_FRONT_BALL,
+  INTAKE_BACK_BALL,
+}
+
+// State of the superstructure state machine
+enum SuperstructureState : ubyte {
+  // Before a ball is intaked, when neither intake beambreak is triggered
+  IDLE,
+  // Transferring ball with transfer rollers. Moves turret to loading position.
+  TRANSFERRING,
+  // Loading the ball into the catapult
+  LOADING,
+  // The ball is loaded into the catapult
+  LOADED,
+  // Waiting for the turret to be at shooting goal and then telling the
+  // catapult to fire.
+  SHOOTING,
+}
+
 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);
+  // The state of the superstructure
+
+  state:SuperstructureState (id: 10);
+  // Intaking state
+  intake_state:IntakeState (id: 11);
+  // Whether the flippers are open for shooting
+  flippers_open:bool (id: 12);
+  // Whether the flippers failed to open and we are retrying
+  reseating_in_catapult:bool (id: 13);
+  // Whether the catapult was told to fire,
+  // meaning that the turret and flippers are ready for firing
+  // and we were asked to fire. Different from fire flag in goal.
+  fire:bool (id: 14);
 
   // Subsystem statuses
   climber:frc971.control_loops.RelativeEncoderProfiledJointStatus (id: 2);