diff --git a/y2024/control_loops/superstructure/shooter.cc b/y2024/control_loops/superstructure/shooter.cc
index 2ddaf64..3216b69 100644
--- a/y2024/control_loops/superstructure/shooter.cc
+++ b/y2024/control_loops/superstructure/shooter.cc
@@ -35,12 +35,12 @@
 Shooter::Iterate(
     const y2024::control_loops::superstructure::Position *position,
     const y2024::control_loops::superstructure::ShooterGoal *shooter_goal,
-    double *catapult_output, double *altitude_output, double *turret_output,
-    double *retention_roller_output,
+    bool fire, double *catapult_output, double *altitude_output,
+    double *turret_output, double *retention_roller_output,
     double *retention_roller_stator_current_limit, double /*battery_voltage*/,
     CollisionAvoidance *collision_avoidance, const double intake_pivot_position,
     double *max_intake_pivot_position, double *min_intake_pivot_position,
-    flatbuffers::FlatBufferBuilder *fbb) {
+    bool standby, flatbuffers::FlatBufferBuilder *fbb) {
   drivetrain_status_fetcher_.Fetch();
 
   // If our current is over the minimum current and our velocity is under our
@@ -91,9 +91,19 @@
 
   bool aiming = false;
 
-  // We don't have the note so we should be ready to intake it.
-  if (shooter_goal == nullptr || !shooter_goal->auto_aim() ||
-      (!piece_loaded && state_ == CatapultState::READY)) {
+  if (standby) {
+    // Note is going to AMP or TRAP, move turret to extend
+    // collision avoidance position.
+    PopulateStaticZeroingSingleDOFProfiledSubsystemGoal(
+        turret_goal_builder.get(),
+        robot_constants_->common()->turret_avoid_extend_collision_position());
+
+    PopulateStaticZeroingSingleDOFProfiledSubsystemGoal(
+        altitude_goal_builder.get(),
+        robot_constants_->common()->altitude_loading_position());
+  } else if (shooter_goal == nullptr || !shooter_goal->auto_aim() ||
+             (!piece_loaded && state_ == CatapultState::READY)) {
+    // We don't have the note so we should be ready to intake it.
     PopulateStaticZeroingSingleDOFProfiledSubsystemGoal(
         turret_goal_builder.get(),
         robot_constants_->common()->turret_loading_position());
@@ -101,6 +111,7 @@
     PopulateStaticZeroingSingleDOFProfiledSubsystemGoal(
         altitude_goal_builder.get(),
         robot_constants_->common()->altitude_loading_position());
+
   } else {
     // We have a game piece, lets start aiming.
     if (drivetrain_status_fetcher_.get() != nullptr) {
@@ -121,11 +132,13 @@
 
   // The builder will contain either the auto-aim goal, or the loading goal. Use
   // it if we have no goal, or no subsystem goal, or if we are auto-aiming.
+
   const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
       *turret_goal = (shooter_goal != nullptr && !shooter_goal->auto_aim() &&
-                      shooter_goal->has_turret_position())
+                      !standby && shooter_goal->has_turret_position())
                          ? shooter_goal->turret_position()
                          : &turret_goal_builder->AsFlatbuffer();
+
   const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
       *altitude_goal = (shooter_goal != nullptr && !shooter_goal->auto_aim() &&
                         shooter_goal->has_altitude_position())
@@ -203,6 +216,7 @@
 
     switch (state_) {
       case CatapultState::READY:
+        [[fallthrough]];
       case CatapultState::LOADED: {
         if (piece_loaded) {
           state_ = CatapultState::LOADED;
@@ -212,8 +226,8 @@
 
         const bool catapult_close = CatapultClose();
 
-        if (subsystems_in_range && shooter_goal != nullptr &&
-            shooter_goal->fire() && catapult_close && piece_loaded) {
+        if (subsystems_in_range && shooter_goal != nullptr && fire &&
+            catapult_close && piece_loaded) {
           state_ = CatapultState::FIRING;
         } else {
           catapult_.set_controller_index(0);
diff --git a/y2024/control_loops/superstructure/shooter.h b/y2024/control_loops/superstructure/shooter.h
index 1550fdc..87b0bc0 100644
--- a/y2024/control_loops/superstructure/shooter.h
+++ b/y2024/control_loops/superstructure/shooter.h
@@ -64,14 +64,16 @@
   }
 
   flatbuffers::Offset<ShooterStatus> Iterate(
-      const Position *position, const ShooterGoal *shooter_goal,
+      const Position *position, const ShooterGoal *shooter_goal, bool fire,
       double *catapult_output, double *altitude_output, double *turret_output,
       double *retention_roller_output,
       double *retention_roller_stator_current_limit, double battery_voltage,
       /* Hacky way to use collision avoidance in this class */
       CollisionAvoidance *collision_avoidance,
       const double intake_pivot_position, double *max_turret_intake_position,
-      double *min_intake_pivot_position, flatbuffers::FlatBufferBuilder *fbb);
+      double *min_intake_pivot_position,
+      /* If true, go to extend collision avoidance position */ bool standby,
+      flatbuffers::FlatBufferBuilder *fbb);
 
  private:
   CatapultState state_ = CatapultState::RETRACTING;
diff --git a/y2024/control_loops/superstructure/superstructure.cc b/y2024/control_loops/superstructure/superstructure.cc
index eeaa2fa..ab863eb 100644
--- a/y2024/control_loops/superstructure/superstructure.cc
+++ b/y2024/control_loops/superstructure/superstructure.cc
@@ -1,13 +1,23 @@
 #include "y2024/control_loops/superstructure/superstructure.h"
 
+#include <chrono>
+
 #include "aos/events/event_loop.h"
 #include "aos/flatbuffer_merge.h"
 #include "aos/network/team_number.h"
+#include "aos/time/time.h"
 #include "frc971/shooter_interpolation/interpolation.h"
 #include "frc971/zeroing/wrap.h"
 
 DEFINE_bool(ignore_distance, false,
-            "If true, ignore distance when shooting and obay joystick_reader");
+            "If true, ignore distance when shooting and obey joystick_reader");
+
+// The threshold used when decided if the extend is close enough to a goal to
+// continue.
+constexpr double kExtendThreshold = 0.01;
+
+constexpr double kTurretLoadingThreshold = 0.01;
+constexpr double kAltitudeLoadingThreshold = 0.01;
 
 namespace y2024::control_loops::superstructure {
 
@@ -18,11 +28,9 @@
 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),
-      values_(values),
       constants_fetcher_(event_loop),
       robot_constants_(CHECK_NOTNULL(&constants_fetcher_.constants())),
       drivetrain_status_fetcher_(
@@ -30,16 +38,22 @@
               "/drivetrain")),
       joystick_state_fetcher_(
           event_loop->MakeFetcher<aos::JoystickState>("/aos")),
-      transfer_goal_(TransferRollerGoal::NONE),
       intake_pivot_(robot_constants_->common()->intake_pivot(),
                     robot_constants_->robot()->intake_constants()),
       climber_(
           robot_constants_->common()->climber(),
           robot_constants_->robot()->climber_constants()->zeroing_constants()),
-      shooter_(event_loop, robot_constants_) {
+      shooter_(event_loop, robot_constants_),
+      extend_(
+          robot_constants_->common()->extend(),
+          robot_constants_->robot()->extend_constants()->zeroing_constants()) {
   event_loop->SetRuntimeRealtimePriority(30);
 }
 
+bool PositionNear(double position, double goal, double threshold) {
+  return std::abs(position - goal) < threshold;
+}
+
 void Superstructure::RunIteration(const Goal *unsafe_goal,
                                   const Position *position,
                                   aos::Sender<Output>::Builder *output,
@@ -54,65 +68,12 @@
     intake_pivot_.Reset();
     climber_.Reset();
     shooter_.Reset();
+    extend_.Reset();
   }
 
   OutputT output_struct;
 
-  double intake_pivot_position =
-      robot_constants_->common()->intake_pivot_set_points()->retracted();
-
-  if (unsafe_goal != nullptr &&
-      unsafe_goal->intake_pivot_goal() == IntakePivotGoal::EXTENDED) {
-    intake_pivot_position =
-        robot_constants_->common()->intake_pivot_set_points()->extended();
-  }
-
-  IntakeRollerStatus intake_roller_state = IntakeRollerStatus::NONE;
-
-  switch (unsafe_goal != nullptr ? unsafe_goal->intake_roller_goal()
-                                 : IntakeRollerGoal::NONE) {
-    case IntakeRollerGoal::NONE:
-      output_struct.intake_roller_voltage = 0.0;
-      break;
-    case IntakeRollerGoal::SPIT:
-      transfer_goal_ = TransferRollerGoal::TRANSFER_OUT;
-      intake_roller_state = IntakeRollerStatus::SPITTING;
-      output_struct.intake_roller_voltage =
-          robot_constants_->common()->intake_roller_voltages()->spitting();
-      break;
-    case IntakeRollerGoal::INTAKE:
-      transfer_goal_ = TransferRollerGoal::TRANSFER_IN;
-      intake_roller_state = IntakeRollerStatus::INTAKING;
-      output_struct.intake_roller_voltage =
-          robot_constants_->common()->intake_roller_voltages()->intaking();
-      break;
-  }
-
-  TransferRollerStatus transfer_roller_state = TransferRollerStatus::NONE;
-
-  switch (unsafe_goal != nullptr ? transfer_goal_ : TransferRollerGoal::NONE) {
-    case TransferRollerGoal::NONE:
-      output_struct.transfer_roller_voltage = 0.0;
-      break;
-    case TransferRollerGoal::TRANSFER_IN:
-      if (position->transfer_beambreak()) {
-        transfer_goal_ = TransferRollerGoal::NONE;
-        transfer_roller_state = TransferRollerStatus::NONE;
-        output_struct.transfer_roller_voltage = 0.0;
-        break;
-      }
-      transfer_roller_state = TransferRollerStatus::TRANSFERING_IN;
-      output_struct.transfer_roller_voltage =
-          robot_constants_->common()->transfer_roller_voltages()->transfer_in();
-      break;
-    case TransferRollerGoal::TRANSFER_OUT:
-      transfer_roller_state = TransferRollerStatus::TRANSFERING_OUT;
-      output_struct.transfer_roller_voltage = robot_constants_->common()
-                                                  ->transfer_roller_voltages()
-                                                  ->transfer_out();
-      break;
-  }
-
+  // Handle Climber Goal separately from main superstructure state machine
   double climber_position =
       robot_constants_->common()->climber_set_points()->retract();
 
@@ -135,6 +96,366 @@
     }
   }
 
+  // If we started off preloaded, skip to the ready state.
+  if (unsafe_goal != nullptr && unsafe_goal->shooter_goal() &&
+      unsafe_goal->shooter_goal()->preloaded()) {
+    if (state_ != SuperstructureState::READY &&
+        state_ != SuperstructureState::FIRING) {
+      state_ = SuperstructureState::READY;
+    }
+  }
+
+  // Handle the intake pivot goal separately from the main superstructure state
+  IntakeRollerStatus intake_roller_state = IntakeRollerStatus::NONE;
+  double intake_pivot_position =
+      robot_constants_->common()->intake_pivot_set_points()->retracted();
+
+  if (unsafe_goal != nullptr) {
+    switch (unsafe_goal->intake_goal()) {
+      case IntakeGoal::INTAKE:
+        intake_pivot_position =
+            robot_constants_->common()->intake_pivot_set_points()->extended();
+        break;
+      case IntakeGoal::SPIT:
+        intake_pivot_position =
+            robot_constants_->common()->intake_pivot_set_points()->retracted();
+        break;
+      case IntakeGoal::NONE:
+        intake_pivot_position =
+            robot_constants_->common()->intake_pivot_set_points()->retracted();
+        break;
+    }
+  }
+
+  ExtendRollerStatus extend_roller_status = ExtendRollerStatus::IDLE;
+  ExtendStatus extend_goal = ExtendStatus::RETRACTED;
+
+  // True if the extend is moving towards a goal
+  bool extend_moving = false;
+
+  TransferRollerStatus transfer_roller_status = TransferRollerStatus::NONE;
+
+  const ExtendSetPoints *extend_set_points =
+      robot_constants_->common()->extend_set_points();
+
+  // Checks if the extend is close enough to the retracted position to be
+  // considered ready to accept note from the transfer rollers.
+  const bool extend_ready_for_transfer = PositionNear(
+      extend_.position(), extend_set_points->retracted(), kExtendThreshold);
+
+  // If true, the turret should be moved to the position to avoid collision with
+  // the extend.
+  bool move_turret_to_standby = false;
+
+  // Superstructure state machine:
+  // 1. IDLE. The intake is retracted and there is no note in the robot.
+  // Wait for a intake goal to switch state to INTAKING if the extend is ready
+  // 2. INTAKING. Intake the note and transfer it towards the extend.
+  // Give intake, transfer, and extend rollers positive voltage to intake and
+  // transfer. Switch to LOADED when the extend beambreak is triggered.
+  // 3. LOADED. The note is in the extend and the extend is retracted.
+  // Wait for a note goal to switch state to MOVING.
+  // For AMP/TRAP goals, check that the turret is in a position to avoid
+  // collision.
+  // 4. MOVING. The extend is moving towards a goal (AMP, TRAP, or CATAPULT).
+  // For CATAPULT goals, wait for the turret and altitude to be in a position to
+  // accept the note from the extend.
+  // Wait for the extend to reach the goal and switch state to READY if
+  // AMP or TRAP, or to LOADING_CATAPULT if CATAPULT.
+  // 5. LOADING_CATAPULT. The extend is at the position to load the catapult.
+  // Activate the extend roller to transfer the note to the catapult.
+  // Switch state to READY when the catapult beambreak is triggered.
+  // 6. READY. Ready for fire command. The note is either loaded in the catapult
+  // or in the extend and at the AMP or TRAP position. Wait for a fire command.
+  // 7. FIRING. The note is being fired, either from the extend or the catapult.
+  // Switch state back to IDLE when the note is fired.
+
+  switch (state_) {
+    case SuperstructureState::IDLE:
+      if (unsafe_goal != nullptr &&
+          unsafe_goal->intake_goal() == IntakeGoal::INTAKE &&
+          extend_ready_for_transfer) {
+        state_ = SuperstructureState::INTAKING;
+      }
+      extend_goal = ExtendStatus::RETRACTED;
+      catapult_requested_ = false;
+      break;
+    case SuperstructureState::INTAKING:
+
+      // Switch to LOADED state when the extend beambreak is triggered
+      // meaning the note is loaded in the extend
+      if (position->extend_beambreak()) {
+        state_ = SuperstructureState::LOADED;
+      }
+      intake_roller_state = IntakeRollerStatus::INTAKING;
+      transfer_roller_status = TransferRollerStatus::TRANSFERING_IN;
+      extend_roller_status = ExtendRollerStatus::TRANSFERING_TO_EXTEND;
+      extend_goal = ExtendStatus::RETRACTED;
+
+      if (!catapult_requested_ && unsafe_goal != nullptr &&
+          unsafe_goal->note_goal() == NoteGoal::CATAPULT) {
+        catapult_requested_ = true;
+      }
+
+      break;
+    case SuperstructureState::LOADED:
+      if (catapult_requested_ == true) {
+        state_ = SuperstructureState::MOVING;
+        break;
+      }
+
+      if (unsafe_goal != nullptr &&
+          unsafe_goal->note_goal() != NoteGoal::NONE) {
+        // If the goal is AMP or TRAP, check if the turret is in a position to
+        // avoid collision when the extend moves.
+        if (unsafe_goal->note_goal() == NoteGoal::AMP ||
+            unsafe_goal->note_goal() == NoteGoal::TRAP) {
+          bool turret_ready_for_extend_move =
+              PositionNear(shooter_.turret().estimated_position(),
+                           robot_constants_->common()
+                               ->turret_avoid_extend_collision_position(),
+                           kTurretLoadingThreshold);
+
+          if (turret_ready_for_extend_move) {
+            state_ = SuperstructureState::MOVING;
+          } else {
+            move_turret_to_standby = true;
+          }
+        } else if (unsafe_goal->note_goal() == NoteGoal::CATAPULT) {
+          // If catapult is requested, switch to MOVING state
+          state_ = SuperstructureState::MOVING;
+        }
+      }
+      extend_goal = ExtendStatus::RETRACTED;
+      if (!catapult_requested_ && unsafe_goal != nullptr &&
+          unsafe_goal->note_goal() == NoteGoal::CATAPULT) {
+        catapult_requested_ = true;
+      }
+      break;
+    case SuperstructureState::MOVING:
+
+      if (catapult_requested_) {
+        extend_goal = ExtendStatus::CATAPULT;
+
+        // Check if the extend is at the position to load the catapult
+        bool extend_ready_for_catapult_transfer =
+            PositionNear(extend_.position(), extend_set_points->catapult(),
+                         kExtendThreshold);
+
+        // Check if the turret is at the position to accept the note from extend
+        bool turret_ready_for_load =
+            PositionNear(shooter_.turret().estimated_position(),
+                         robot_constants_->common()->turret_loading_position(),
+                         kTurretLoadingThreshold);
+
+        // Check if the altitude is at the position to accept the note from
+        // extend
+        bool altitude_ready_for_load = PositionNear(
+            shooter_.altitude().estimated_position(),
+            robot_constants_->common()->altitude_loading_position(),
+            kAltitudeLoadingThreshold);
+
+        if (extend_ready_for_catapult_transfer && turret_ready_for_load &&
+            altitude_ready_for_load) {
+          state_ = SuperstructureState::LOADING_CATAPULT;
+        }
+      } else {
+        if (unsafe_goal != nullptr) {
+          switch (unsafe_goal->note_goal()) {
+            case NoteGoal::AMP:
+              extend_goal = ExtendStatus::AMP;
+              move_turret_to_standby = true;
+              // Check if the extend is at the AMP position and if it is
+              // switch to READY state
+              if (PositionNear(extend_.position(), extend_set_points->amp(),
+                               kExtendThreshold)) {
+                state_ = SuperstructureState::READY;
+              }
+              break;
+            case NoteGoal::TRAP:
+              extend_goal = ExtendStatus::TRAP;
+              move_turret_to_standby = true;
+              // Check if the extend is at the TRAP position and if it is
+              // switch to READY state
+              if (PositionNear(extend_.position(), extend_set_points->trap(),
+                               kExtendThreshold)) {
+                state_ = SuperstructureState::READY;
+              }
+              break;
+            case NoteGoal::NONE:
+              extend_goal = ExtendStatus::RETRACTED;
+              move_turret_to_standby = true;
+              if (extend_ready_for_transfer) {
+                state_ = SuperstructureState::LOADED;
+              }
+              break;
+            case NoteGoal::CATAPULT:
+              catapult_requested_ = true;
+              extend_goal = ExtendStatus::CATAPULT;
+              break;
+          }
+        }
+      }
+
+      extend_moving = true;
+      break;
+    case SuperstructureState::LOADING_CATAPULT:
+      extend_moving = false;
+      extend_goal = ExtendStatus::CATAPULT;
+      extend_roller_status = ExtendRollerStatus::TRANSFERING_TO_CATAPULT;
+
+      // Switch to READY state when the catapult beambreak is triggered
+      if (position->catapult_beambreak()) {
+        state_ = SuperstructureState::READY;
+      }
+      break;
+    case SuperstructureState::READY:
+      extend_moving = false;
+
+      // Switch to FIRING state when the fire button is pressed
+      if (unsafe_goal != nullptr && unsafe_goal->fire()) {
+        state_ = SuperstructureState::FIRING;
+      }
+
+      if (catapult_requested_) {
+        extend_goal = ExtendStatus::CATAPULT;
+      } else {
+        if (unsafe_goal != nullptr) {
+          if (unsafe_goal->note_goal() == NoteGoal::AMP) {
+            extend_goal = ExtendStatus::AMP;
+            move_turret_to_standby = true;
+          } else if (unsafe_goal->note_goal() == NoteGoal::TRAP) {
+            extend_goal = ExtendStatus::TRAP;
+            move_turret_to_standby = true;
+          } else {
+            extend_goal = ExtendStatus::RETRACTED;
+            extend_moving = true;
+            state_ = SuperstructureState::MOVING;
+          }
+        }
+      }
+
+      break;
+    case SuperstructureState::FIRING:
+      if (catapult_requested_) {
+        extend_goal = ExtendStatus::CATAPULT;
+
+        // Reset the state to IDLE when the game piece is fired from the
+        // catapult. We consider the game piece to be fired from the catapult
+        // when the catapultbeambreak is no longer triggered.
+        if (!position->catapult_beambreak()) {
+          state_ = SuperstructureState::IDLE;
+        }
+      } else {
+        if (unsafe_goal != nullptr &&
+            unsafe_goal->note_goal() == NoteGoal::AMP) {
+          extend_roller_status = ExtendRollerStatus::SCORING_IN_AMP;
+          extend_goal = ExtendStatus::AMP;
+        } else if (unsafe_goal != nullptr &&
+                   unsafe_goal->note_goal() == NoteGoal::TRAP) {
+          extend_roller_status = ExtendRollerStatus::SCORING_IN_TRAP;
+          extend_goal = ExtendStatus::TRAP;
+        }
+
+        // Reset the state to IDLE when the game piece is fired from the extend.
+        // We consider the game piece to be fired from the extend when
+        // the extend beambreak is no longer triggered and the fire button is
+        // released.
+        if (!position->extend_beambreak() && unsafe_goal != nullptr &&
+            !unsafe_goal->fire()) {
+          state_ = SuperstructureState::IDLE;
+        }
+      }
+      break;
+  }
+
+  if (unsafe_goal != nullptr &&
+      unsafe_goal->intake_goal() == IntakeGoal::SPIT) {
+    intake_roller_state = IntakeRollerStatus::SPITTING;
+    transfer_roller_status = TransferRollerStatus::TRANSFERING_OUT;
+  }
+
+  // Update Intake Roller voltage based on status from state machine.
+  switch (intake_roller_state) {
+    case IntakeRollerStatus::NONE:
+      output_struct.intake_roller_voltage = 0.0;
+      break;
+    case IntakeRollerStatus::SPITTING:
+      output_struct.intake_roller_voltage =
+          robot_constants_->common()->intake_roller_voltages()->spitting();
+      break;
+    case IntakeRollerStatus::INTAKING:
+      output_struct.intake_roller_voltage =
+          robot_constants_->common()->intake_roller_voltages()->intaking();
+      break;
+  }
+
+  // Update Transfer Roller voltage based on status from state machine.
+  switch (transfer_roller_status) {
+    case TransferRollerStatus::NONE:
+      output_struct.transfer_roller_voltage = 0.0;
+      break;
+    case TransferRollerStatus::TRANSFERING_IN:
+      output_struct.transfer_roller_voltage =
+          robot_constants_->common()->transfer_roller_voltages()->transfer_in();
+      break;
+    case TransferRollerStatus::TRANSFERING_OUT:
+      output_struct.transfer_roller_voltage = robot_constants_->common()
+                                                  ->transfer_roller_voltages()
+                                                  ->transfer_out();
+      break;
+  }
+
+  // Update Extend Roller voltage based on status from state machine.
+  const ExtendRollerVoltages *extend_roller_voltages =
+      robot_constants_->common()->extend_roller_voltages();
+  switch (extend_roller_status) {
+    case ExtendRollerStatus::IDLE:
+      // No voltage applied when idle
+      output_struct.extend_roller_voltage = 0.0;
+      break;
+    case ExtendRollerStatus::TRANSFERING_TO_EXTEND:
+      output_struct.extend_roller_voltage = extend_roller_voltages->scoring();
+      break;
+    case ExtendRollerStatus::SCORING_IN_AMP:
+      [[fallthrough]];
+    case ExtendRollerStatus::SCORING_IN_TRAP:
+      // Apply scoring voltage during scoring in amp or trap
+      output_struct.extend_roller_voltage = extend_roller_voltages->scoring();
+      break;
+    case ExtendRollerStatus::TRANSFERING_TO_CATAPULT:
+      // Apply scoring voltage during transferring to catapult
+      output_struct.extend_roller_voltage = extend_roller_voltages->scoring();
+      break;
+  }
+
+  double extend_position = 0.0;
+
+  // Set the extend position based on the state machine output
+  switch (extend_goal) {
+    case ExtendStatus::RETRACTED:
+      extend_position = extend_set_points->retracted();
+      break;
+    case ExtendStatus::AMP:
+      extend_position = extend_set_points->amp();
+      break;
+    case ExtendStatus::TRAP:
+      extend_position = extend_set_points->trap();
+      break;
+    case ExtendStatus::CATAPULT:
+      extend_position = extend_set_points->catapult();
+      break;
+    case ExtendStatus::MOVING:
+      // Should never happen
+      break;
+  }
+
+  // Set the extend status based on the state machine output
+  // If the extend is moving, the status is MOVING, otherwise it is the same
+  // as extend_status
+  ExtendStatus extend_status =
+      (extend_moving ? ExtendStatus::MOVING : extend_goal);
+
   if (joystick_state_fetcher_.Fetch() &&
       joystick_state_fetcher_->has_alliance()) {
     alliance_ = joystick_state_fetcher_->alliance();
@@ -183,12 +504,13 @@
   const bool disabled = intake_pivot_.Correct(
       intake_pivot_goal, position->intake_pivot(), intake_output == nullptr);
 
-  // TODO(max): Change how we handle the collision with the turret and intake to
-  // be clearer
+  // TODO(max): Change how we handle the collision with the turret and
+  // intake to be clearer
   const flatbuffers::Offset<ShooterStatus> shooter_status_offset =
       shooter_.Iterate(
           position,
           unsafe_goal != nullptr ? unsafe_goal->shooter_goal() : nullptr,
+          unsafe_goal != nullptr ? unsafe_goal->fire() : false,
           output != nullptr ? &output_struct.catapult_voltage : nullptr,
           output != nullptr ? &output_struct.altitude_voltage : nullptr,
           output != nullptr ? &output_struct.turret_voltage : nullptr,
@@ -198,7 +520,7 @@
               : nullptr,
           robot_state().voltage_battery(), &collision_avoidance_,
           intake_pivot_.estimated_position(), &max_intake_pivot_position,
-          &min_intake_pivot_position, status->fbb());
+          &min_intake_pivot_position, move_turret_to_standby, status->fbb());
 
   intake_pivot_.set_min_position(min_intake_pivot_position);
   intake_pivot_.set_max_position(max_intake_pivot_position);
@@ -216,25 +538,46 @@
   const flatbuffers::Offset<AbsoluteEncoderProfiledJointStatus>
       intake_pivot_status_offset = intake_pivot_.MakeStatus(status->fbb());
 
+  aos::FlatbufferFixedAllocatorArray<
+      frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal, 512>
+      note_goal_buffer;
+
+  note_goal_buffer.Finish(
+      frc971::control_loops::CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+          *note_goal_buffer.fbb(), extend_position));
+
+  const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
+      *note_goal = &note_goal_buffer.message();
+
+  const flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
+      extend_status_offset = extend_.Iterate(
+          note_goal, position->extend(),
+          output != nullptr ? &output_struct.extend_voltage : nullptr,
+          status->fbb());
+
   if (output) {
     output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
   }
 
   Status::Builder status_builder = status->MakeBuilder<Status>();
 
-  const bool zeroed =
-      intake_pivot_.zeroed() && climber_.zeroed() && shooter_.zeroed();
-  const bool estopped =
-      intake_pivot_.estopped() || climber_.estopped() || shooter_.estopped();
+  const bool zeroed = intake_pivot_.zeroed() && climber_.zeroed() &&
+                      shooter_.zeroed() && extend_.zeroed();
+  const bool estopped = intake_pivot_.estopped() || climber_.estopped() ||
+                        shooter_.estopped() || extend_.estopped();
 
   status_builder.add_zeroed(zeroed);
   status_builder.add_estopped(estopped);
   status_builder.add_intake_roller(intake_roller_state);
   status_builder.add_intake_pivot(intake_pivot_status_offset);
-  status_builder.add_transfer_roller(transfer_roller_state);
+  status_builder.add_transfer_roller(transfer_roller_status);
   status_builder.add_climber(climber_status_offset);
   status_builder.add_shooter(shooter_status_offset);
   status_builder.add_collided(collided);
+  status_builder.add_extend_roller(extend_roller_status);
+  status_builder.add_extend_status(extend_status);
+  status_builder.add_extend(extend_status_offset);
+  status_builder.add_state(state_);
 
   (void)status->Send(status_builder.Finish());
 }
diff --git a/y2024/control_loops/superstructure/superstructure.h b/y2024/control_loops/superstructure/superstructure.h
index 650092b..963d8c4 100644
--- a/y2024/control_loops/superstructure/superstructure.h
+++ b/y2024/control_loops/superstructure/superstructure.h
@@ -3,6 +3,7 @@
 
 #include "aos/events/event_loop.h"
 #include "aos/json_to_flatbuffer.h"
+#include "aos/time/time.h"
 #include "frc971/constants/constants_sender_lib.h"
 #include "frc971/control_loops/control_loop.h"
 #include "frc971/control_loops/drivetrain/drivetrain_can_position_generated.h"
@@ -33,7 +34,6 @@
           ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>;
 
   explicit Superstructure(::aos::EventLoop *event_loop,
-                          std::shared_ptr<const constants::Values> values,
                           const ::std::string &name = "/superstructure");
 
   inline const AbsoluteEncoderSubsystem &intake_pivot() const {
@@ -45,6 +45,9 @@
   }
 
   inline const Shooter &shooter() const { return shooter_; }
+  inline const PotAndAbsoluteEncoderSubsystem &extend() const {
+    return extend_;
+  }
 
   double robot_velocity() const;
 
@@ -54,7 +57,6 @@
                             aos::Sender<Status>::Builder *status) override;
 
  private:
