diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json
new file mode 100644
index 00000000..f0f03625
--- /dev/null
+++ b/.pathplanner/settings.json
@@ -0,0 +1,12 @@
+{
+  "robotWidth": 0.9,
+  "robotLength": 0.9,
+  "holonomicMode": true,
+  "pathFolders": [],
+  "autoFolders": [],
+  "defaultMaxVel": 3.0,
+  "defaultMaxAccel": 3.0,
+  "defaultMaxAngVel": 270.0,
+  "defaultMaxAngAccel": 600.0,
+  "maxModuleSpeed": 3.0
+}
\ No newline at end of file
diff --git a/simgui.json b/simgui.json
index a5719869..d078f7a2 100644
--- a/simgui.json
+++ b/simgui.json
@@ -23,11 +23,17 @@
       "open": false
     },
     "transitory": {
+      "AdvantageKit": {
+        "open": true
+      },
       "SmartDashboard": {
         "TunableNumbers": {
           "Flywheel": {
             "open": true
           },
+          "Pathfollow": {
+            "open": true
+          },
           "open": true
         },
         "open": true
diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json
new file mode 100644
index 00000000..bab0da93
--- /dev/null
+++ b/src/main/deploy/pathplanner/navgrid.json
@@ -0,0 +1 @@
+{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path
new file mode 100644
index 00000000..4da23a2a
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Example Path.path	
@@ -0,0 +1,81 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 7.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.0,
+        "y": 6.5
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.061558522422601,
+        "y": 6.1060047436211775
+      },
+      "prevControl": {
+        "x": 5.061558522422601,
+        "y": 7.1060047436211775
+      },
+      "nextControl": {
+        "x": 7.061558522422601,
+        "y": 5.1060047436211775
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.9885342675054565,
+        "y": 3.354442578025781
+      },
+      "prevControl": {
+        "x": 7.7385342675054565,
+        "y": 4.8544425780257825
+      },
+      "nextControl": {
+        "x": 8.238534267505454,
+        "y": 1.8544425780257794
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.3963635116216997,
+        "y": 1.641386992337064
+      },
+      "prevControl": {
+        "x": 5.380848911075716,
+        "y": 2.0910493199386275
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 270.0,
+    "maxAngularAcceleration": 600.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 173.99592794454193,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": null,
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
index 9f8de0aa..b7d5cb53 100644
--- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
+++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
@@ -151,7 +151,6 @@ object RobotContainer {
   fun mapTeleopControls() {
 
     ControlBoard.resetGyro.whileTrue(ResetGyroYawCommand(drivetrain, toAngle = 180.degrees))
-
     ControlBoard.runGroundIntake.whileTrue(superstructure.groundIntakeCommand())
     ControlBoard.ejectGamePiece.whileTrue(superstructure.ejectGamePieceCommand())
     ControlBoard.prepAmpScore.whileTrue(superstructure.prepAmpCommand())
diff --git a/src/main/kotlin/com/team4099/robot2023/auto/PathStore.kt b/src/main/kotlin/com/team4099/robot2023/auto/PathStore.kt
index 0ae2c675..44e951f4 100644
--- a/src/main/kotlin/com/team4099/robot2023/auto/PathStore.kt
+++ b/src/main/kotlin/com/team4099/robot2023/auto/PathStore.kt
@@ -1,3 +1,7 @@
 package com.team4099.robot2023.auto
 
-object PathStore
+import com.pathplanner.lib.path.PathPlannerPath
+
+object PathStore {
+  val examplePath: PathPlannerPath = PathPlannerPath.fromPathFile("Example Path")
+}
diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/ExamplePathAuto.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/ExamplePathAuto.kt
new file mode 100644
index 00000000..d1e4b419
--- /dev/null
+++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/ExamplePathAuto.kt
@@ -0,0 +1,19 @@
+package com.team4099.robot2023.auto.mode
+
+import com.team4099.robot2023.auto.PathStore
+import com.team4099.robot2023.commands.drivetrain.FollowPathPlannerPathCommand
+import com.team4099.robot2023.commands.drivetrain.ResetPoseCommand
+import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup
+import org.team4099.lib.geometry.Pose2d
+
+class ExamplePathAuto(val drivetrain: Drivetrain) : SequentialCommandGroup() {
+  init {
+    addRequirements(drivetrain)
+
+    addCommands(
+      ResetPoseCommand(drivetrain, Pose2d(PathStore.examplePath.previewStartingHolonomicPose)),
+      FollowPathPlannerPathCommand(drivetrain, PathStore.examplePath)
+    )
+  }
+}
diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/FollowPathPlannerPathCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/FollowPathPlannerPathCommand.kt
new file mode 100644
index 00000000..e9c14ed4
--- /dev/null
+++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/FollowPathPlannerPathCommand.kt
@@ -0,0 +1,241 @@
+package com.team4099.robot2023.commands.drivetrain
+
+import com.pathplanner.lib.path.PathPlannerPath
+import com.team4099.lib.logging.LoggedTunableValue
+import com.team4099.robot2023.config.constants.DrivetrainConstants
+import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
+import com.team4099.robot2023.subsystems.superstructure.Request
+import edu.wpi.first.math.kinematics.ChassisSpeeds
+import edu.wpi.first.wpilibj2.command.Command
+import org.littletonrobotics.junction.Logger
+import org.team4099.lib.geometry.Pose2d
+import org.team4099.lib.geometry.Translation2d
+import org.team4099.lib.hal.Clock
+import org.team4099.lib.pplib.PathPlannerHolonomicDriveController
+import org.team4099.lib.pplib.PathPlannerRotationPID
+import org.team4099.lib.pplib.PathPlannerTranslationPID
+import org.team4099.lib.units.base.inMeters
+import org.team4099.lib.units.base.inSeconds
+import org.team4099.lib.units.base.inches
+import org.team4099.lib.units.base.meters
+import org.team4099.lib.units.base.seconds
+import org.team4099.lib.units.derived.degrees
+import org.team4099.lib.units.derived.inDegrees
+import org.team4099.lib.units.derived.inDegreesPerSecondPerDegree
+import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreePerSecond
+import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreeSeconds
+import org.team4099.lib.units.derived.inMetersPerSecondPerMeter
+import org.team4099.lib.units.derived.inMetersPerSecondPerMeterSeconds
+import org.team4099.lib.units.derived.inMetersPerSecondPerMetersPerSecond
+import org.team4099.lib.units.derived.inRadians
+import org.team4099.lib.units.derived.inRotation2ds
+import org.team4099.lib.units.derived.metersPerSecondPerMetersPerSecond
+import org.team4099.lib.units.derived.perDegree
+import org.team4099.lib.units.derived.perDegreePerSecond
+import org.team4099.lib.units.derived.perDegreeSeconds
+import org.team4099.lib.units.derived.perMeter
+import org.team4099.lib.units.derived.perMeterSeconds
+import org.team4099.lib.units.derived.radians
+import org.team4099.lib.units.perSecond
+
+class FollowPathPlannerPathCommand(val drivetrain: Drivetrain, val path: PathPlannerPath) :
+  Command() {
+  private val translationToleranceAtEnd = 1.inches
+  private val thetaToleranceAtEnd = 2.5.degrees
+
+  private var swerveDriveController: PathPlannerHolonomicDriveController
+
+  private var trajCurTime = 0.0.seconds
+  private var trajStartTime = 0.0.seconds
+
+  private var pathFollowRequest = Request.DrivetrainRequest.ClosedLoop(ChassisSpeeds(0.0, 0.0, 0.0))
+  private var lastSampledPose = Pose2d()
+  private val atReference: Boolean
+    get() =
+      (
+        (lastSampledPose.translation - drivetrain.odometryPose.translation).magnitude.meters <=
+          translationToleranceAtEnd &&
+          (lastSampledPose.rotation - drivetrain.odometryPose.rotation).absoluteValue <=
+          thetaToleranceAtEnd
+        )
+
+  val thetakP =
+    LoggedTunableValue(
+      "Pathfollow/thetakP",
+      DrivetrainConstants.PID.AUTO_THETA_PID_KP,
+      Pair({ it.inDegreesPerSecondPerDegree }, { it.degrees.perSecond.perDegree })
+    )
+  val thetakI =
+    LoggedTunableValue(
+      "Pathfollow/thetakI",
+      DrivetrainConstants.PID.AUTO_THETA_PID_KI,
+      Pair(
+        { it.inDegreesPerSecondPerDegreeSeconds }, { it.degrees.perSecond.perDegreeSeconds }
+      )
+    )
+  val thetakD =
+    LoggedTunableValue(
+      "Pathfollow/thetakD",
+      DrivetrainConstants.PID.AUTO_THETA_PID_KD,
+      Pair(
+        { it.inDegreesPerSecondPerDegreePerSecond },
+        { it.degrees.perSecond.perDegreePerSecond }
+      )
+    )
+
+  val thetaMaxVel =
+    LoggedTunableValue("Pathfollow/thetaMaxVel", DrivetrainConstants.PID.MAX_AUTO_ANGULAR_VEL)
+  val thetaMaxAccel =
+    LoggedTunableValue("Pathfollow/thetaMaxAccel", DrivetrainConstants.PID.MAX_AUTO_ANGULAR_ACCEL)
+
+  val poskP =
+    LoggedTunableValue(
+      "Pathfollow/poskP",
+      DrivetrainConstants.PID.AUTO_POS_KP,
+      Pair({ it.inMetersPerSecondPerMeter }, { it.meters.perSecond.perMeter })
+    )
+  val poskI =
+    LoggedTunableValue(
+      "Pathfollow/poskI",
+      DrivetrainConstants.PID.AUTO_POS_KI,
+      Pair({ it.inMetersPerSecondPerMeterSeconds }, { it.meters.perSecond.perMeterSeconds })
+    )
+  val poskD =
+    LoggedTunableValue(
+      "Pathfollow/poskD",
+      DrivetrainConstants.PID.AUTO_POS_KD,
+      Pair(
+        { it.inMetersPerSecondPerMetersPerSecond }, { it.metersPerSecondPerMetersPerSecond }
+      )
+    )
+
+  var translationPID: PathPlannerTranslationPID
+  var rotationPID: PathPlannerRotationPID
+  //    private val pathConstraints = PathPlannerHolonomicDriveController.Companion.PathConstraints(
+  //        maxVelocity = DrivetrainConstants.MAX_AUTO_VEL,
+  //        maxAcceleration = DrivetrainConstants.MAX_AUTO_ACCEL,
+  //        maxAngularVelocity = thetaMaxVel.get(),
+  //        maxAngularAcceleration = thetaMaxAccel.get()
+  //    )
+
+  init {
+    addRequirements(drivetrain)
+
+    translationPID = PathPlannerTranslationPID(poskP.get(), poskI.get(), poskD.get())
+    rotationPID = PathPlannerRotationPID(thetakP.get(), thetakI.get(), thetakD.get())
+
+    swerveDriveController =
+      PathPlannerHolonomicDriveController(
+        translationPID,
+        rotationPID,
+        DrivetrainConstants.MAX_AUTO_VEL,
+        Translation2d(
+          DrivetrainConstants.DRIVETRAIN_LENGTH / 2,
+          DrivetrainConstants.DRIVETRAIN_WIDTH / 2
+        )
+          .magnitude
+          .meters,
+      )
+  }
+
+  override fun initialize() {
+    trajStartTime = Clock.fpgaTime
+  }
+
+  override fun execute() {
+    if (thetakP.hasChanged() || thetakI.hasChanged() || thetakD.hasChanged()) {
+      rotationPID = PathPlannerRotationPID(thetakP.get(), thetakI.get(), thetakD.get())
+      swerveDriveController =
+        PathPlannerHolonomicDriveController(
+          translationPID,
+          rotationPID,
+          DrivetrainConstants.MAX_AUTO_VEL,
+          Translation2d(
+            DrivetrainConstants.DRIVETRAIN_LENGTH / 2,
+            DrivetrainConstants.DRIVETRAIN_WIDTH / 2
+          )
+            .magnitude
+            .meters,
+        )
+    }
+
+    if (poskP.hasChanged() || poskI.hasChanged() || poskD.hasChanged()) {
+      translationPID = PathPlannerTranslationPID(poskP.get(), poskI.get(), poskD.get())
+      swerveDriveController =
+        PathPlannerHolonomicDriveController(
+          translationPID,
+          rotationPID,
+          DrivetrainConstants.MAX_AUTO_VEL,
+          Translation2d(
+            DrivetrainConstants.DRIVETRAIN_LENGTH / 2,
+            DrivetrainConstants.DRIVETRAIN_WIDTH / 2
+          )
+            .magnitude
+            .meters,
+        )
+    }
+
+    val currentSpeeds = drivetrain.targetedChassisSpeeds
+    val poseRotation = drivetrain.odometryPose.rotation.inRotation2ds
+    val generatedTrajectory = path.getTrajectory(currentSpeeds, poseRotation)
+
+    // Sampling the trajectory for a state that we're trying to target
+    val stateFromTrajectory = generatedTrajectory.sample(trajCurTime.inSeconds)
+
+    // Retrieves the last sampled pose, so we can keep our `atReference` variable updated
+    Logger.recordOutput(
+      "Odometry/targetPose",
+      doubleArrayOf(
+        lastSampledPose.x.inMeters,
+        lastSampledPose.y.inMeters,
+        lastSampledPose.rotation.inRadians
+      )
+    )
+    Logger.recordOutput(
+      "Pathfollow/thetaSetpoint", stateFromTrajectory.targetHolonomicPose.rotation.degrees
+    )
+    Logger.recordOutput("Pathfollow/currentTheta", drivetrain.odometryPose.rotation.inDegrees)
+
+    lastSampledPose = Pose2d(stateFromTrajectory.targetHolonomicPose)
+
+    val targetedChassisSpeeds =
+      swerveDriveController.calculateRobotRelativeSpeeds(
+        drivetrain.odometryPose, stateFromTrajectory
+      )
+
+    // Set closed loop request
+    pathFollowRequest.chassisSpeeds = targetedChassisSpeeds.chassisSpeedsWPILIB
+    drivetrain.currentRequest = pathFollowRequest
+
+    // Update trajectory time
+    trajCurTime = Clock.fpgaTime - trajStartTime
+  }
+
+  //    override fun isFinished(): Boolean {
+  //        trajCurTime = Clock.fpgaTime - trajStartTime
+  //        return endPathOnceAtReference &&
+  //                (!keepTrapping || swerveDriveController.atReference()) &&
+  //                trajCurTime > trajectoryGenerator.driveTrajectory.totalTimeSeconds.seconds
+  //    }
+  override fun isFinished(): Boolean {
+    return atReference
+  }
+
+  override fun end(interrupted: Boolean) {
+    if (interrupted) {
+      // Stop where we are if interrupted
+      drivetrain.currentRequest =
+        Request.DrivetrainRequest.OpenLoop(
+          0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond)
+        )
+    } else {
+      // Execute one last time to end up in the final state of the trajectory
+      // Since we weren't interrupted, we know curTime > endTime
+      execute()
+      drivetrain.currentRequest =
+        Request.DrivetrainRequest.OpenLoop(
+          0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond)
+        )
+    }
+  }
+}
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt
index 66abed12..cdfd1c29 100644
--- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt
@@ -39,7 +39,7 @@ sealed interface Request {
     ) : DrivetrainRequest
 
     class ClosedLoop(
-      val chassisSpeeds: ChassisSpeeds,
+      var chassisSpeeds: ChassisSpeeds,
       val chassisAccels: ChassisSpeeds =
         edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0)
     ) : DrivetrainRequest