-  std::shared_ptr<const constants::Values> values_;
   frc971::constants::ConstantsFetcher<Constants> constants_fetcher_;
   const Constants *robot_constants_;
   aos::Fetcher<frc971::control_loops::drivetrain::Status>
@@ -65,12 +67,19 @@
 
   aos::Alliance alliance_ = aos::Alliance::kInvalid;
 
-  TransferRollerGoal transfer_goal_;
+  bool catapult_requested_ = false;
+
+  SuperstructureState state_ = SuperstructureState::IDLE;
+
+  aos::monotonic_clock::time_point transfer_start_time_ =
+      aos::monotonic_clock::time_point::min();
+
   AbsoluteEncoderSubsystem intake_pivot_;
   PotAndAbsoluteEncoderSubsystem climber_;
 
   Shooter shooter_;
 
+  PotAndAbsoluteEncoderSubsystem extend_;
   DISALLOW_COPY_AND_ASSIGN(Superstructure);
 };
 
diff --git a/y2024/control_loops/superstructure/superstructure_goal.fbs b/y2024/control_loops/superstructure/superstructure_goal.fbs
index a57422b..b346e97 100644
--- a/y2024/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2024/control_loops/superstructure/superstructure_goal.fbs
@@ -3,26 +3,13 @@
 
 namespace y2024.control_loops.superstructure;
 
-// Represents goal for intake rollers
-enum IntakeRollerGoal : ubyte {
+// Represents goal for the intake pivot and rollers
+// INTAKE will extend the pivot and turn on the rollers to intake the note.
+// SPIT will extend the pivot and turn on the rollers (in reverse) to spit out the note.
+enum IntakeGoal : ubyte {
     NONE = 0,
-    SPIT = 1,
-    INTAKE = 2,
-}
-
-// Represents goal for pivot on intake
-enum IntakePivotGoal : ubyte {
-    RETRACTED = 0,
-    EXTENDED = 1,
-}
-
-// Represents goal of transfer rollers
-// TRANSFER_IN is for transfering game piece in from the intake to the shooter
-// TRANSFER_OUT is for transfering game piece out to the intake for spitting
-enum TransferRollerGoal : ubyte {
-    NONE = 0,
-    TRANSFER_IN = 1,
-    TRANSFER_OUT = 2,
+    INTAKE = 1,
+    SPIT = 2,
 }
 
 // Represents goal for climber
@@ -37,45 +24,39 @@
 
 table ShooterGoal {
     catapult_goal:frc971.control_loops.catapult.CatapultGoal (id: 0);
-    fire: bool (id: 1);
-    // If true we ignore the other provided positions
-    auto_aim: bool (id: 2);
+    auto_aim: bool (id: 1);
 
     // Position for the turret when we aren't auto aiming
-    turret_position: frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 3);
+    turret_position: frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 2);
 
     // Position for the altitude when we aren't auto aiming
-    altitude_position: frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 4);
+    altitude_position: frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 3);
 
      // If true, we started with the ball loaded and should proceed to that state.
-    preloaded:bool = false (id: 5);
+    preloaded:bool = false (id: 4);
 }
 
-// Represents goal for extend
-// RETRACT is for retracting the extender to stowed position
-// In the retracted position, the game piece may be transfered to the catapult
-// AMP is for extending the extender to the AMP scoring position
-// TRAP is for extending the extender to the TRAP scoring position
-enum ExtendGoal : ubyte {
-    RETRACT = 0,
+// Represents goal for the note movement through the robot 
+// to various scoring positions
+// NONE represents no goal for the note
+// AMP represents the goal to move the note and the extend to the AMP scoring position
+// TRAP represents the goal to move the note and the extend to the TRAP scoring position
+// CATAPULT represents the goal to load the note in the catapult.
+// It will complete the catapult goal before accepting new goals.
+enum NoteGoal : ubyte {
+    NONE = 0,
     AMP = 1,
     TRAP = 2,
+    CATAPULT = 3,
 }
 
-enum ExtendRollerGoal : ubyte {
-    NONE = 0,
-    SCORING = 1,
-    REVERSING = 2,
-}
 
 table Goal {
-    intake_roller_goal:IntakeRollerGoal (id: 0);
-    intake_pivot_goal:IntakePivotGoal (id: 1);
-    catapult_goal:frc971.control_loops.catapult.CatapultGoal (id: 2);
-    transfer_roller_goal:TransferRollerGoal (id: 3);
-    climber_goal:ClimberGoal (id: 4);
-    shooter_goal:ShooterGoal (id: 5);
-    extend_goal:ExtendGoal (id: 6);
-    extend_roller_goal:ExtendRollerGoal (id: 7);
+    intake_goal:IntakeGoal = NONE (id: 0);
+    catapult_goal:frc971.control_loops.catapult.CatapultGoal (id: 1);
+    climber_goal:ClimberGoal (id: 2);
+    shooter_goal:ShooterGoal (id: 3);
+    note_goal:NoteGoal (id: 4);
+    fire: bool (id: 5);
 }
 root_type Goal;
diff --git a/y2024/control_loops/superstructure/superstructure_lib_test.cc b/y2024/control_loops/superstructure/superstructure_lib_test.cc
index 701690b..d1f6641 100644
--- a/y2024/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2024/control_loops/superstructure/superstructure_lib_test.cc
@@ -16,6 +16,7 @@
 #include "y2024/control_loops/superstructure/altitude/altitude_plant.h"
 #include "y2024/control_loops/superstructure/catapult/catapult_plant.h"
 #include "y2024/control_loops/superstructure/climber/climber_plant.h"
+#include "y2024/control_loops/superstructure/extend/extend_plant.h"
 #include "y2024/control_loops/superstructure/intake_pivot/intake_pivot_plant.h"
 #include "y2024/control_loops/superstructure/superstructure.h"
 #include "y2024/control_loops/superstructure/turret/turret_plant.h"
@@ -63,7 +64,7 @@
             event_loop_->MakeFetcher<Status>("/superstructure")),
         superstructure_output_fetcher_(
             event_loop_->MakeFetcher<Output>("/superstructure")),
-        transfer_beambreak_(false),
+        extend_beambreak_(false),
         catapult_beambreak_(false),
         intake_pivot_(
             new CappedTestPlant(intake_pivot::MakeIntakePivotPlant()),
@@ -158,6 +159,26 @@
                 ->turret_constants()
                 ->zeroing_constants()
                 ->measured_absolute_position(),
+            dt_),
+        extend_(
+            new CappedTestPlant(extend::MakeExtendPlant()),
+            PositionSensorSimulator(simulated_robot_constants->robot()
+                                        ->extend_constants()
+                                        ->zeroing_constants()
+                                        ->one_revolution_distance()),
+            {.subsystem_params = {simulated_robot_constants->common()->extend(),
+                                  simulated_robot_constants->robot()
+                                      ->extend_constants()
+                                      ->zeroing_constants()},
+             .potentiometer_offset = simulated_robot_constants->robot()
+                                         ->extend_constants()
+                                         ->potentiometer_offset()},
+            frc971::constants::Range::FromFlatbuffer(
+                simulated_robot_constants->common()->extend()->range()),
+            simulated_robot_constants->robot()
+                ->extend_constants()
+                ->zeroing_constants()
+                ->measured_absolute_position(),
             dt_) {
     intake_pivot_.InitializePosition(
         frc971::constants::Range::FromFlatbuffer(
@@ -179,6 +200,11 @@
         frc971::constants::Range::FromFlatbuffer(
             simulated_robot_constants->common()->turret()->range())
             .middle());
+    extend_.InitializePosition(
+        frc971::constants::Range::FromFlatbuffer(
+            simulated_robot_constants->common()->extend()->range())
+            .middle());
+
     phased_loop_handle_ = event_loop_->AddPhasedLoop(
         [this](int) {
           // Skip this the first time.
@@ -203,6 +229,9 @@
             turret_.Simulate(
                 superstructure_output_fetcher_->turret_voltage(),
                 superstructure_status_fetcher_->shooter()->turret());
+
+            extend_.Simulate(superstructure_output_fetcher_->extend_voltage(),
+                             superstructure_status_fetcher_->extend());
           }
           first_ = false;
           SendPositionMessage();
@@ -241,23 +270,27 @@
     flatbuffers::Offset<frc971::PotAndAbsolutePosition> turret_offset =
         turret_.encoder()->GetSensorValues(&turret_builder);
 
+    frc971::PotAndAbsolutePosition::Builder extend_builder =
+        builder.MakeBuilder<frc971::PotAndAbsolutePosition>();
+    flatbuffers::Offset<frc971::PotAndAbsolutePosition> extend_offset =
+        extend_.encoder()->GetSensorValues(&extend_builder);
+
     Position::Builder position_builder = builder.MakeBuilder<Position>();
 
-    position_builder.add_transfer_beambreak(transfer_beambreak_);
+    position_builder.add_extend_beambreak(extend_beambreak_);
     position_builder.add_catapult_beambreak(catapult_beambreak_);
     position_builder.add_intake_pivot(intake_pivot_offset);
     position_builder.add_catapult(catapult_offset);
     position_builder.add_altitude(altitude_offset);
     position_builder.add_turret(turret_offset);
-
     position_builder.add_climber(climber_offset);
+    position_builder.add_extend(extend_offset);
+
     CHECK_EQ(builder.Send(position_builder.Finish()),
              aos::RawSender::Error::kOk);
   }
 
-  void set_transfer_beambreak(bool triggered) {
-    transfer_beambreak_ = triggered;
-  }
+  void set_extend_beambreak(bool triggered) { extend_beambreak_ = triggered; }
 
   void set_catapult_beambreak(bool triggered) {
     catapult_beambreak_ = triggered;
@@ -269,6 +302,8 @@
   PotAndAbsoluteEncoderSimulator *turret() { return &turret_; }
   PotAndAbsoluteEncoderSimulator *climber() { return &climber_; }
 
+  PotAndAbsoluteEncoderSimulator *extend() { return &extend_; }
+
  private:
   ::aos::EventLoop *event_loop_;
   const chrono::nanoseconds dt_;
@@ -279,7 +314,7 @@
   ::aos::Fetcher<Status> superstructure_status_fetcher_;
   ::aos::Fetcher<Output> superstructure_output_fetcher_;
 
-  bool transfer_beambreak_;
+  bool extend_beambreak_;
   bool catapult_beambreak_;
 
   AbsoluteEncoderSimulator intake_pivot_;
@@ -287,6 +322,7 @@
   PotAndAbsoluteEncoderSimulator catapult_;
   PotAndAbsoluteEncoderSimulator altitude_;
   PotAndAbsoluteEncoderSimulator turret_;
+  PotAndAbsoluteEncoderSimulator extend_;
 
   bool first_ = true;
 };
@@ -297,13 +333,12 @@
       : ::frc971::testing::ControlLoopTest(
             aos::configuration::ReadConfig("y2024/aos_config.json"),
             std::chrono::microseconds(5050)),
-        values_(std::make_shared<constants::Values>(constants::MakeValues())),
         simulated_constants_dummy_(SendSimulationConstants(
             event_loop_factory(), 7971, "y2024/constants/test_constants.json")),
         roborio_(aos::configuration::GetNode(configuration(), "roborio")),
         logger_pi_(aos::configuration::GetNode(configuration(), "logger")),
         superstructure_event_loop(MakeEventLoop("Superstructure", roborio_)),
-        superstructure_(superstructure_event_loop.get(), (values_)),
+        superstructure_(superstructure_event_loop.get()),
         test_event_loop_(MakeEventLoop("test", roborio_)),
         constants_fetcher_(test_event_loop_.get()),
         simulated_robot_constants_(
@@ -354,11 +389,10 @@
 
     EXPECT_FALSE(superstructure_status_fetcher_->collided());
 
-    double set_point = superstructure_status_fetcher_->intake_pivot()
-                           ->unprofiled_goal_position();
+    double set_point =
+        superstructure_status_fetcher_->intake_pivot()->goal_position();
 
-    if (superstructure_goal_fetcher_->intake_pivot_goal() ==
-        IntakePivotGoal::EXTENDED) {
+    if (superstructure_goal_fetcher_->intake_goal() == IntakeGoal::INTAKE) {
       set_point = simulated_robot_constants_->common()
                       ->intake_pivot_set_points()
                       ->extended();
@@ -366,9 +400,11 @@
 
     EXPECT_NEAR(set_point,
                 superstructure_status_fetcher_->intake_pivot()->position(),
-                0.001);
+                0.01);
 
-    if (superstructure_goal_fetcher_->has_shooter_goal()) {
+    if (superstructure_goal_fetcher_->has_shooter_goal() &&
+        superstructure_goal_fetcher_->note_goal() != NoteGoal::AMP &&
+        superstructure_goal_fetcher_->note_goal() != NoteGoal::TRAP) {
       if (superstructure_goal_fetcher_->shooter_goal()->has_turret_position() &&
           !superstructure_goal_fetcher_->shooter_goal()->auto_aim()) {
         EXPECT_NEAR(
@@ -378,6 +414,13 @@
             superstructure_status_fetcher_->shooter()->turret()->position(),
             0.001);
       }
+    } else if (superstructure_goal_fetcher_->note_goal() == NoteGoal::AMP ||
+               superstructure_goal_fetcher_->note_goal() == NoteGoal::TRAP) {
+      EXPECT_NEAR(
+          simulated_robot_constants_->common()
+              ->turret_avoid_extend_collision_position(),
+          superstructure_status_fetcher_->shooter()->turret()->position(),
+          0.001);
     }
 
     if (superstructure_goal_fetcher_->has_shooter_goal()) {
@@ -421,6 +464,27 @@
       EXPECT_NEAR(set_point,
                   superstructure_status_fetcher_->climber()->position(), 0.001);
     }
+
+    if (superstructure_goal_fetcher_->has_note_goal()) {
+      double set_point = simulated_robot_constants_->common()
+                             ->extend_set_points()
+                             ->retracted();
+      if (superstructure_goal_fetcher_->note_goal() == NoteGoal::TRAP) {
+        set_point =
+            simulated_robot_constants_->common()->extend_set_points()->trap();
+      } else if (superstructure_goal_fetcher_->note_goal() == NoteGoal::AMP) {
+        set_point =
+            simulated_robot_constants_->common()->extend_set_points()->amp();
+      } else if (superstructure_goal_fetcher_->note_goal() ==
+                 NoteGoal::CATAPULT) {
+        set_point = simulated_robot_constants_->common()
+                        ->extend_set_points()
+                        ->catapult();
+      }
+
+      EXPECT_NEAR(set_point,
+                  superstructure_status_fetcher_->extend()->position(), 0.001);
+    }
   }
 
   void CheckIfZeroed() {
@@ -481,7 +545,6 @@
     builder.CheckOk(builder.Send(drivetrain_status_builder.Finish()));
   }
 
-  std::shared_ptr<const constants::Values> values_;
   const bool simulated_constants_dummy_;
 
   const aos::Node *const roborio_;
@@ -559,7 +622,8 @@
     Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
     goal_builder.add_climber_goal(ClimberGoal::RETRACT);
     goal_builder.add_shooter_goal(shooter_goal_offset);
-    goal_builder.add_intake_pivot_goal(IntakePivotGoal::RETRACTED);
+    goal_builder.add_intake_goal(IntakeGoal::NONE);
+    goal_builder.add_note_goal(NoteGoal::NONE);
 
     ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
   }
@@ -591,6 +655,10 @@
           simulated_robot_constants_->common()->climber()->range())
           .lower);
 
+  superstructure_plant_.extend()->InitializePosition(
+      frc971::constants::Range::FromFlatbuffer(
+          simulated_robot_constants_->common()->extend()->range())
+          .lower);
   WaitUntilZeroed();
 
   {
@@ -621,15 +689,19 @@
         shooter_goal_builder.Finish();
 
     Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
-    goal_builder.add_intake_pivot_goal(IntakePivotGoal::EXTENDED);
+    goal_builder.add_intake_goal(IntakeGoal::INTAKE);
     goal_builder.add_climber_goal(ClimberGoal::FULL_EXTEND);
     goal_builder.add_shooter_goal(shooter_goal_offset);
+    goal_builder.add_note_goal(NoteGoal::NONE);
 
     ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
   }
 
+  superstructure_plant_.set_extend_beambreak(true);
+
   // Give it a lot of time to get there.
   RunFor(chrono::seconds(15));
+
   VerifyNearGoal();
 }
 
@@ -669,12 +741,15 @@
         shooter_goal_builder.Finish();
 
     Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
-    goal_builder.add_intake_pivot_goal(IntakePivotGoal::EXTENDED);
+    goal_builder.add_intake_goal(IntakeGoal::INTAKE);
     goal_builder.add_climber_goal(ClimberGoal::FULL_EXTEND);
     goal_builder.add_shooter_goal(shooter_goal_offset);
+    goal_builder.add_note_goal(NoteGoal::AMP);
 
     ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
   }
+  superstructure_plant_.set_extend_beambreak(true);
+
   RunFor(chrono::seconds(20));
   VerifyNearGoal();
 
@@ -710,13 +785,16 @@
         shooter_goal_builder.Finish();
 
     Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
-    goal_builder.add_intake_pivot_goal(IntakePivotGoal::RETRACTED);
+    goal_builder.add_intake_goal(IntakeGoal::NONE);
     goal_builder.add_climber_goal(ClimberGoal::RETRACT);
     goal_builder.add_shooter_goal(shooter_goal_offset);
+    goal_builder.add_note_goal(NoteGoal::NONE);
 
     ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
   }
 
+  superstructure_plant_.set_extend_beambreak(false);
+
   RunFor(chrono::seconds(10));
   VerifyNearGoal();
 }
@@ -738,6 +816,8 @@
 
   EXPECT_EQ(PotAndAbsoluteEncoderSubsystem::State::RUNNING,
             superstructure_.shooter().altitude().state());
+  EXPECT_EQ(PotAndAbsoluteEncoderSubsystem::State::RUNNING,
+            superstructure_.extend().state());
 }
 
 // Tests that running disabled works
@@ -822,14 +902,12 @@
 
     Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
 
-    goal_builder.add_intake_pivot_goal(IntakePivotGoal::RETRACTED);
-    goal_builder.add_intake_roller_goal(IntakeRollerGoal::NONE);
+    goal_builder.add_intake_goal(IntakeGoal::NONE);
+    goal_builder.add_note_goal(NoteGoal::NONE);
 
     ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
   }
 
-  superstructure_plant_.set_transfer_beambreak(false);
-
   RunFor(chrono::seconds(5));
 
   VerifyNearGoal();
@@ -841,14 +919,12 @@
 
     Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
 
-    goal_builder.add_intake_pivot_goal(IntakePivotGoal::EXTENDED);
-    goal_builder.add_intake_roller_goal(IntakeRollerGoal::SPIT);
+    goal_builder.add_intake_goal(IntakeGoal::SPIT);
+    goal_builder.add_note_goal(NoteGoal::NONE);
 
     ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
   }
 
-  superstructure_plant_.set_transfer_beambreak(false);
-
   RunFor(chrono::seconds(5));
 
   VerifyNearGoal();
@@ -863,14 +939,12 @@
 
     Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
 
-    goal_builder.add_intake_pivot_goal(IntakePivotGoal::EXTENDED);
-    goal_builder.add_intake_roller_goal(IntakeRollerGoal::INTAKE);
+    goal_builder.add_intake_goal(IntakeGoal::INTAKE);
+    goal_builder.add_note_goal(NoteGoal::NONE);
 
     ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
   }
 
-  superstructure_plant_.set_transfer_beambreak(false);
-
   RunFor(chrono::seconds(5));
 
   VerifyNearGoal();
@@ -880,13 +954,12 @@
 
     Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
 
-    goal_builder.add_intake_roller_goal(IntakeRollerGoal::INTAKE);
+    goal_builder.add_intake_goal(IntakeGoal::INTAKE);
+    goal_builder.add_note_goal(NoteGoal::NONE);
 
     ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
   }
 
-  superstructure_plant_.set_transfer_beambreak(false);
-
   RunFor(chrono::seconds(5));
 
   VerifyNearGoal();
@@ -896,7 +969,17 @@
                 ->transfer_roller_voltages()
                 ->transfer_in());
 
-  superstructure_plant_.set_transfer_beambreak(true);
+  {
+    auto builder = superstructure_goal_sender_.MakeBuilder();
+
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+    goal_builder.add_intake_goal(IntakeGoal::INTAKE);
+
+    ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+  }
+
+  superstructure_plant_.set_extend_beambreak(true);
 
   RunFor(chrono::seconds(2));
 
@@ -926,21 +1009,20 @@
         builder.MakeBuilder<ShooterGoal>();
 
     shooter_goal_builder.add_auto_aim(true);
-    shooter_goal_builder.add_fire(false);
 
     flatbuffers::Offset<ShooterGoal> shooter_goal_offset =
         shooter_goal_builder.Finish();
 
     Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
 
-    goal_builder.add_intake_pivot_goal(IntakePivotGoal::RETRACTED);
+    goal_builder.add_intake_goal(IntakeGoal::NONE);
     goal_builder.add_shooter_goal(shooter_goal_offset);
+    goal_builder.add_fire(false);
 
     ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
   }
 
   superstructure_plant_.set_catapult_beambreak(false);
-  superstructure_plant_.set_transfer_beambreak(false);
 
   RunFor(chrono::seconds(5));
 
@@ -964,22 +1046,19 @@
         builder.MakeBuilder<ShooterGoal>();
 
     shooter_goal_builder.add_auto_aim(true);
-    shooter_goal_builder.add_fire(false);
 
     flatbuffers::Offset<ShooterGoal> shooter_goal_offset =
         shooter_goal_builder.Finish();
 
     Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
 
-    goal_builder.add_intake_pivot_goal(IntakePivotGoal::EXTENDED);
-    goal_builder.add_intake_roller_goal(IntakeRollerGoal::INTAKE);
+    goal_builder.add_intake_goal(IntakeGoal::INTAKE);
     goal_builder.add_shooter_goal(shooter_goal_offset);
+    goal_builder.add_fire(false);
 
     ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
   }
 
-  superstructure_plant_.set_transfer_beambreak(false);
-
   RunFor(chrono::seconds(5));
 
   VerifyNearGoal();
@@ -994,32 +1073,114 @@
   EXPECT_EQ(superstructure_status_fetcher_->shooter()->catapult_state(),
             CatapultState::READY);
 
-  // Signal through the transfer roller that we got it.
-  superstructure_plant_.set_transfer_beambreak(true);
+  {
+    auto builder = superstructure_goal_sender_.MakeBuilder();
 
-  RunFor(chrono::seconds(5));
+    ShooterGoal::Builder shooter_goal_builder =
+        builder.MakeBuilder<ShooterGoal>();
+
+    shooter_goal_builder.add_auto_aim(true);
+
+    flatbuffers::Offset<ShooterGoal> shooter_goal_offset =
+        shooter_goal_builder.Finish();
+
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+    goal_builder.add_intake_goal(IntakeGoal::INTAKE);
+    goal_builder.add_shooter_goal(shooter_goal_offset);
+    goal_builder.add_note_goal(NoteGoal::NONE);
+    goal_builder.add_fire(false);
+
+    ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+  }
+
+  superstructure_plant_.set_extend_beambreak(false);
+
+  RunFor(10 * dt());
 
   VerifyNearGoal();
 
+  EXPECT_EQ(superstructure_status_fetcher_->shooter()->catapult_state(),
+            CatapultState::READY);
+
+  EXPECT_EQ(superstructure_status_fetcher_->extend_status(),
+            ExtendStatus::RETRACTED);
+  EXPECT_EQ(superstructure_status_fetcher_->extend_roller(),
+            ExtendRollerStatus::TRANSFERING_TO_EXTEND);
+
+  EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(), 12.0);
+  EXPECT_EQ(superstructure_output_fetcher_->extend_roller_voltage(), 12.0);
+
+  superstructure_plant_.set_extend_beambreak(true);
+
+  RunFor(chrono::milliseconds(750));
+
+  VerifyNearGoal();
+
+  EXPECT_EQ(superstructure_status_fetcher_->shooter()->catapult_state(),
+            CatapultState::READY);
+
+  EXPECT_EQ(superstructure_status_fetcher_->extend_status(),
+            ExtendStatus::RETRACTED);
+  EXPECT_EQ(superstructure_status_fetcher_->extend_roller(),
+            ExtendRollerStatus::IDLE);
+
   EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(), 0.0);
+  EXPECT_EQ(superstructure_output_fetcher_->extend_roller_voltage(), 0.0);
 
   // Verify we are in the loading position.
   EXPECT_NEAR(superstructure_status_fetcher_->shooter()->turret()->position(),
               simulated_robot_constants_->common()->turret_loading_position(),
               0.01);
+  {
+    auto builder = superstructure_goal_sender_.MakeBuilder();
 
-  EXPECT_NEAR(superstructure_status_fetcher_->shooter()->altitude()->position(),
-              0.0, 0.01);
+    ShooterGoal::Builder shooter_goal_builder =
+        builder.MakeBuilder<ShooterGoal>();
+
+    shooter_goal_builder.add_auto_aim(true);
+
+    flatbuffers::Offset<ShooterGoal> shooter_goal_offset =
+        shooter_goal_builder.Finish();
+
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+    goal_builder.add_shooter_goal(shooter_goal_offset);
+    goal_builder.add_note_goal(NoteGoal::CATAPULT);
+    goal_builder.add_fire(false);
+
+    ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+  }
+
+  superstructure_plant_.set_extend_beambreak(true);
+
+  RunFor(chrono::milliseconds(500));
+
+  VerifyNearGoal();
 
   EXPECT_EQ(superstructure_status_fetcher_->shooter()->catapult_state(),
             CatapultState::READY);
 
   superstructure_plant_.set_catapult_beambreak(true);
-  superstructure_plant_.set_transfer_beambreak(true);
+  // Set retention roller to show that we are loaded.
+  EXPECT_EQ(superstructure_status_fetcher_->extend_status(),
+            ExtendStatus::CATAPULT);
+  EXPECT_EQ(superstructure_status_fetcher_->extend_roller(),
+            ExtendRollerStatus::TRANSFERING_TO_CATAPULT);
 
-  RunFor(chrono::seconds(5));
+  superstructure_plant_.set_extend_beambreak(false);
 
-  VerifyNearGoal();
+  RunFor(chrono::seconds(10));
+
+  ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+
+  EXPECT_EQ(superstructure_status_fetcher_->shooter()->catapult_state(),
+            CatapultState::LOADED);
+
+  EXPECT_EQ(superstructure_status_fetcher_->extend_status(),
+            ExtendStatus::CATAPULT);
+  EXPECT_EQ(superstructure_status_fetcher_->extend_roller(),
+            ExtendRollerStatus::IDLE);
 
   // Should now be loaded.
   EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(), 0.0);
@@ -1053,7 +1214,6 @@
         builder.MakeBuilder<ShooterGoal>();
 
     shooter_goal_builder.add_auto_aim(false);
-    shooter_goal_builder.add_fire(true);
     shooter_goal_builder.add_catapult_goal(catapult_offset);
     shooter_goal_builder.add_altitude_position(altitude_offset);
     shooter_goal_builder.add_turret_position(turret_offset);
@@ -1063,14 +1223,13 @@
 
     Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
 
-    goal_builder.add_intake_pivot_goal(IntakePivotGoal::RETRACTED);
+    goal_builder.add_intake_goal(IntakeGoal::NONE);
     goal_builder.add_shooter_goal(shooter_goal_offset);
+    goal_builder.add_fire(true);
 
     ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
   }
 
-  superstructure_plant_.set_transfer_beambreak(false);
-
   // Wait until the bot finishes auto-aiming.
   WaitUntilNear(kTurretGoal, kAltitudeGoal);
 
@@ -1105,7 +1264,6 @@
         builder.MakeBuilder<ShooterGoal>();
 
     shooter_goal_builder.add_auto_aim(false);
-    shooter_goal_builder.add_fire(false);
 
     flatbuffers::Offset<ShooterGoal> shooter_goal_offset =
         shooter_goal_builder.Finish();
@@ -1113,17 +1271,19 @@
     Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
 
     goal_builder.add_shooter_goal(shooter_goal_offset);
+    goal_builder.add_fire(false);
 
     ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
   }
 
   superstructure_plant_.set_catapult_beambreak(false);
-  superstructure_plant_.set_transfer_beambreak(false);
 
   RunFor(chrono::seconds(5));
 
   VerifyNearGoal();
 
+  EXPECT_EQ(superstructure_status_fetcher_->state(), SuperstructureState::IDLE);
+
   EXPECT_EQ(superstructure_status_fetcher_->shooter()->catapult_state(),
             CatapultState::READY);
 
@@ -1290,4 +1450,139 @@
       superstructure_status_fetcher_->shooter()->aimer()->target_distance());
 }
 
-}  // namespace y2024::control_loops::superstructure::testing
+// Test entire sequence of loading, transfering, and scoring at amp position.
+TEST_F(SuperstructureTest, ScoreInAmp) {
+  SetEnabled(true);
+
+  WaitUntilZeroed();
+
+  {
+    auto builder = superstructure_goal_sender_.MakeBuilder();
+
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+    goal_builder.add_intake_goal(IntakeGoal::INTAKE);
+    goal_builder.add_note_goal(NoteGoal::NONE);
+
+    ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+  }
+
+  RunFor(chrono::seconds(5));
+
+  VerifyNearGoal();
+
+  EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(),
+            simulated_robot_constants_->common()
+                ->transfer_roller_voltages()
+                ->transfer_in());
+
+  EXPECT_EQ(superstructure_status_fetcher_->extend_roller(),
+            ExtendRollerStatus::TRANSFERING_TO_EXTEND);
+
+  EXPECT_EQ(superstructure_status_fetcher_->state(),
+            SuperstructureState::INTAKING);
+
+  {
+    auto builder = superstructure_goal_sender_.MakeBuilder();
+
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+    goal_builder.add_intake_goal(IntakeGoal::INTAKE);
+    goal_builder.add_note_goal(NoteGoal::NONE);
+
+    ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+  }
+
+  superstructure_plant_.set_extend_beambreak(true);
+
+  RunFor(chrono::milliseconds(10));
+
+  VerifyNearGoal();
+
+  EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(), 0.0);
+
+  EXPECT_EQ(superstructure_status_fetcher_->extend_roller(),
+            ExtendRollerStatus::IDLE);
+
+  EXPECT_EQ(superstructure_status_fetcher_->state(),
+            SuperstructureState::LOADED);
+
+  {
+    auto builder = superstructure_goal_sender_.MakeBuilder();
+
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+    goal_builder.add_intake_goal(IntakeGoal::NONE);
+    goal_builder.add_note_goal(NoteGoal::AMP);
+
+    ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+  }
+
+  RunFor(100 * dt());
+
+  ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+
+  EXPECT_EQ(superstructure_status_fetcher_->extend_status(),
+            ExtendStatus::MOVING);
+
+  RunFor(chrono::seconds(5));
+
+  ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+
+  EXPECT_NEAR(superstructure_status_fetcher_->extend()->position(),
+              simulated_robot_constants_->common()->extend_set_points()->amp(),
+              0.01);
+  EXPECT_EQ(superstructure_status_fetcher_->extend_status(), ExtendStatus::AMP);
+
+  EXPECT_EQ(superstructure_status_fetcher_->state(),
+            SuperstructureState::READY);
+
+  {
+    auto builder = superstructure_goal_sender_.MakeBuilder();
+
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+    goal_builder.add_intake_goal(IntakeGoal::NONE);
+    goal_builder.add_note_goal(NoteGoal::AMP);
+    goal_builder.add_fire(true);
+
+    ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+  }
+
+  superstructure_plant_.set_extend_beambreak(true);
+
+  RunFor(chrono::milliseconds(10));
+
+  ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+  ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
+
+  EXPECT_EQ(superstructure_status_fetcher_->extend_roller(),
+            ExtendRollerStatus::SCORING_IN_AMP);
+  EXPECT_EQ(superstructure_output_fetcher_->extend_roller_voltage(), 12.0);
+
+  {
+    auto builder = superstructure_goal_sender_.MakeBuilder();
+
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+    goal_builder.add_intake_goal(IntakeGoal::NONE);
+    goal_builder.add_note_goal(NoteGoal::AMP);
+    goal_builder.add_fire(false);
+
+    ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+  }
+
+  superstructure_plant_.set_extend_beambreak(false);
+
+  RunFor(chrono::milliseconds(100));
+
+  ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+  ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
+
+  EXPECT_EQ(superstructure_status_fetcher_->state(), SuperstructureState::IDLE);
+
+  EXPECT_EQ(superstructure_status_fetcher_->extend_roller(),
+            ExtendRollerStatus::IDLE);
+  EXPECT_EQ(superstructure_output_fetcher_->extend_roller_voltage(), 0.0);
+}
+}  // namespace y2024::control_loops::superstructure::testing
\ No newline at end of file
diff --git a/y2024/control_loops/superstructure/superstructure_main.cc b/y2024/control_loops/superstructure/superstructure_main.cc
index 58932c7..a5ab8ed 100644
--- a/y2024/control_loops/superstructure/superstructure_main.cc
+++ b/y2024/control_loops/superstructure/superstructure_main.cc
@@ -17,10 +17,7 @@
 
   frc971::constants::WaitForConstants<y2024::Constants>(&config.message());
 
-  std::shared_ptr<const y2024::constants::Values> values =
-      std::make_shared<const y2024::constants::Values>(
-          y2024::constants::MakeValues());
-  Superstructure superstructure(&event_loop, values);
+  Superstructure superstructure(&event_loop);
 
   event_loop.Run();
 
diff --git a/y2024/control_loops/superstructure/superstructure_replay.cc b/y2024/control_loops/superstructure/superstructure_replay.cc
index bd7f9fd..ab88a46 100644
--- a/y2024/control_loops/superstructure/superstructure_replay.cc
+++ b/y2024/control_loops/superstructure/superstructure_replay.cc
@@ -48,8 +48,7 @@
 
   roborio->OnStartup([roborio]() {
     roborio->AlwaysStart<y2024::control_loops::superstructure::Superstructure>(
-        "superstructure", std::make_shared<y2024::constants::Values>(
-                              y2024::constants::MakeValues()));
+        "superstructure");
   });
 
   std::unique_ptr<aos::EventLoop> print_loop = roborio->MakeEventLoop("print");
diff --git a/y2024/control_loops/superstructure/superstructure_status.fbs b/y2024/control_loops/superstructure/superstructure_status.fbs
index 872fb21..7541b7e 100644
--- a/y2024/control_loops/superstructure/superstructure_status.fbs
+++ b/y2024/control_loops/superstructure/superstructure_status.fbs
@@ -3,6 +3,27 @@
 
 namespace y2024.control_loops.superstructure;
 
+enum SuperstructureState : ubyte {
+  // Before a note has been intaked, the extend should be retracted.
+  IDLE = 0,
+  // Intaking a note and transferring it to the extned through the 
+  // intake, transfer, and extend rollers.
+  INTAKING = 1,
+  // The note is in the extend and the extend is not moving.
+  LOADED = 2,
+  // The note is in the extend and the extend is moving towards a goal, 
+  // either the catapult, amp, or trap.
+  MOVING = 3,
+  // For Catapult Path, the note is being transferred between the extend and the catapult.
+  LOADING_CATAPULT = 4,
+  // The note is either:
+  // 1. Loaded in the catapult and ready to fire
+  // 2. In the extend and the extend is at the amp or trap scoring position.
+  READY = 5,
+  // Fire goal recieved and the note is being fired from the catapult or being scored in the amp or trap.
+  FIRING = 6,
+}
+
 // Contains if intake is intaking
 enum IntakeRollerStatus : ubyte {
   NONE = 0,
@@ -57,11 +78,30 @@
 }
 
 // Contains status of extend rollers
-// HAS_GAME_PIECE means we have a game piece
-// EMPTY means we don't have a game piece
 enum ExtendRollerStatus: ubyte {
-  HAS_GAME_PIECE = 0,
-  EMPTY = 1
+  // Means the rollers are not moving.
+  IDLE = 0,
+  // Means we're transfer from the transfer rollers into the extend rollers.
+  TRANSFERING_TO_EXTEND = 1,
+  // Means we are transfering from the extend to the catapult.
+  TRANSFERING_TO_CATAPULT = 2,
+  // Means we're trying to score in the amp/trap
+  SCORING_IN_AMP = 3,
+  SCORING_IN_TRAP = 4,
+}
+
+// Contains the status of the extend subsystem
+enum ExtendStatus : ubyte  {
+  // Means we are near 0 and ready to transfer a game piece.
+  RETRACTED = 0,
+  // Means we are moving to some goal.
+  MOVING = 1,
+  // Means we are currently at the catapult.
+  CATAPULT = 2,
+  // Means we are at the amp position.
+  AMP = 3,
+  // Means we are at the trap positon.
+  TRAP = 4,
 }
 
 table Status {
@@ -71,29 +111,33 @@
   // If true, we have aborted. This is the or of all subsystem estops.
   estopped:bool (id: 1);
 
+  state : SuperstructureState (id: 2);
+
   // Status of the rollers
-  intake_roller:IntakeRollerStatus (id: 2);
+  intake_roller:IntakeRollerStatus (id: 3);
 
   // Estimated angle and angular velocitiy of the intake.
-  intake_pivot:frc971.control_loops.AbsoluteEncoderProfiledJointStatus (id: 3);
+  intake_pivot:frc971.control_loops.AbsoluteEncoderProfiledJointStatus (id: 4);
 
   // Status of transfer rollers
-  transfer_roller:TransferRollerStatus (id: 4);
+  transfer_roller:TransferRollerStatus (id: 5);
 
   // Estimated angle and angular velocitiy of the climber.
-  climber:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 5);
+  climber:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 6);
 
   // Status of the subsytems involved in the shooter
-  shooter:ShooterStatus (id: 6);
+  shooter:ShooterStatus (id: 7);
 
   // Estimated angle and angular velocitiy of the extend.
-  extend:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 7);
+  extend:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 8);
 
   // State of the extender rollers
-  extend_roller:ExtendRollerStatus (id: 8);
+  extend_roller:ExtendRollerStatus (id: 9);
 
   // The status of if the turret and intake is colliding
-  collided: bool (id: 9);
+  collided: bool (id: 10);
+
+  extend_status:ExtendStatus (id: 11);
 }
 
 root_type Status;