diff --git a/.github/workflows/gradle.yml b/.github/workflows/gradle.yml
index ff145033..543287a4 100644
--- a/.github/workflows/gradle.yml
+++ b/.github/workflows/gradle.yml
@@ -8,7 +8,7 @@ jobs:
     runs-on: ubuntu-latest
 
     # This grabs the WPILib docker container
-    container: wpilib/roborio-cross-ubuntu:2024.1.1-beta-4
+    container: wpilib/roborio-cross-ubuntu:2024.1.1
     steps:
         # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it
         - uses: actions/checkout@v2
diff --git a/src/main/kotlin/com/team4099/lib/phoenix6/PositionVoltage.kt b/src/main/kotlin/com/team4099/lib/phoenix6/PositionVoltage.kt
new file mode 100644
index 00000000..0fd15fdc
--- /dev/null
+++ b/src/main/kotlin/com/team4099/lib/phoenix6/PositionVoltage.kt
@@ -0,0 +1,61 @@
+package com.team4099.lib.phoenix6
+
+import org.team4099.lib.units.AngularVelocity
+import org.team4099.lib.units.derived.Angle
+import org.team4099.lib.units.derived.ElectricalPotential
+import org.team4099.lib.units.derived.degrees
+import org.team4099.lib.units.derived.inRotations
+import org.team4099.lib.units.derived.inVolts
+import org.team4099.lib.units.derived.volts
+import org.team4099.lib.units.inRotationsPerSecond
+import org.team4099.lib.units.perSecond
+import com.ctre.phoenix6.controls.PositionVoltage as PositionVoltagePhoenix6
+
+class PositionVoltage(
+    private var position: Angle, // Assuming an AngularPosition type exists similar to AngularVelocity
+    private var enableFOC: Boolean = true,
+    private var feedforward: ElectricalPotential = 0.0.volts,
+    private var slot: Int = 0,
+    private var overrideBrakeDurNeutral: Boolean = false,
+    private var limitForwardMotion: Boolean = false,
+    private var limitReverseMotion: Boolean = false,
+    private var velocity: AngularVelocity = 0.0.degrees.perSecond,
+) {
+
+    val positionVoltagePhoenix6 = PositionVoltagePhoenix6(position.inRotations, velocity.inRotationsPerSecond, enableFOC, feedforward.inVolts, slot, overrideBrakeDurNeutral, limitForwardMotion, limitReverseMotion)
+
+    fun setPosition(new_position: Angle) {
+        position = new_position
+        positionVoltagePhoenix6.Position = new_position.inRotations
+    }
+
+    fun setEnableFOC(new_enableFOC: Boolean) {
+        enableFOC = new_enableFOC
+        positionVoltagePhoenix6.EnableFOC = new_enableFOC
+    }
+
+    fun setFeedforward(new_feedforward: ElectricalPotential) {
+        feedforward = new_feedforward
+        positionVoltagePhoenix6.FeedForward = new_feedforward.inVolts
+    }
+
+    fun setSlot(new_slot: Int) {
+        slot = new_slot
+        positionVoltagePhoenix6.Slot = new_slot
+    }
+
+    fun setOverrideBrakeDurNeutral(new_override: Boolean) {
+        overrideBrakeDurNeutral = new_override
+        positionVoltagePhoenix6.OverrideBrakeDurNeutral = new_override
+    }
+
+    fun setLimitForwardMotion(new_limitForward: Boolean) {
+        limitForwardMotion = new_limitForward
+        positionVoltagePhoenix6.LimitForwardMotion = new_limitForward
+    }
+
+    fun setLimitReverseMotion(new_limitReverse: Boolean) {
+        limitReverseMotion = new_limitReverse
+        positionVoltagePhoenix6.LimitReverseMotion = new_limitReverse
+    }
+}
\ No newline at end of file
diff --git a/src/main/kotlin/com/team4099/lib/phoenix6/VelocityVoltage.kt b/src/main/kotlin/com/team4099/lib/phoenix6/VelocityVoltage.kt
new file mode 100644
index 00000000..ccef0a1a
--- /dev/null
+++ b/src/main/kotlin/com/team4099/lib/phoenix6/VelocityVoltage.kt
@@ -0,0 +1,64 @@
+package com.team4099.lib.phoenix6
+
+import com.ctre.phoenix6.controls.VelocityVoltage as VelocityVoltagePhoenix6
+import org.team4099.lib.units.AngularAcceleration
+import org.team4099.lib.units.AngularVelocity
+import org.team4099.lib.units.derived.ElectricalPotential
+import org.team4099.lib.units.derived.degrees
+import org.team4099.lib.units.derived.inVolts
+import org.team4099.lib.units.derived.volts
+import org.team4099.lib.units.inRotationsPerSecond
+import org.team4099.lib.units.inRotationsPerSecondPerSecond
+import org.team4099.lib.units.perSecond
+
+class VelocityVoltage(private var velocity: AngularVelocity,
+                           private var acceleration: AngularAcceleration = 0.0.degrees.perSecond.perSecond,
+                           private var enableFOC:Boolean = true,
+                            private var feedforward: ElectricalPotential = 0.0.volts,
+                           private var slot:Int = 0,
+                           private var overrideBrakeDurNeutral: Boolean = false,
+                           private var limitForwardMotion: Boolean = false,
+                           private var limitReverseMotion: Boolean = false){
+
+    val velocityVoltagePhoenix6 = VelocityVoltagePhoenix6(velocity.inRotationsPerSecond, acceleration.inRotationsPerSecondPerSecond, enableFOC, feedforward.inVolts, slot, overrideBrakeDurNeutral, limitForwardMotion, limitReverseMotion)
+
+    fun setVelocity(new_velocity: AngularVelocity) {
+        velocity = new_velocity
+        velocityVoltagePhoenix6.Velocity = velocity.inRotationsPerSecond
+    }
+
+    fun setAcceleration(new_accel: AngularAcceleration) {
+        acceleration = new_accel
+        velocityVoltagePhoenix6.Acceleration = acceleration.inRotationsPerSecondPerSecond
+    }
+
+    fun setEnableFOC(new_enableFOC: Boolean) {
+        enableFOC = new_enableFOC
+        velocityVoltagePhoenix6.EnableFOC = new_enableFOC
+    }
+
+    fun setFeedforward(new_feedforward: ElectricalPotential) {
+        feedforward = new_feedforward
+        velocityVoltagePhoenix6.FeedForward = new_feedforward.inVolts
+    }
+
+    fun setSlot(new_slot: Int) {
+        slot = new_slot
+        velocityVoltagePhoenix6.Slot = new_slot
+    }
+
+    fun setOverrideBrakeDurNeutral(new_override: Boolean) {
+        overrideBrakeDurNeutral = new_override
+        velocityVoltagePhoenix6.OverrideBrakeDurNeutral = new_override
+    }
+
+    fun setLimitForwardMotion(new_limitForward: Boolean) {
+        limitForwardMotion = new_limitForward
+        velocityVoltagePhoenix6.LimitForwardMotion = new_limitForward
+    }
+
+    fun setLimitReverseMotion(new_limitReverse: Boolean) {
+        limitReverseMotion = new_limitReverse
+        velocityVoltagePhoenix6.LimitReverseMotion = new_limitReverse
+    }
+}
\ No newline at end of file
diff --git a/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt b/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt
index 65645c9c..24e5d450 100644
--- a/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt
+++ b/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt
@@ -6,10 +6,10 @@ package com.team4099.robot2023
 const val MAVEN_GROUP = ""
 const val MAVEN_NAME = "Crescendo-2024"
 const val VERSION = "unspecified"
-const val GIT_REVISION = 26
-const val GIT_SHA = "73f4f3ef5bab17de6b7bbc919ece786ceb7b52ea"
-const val GIT_DATE = "2024-01-15T19:49:07Z"
+const val GIT_REVISION = 34
+const val GIT_SHA = "d5fd1849cf1454f10fbe3db431150aff1c19e8cc"
+const val GIT_DATE = "2024-01-19T23:40:38Z"
 const val GIT_BRANCH = "intake"
-const val BUILD_DATE = "2024-01-15T20:41:51Z"
-const val BUILD_UNIX_TIME = 1705369311220L
+const val BUILD_DATE = "2024-01-20T00:01:13Z"
+const val BUILD_UNIX_TIME = 1705726873776L
 const val DIRTY = 1
diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
index f648bf94..c04891d5 100644
--- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
+++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
@@ -14,11 +14,14 @@ import com.team4099.robot2023.subsystems.intake.IntakeIONEO
 import com.team4099.robot2023.subsystems.intake.IntakeIOSim
 import com.team4099.robot2023.subsystems.limelight.LimelightVision
 import com.team4099.robot2023.subsystems.limelight.LimelightVisionIO
+import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest
 import com.team4099.robot2023.subsystems.vision.Vision
 import com.team4099.robot2023.subsystems.vision.camera.CameraIONorthstar
 import com.team4099.robot2023.util.driver.Ryan
 import edu.wpi.first.wpilibj.RobotBase
+import org.team4099.lib.geometry.Pose2d
 import org.team4099.lib.smoothDeadband
+import org.team4099.lib.units.base.feet
 import org.team4099.lib.units.derived.Angle
 import org.team4099.lib.units.derived.degrees
 
@@ -90,7 +93,7 @@ object RobotContainer {
   }
 
   fun zeroSensors() {
-    drivetrain.zeroSensors()
+    drivetrain.currentRequest = DrivetrainRequest.ZeroSensors()
   }
 
   fun zeroAngle(toAngle: Angle) {
@@ -114,7 +117,6 @@ object RobotContainer {
 
   fun mapTeleopControls() {
     ControlBoard.resetGyro.whileTrue(ResetGyroYawCommand(drivetrain, toAngle = 180.degrees))
-    ControlBoard.groundIntakeTest.whileTrue(intake.generateIntakeTestCommand())
     //    ControlBoard.autoLevel.whileActiveContinuous(
     //      GoToAngle(drivetrain).andThen(AutoLevel(drivetrain))
     //    )
diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt
index d7c4c6dd..97472978 100644
--- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt
+++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt
@@ -1,6 +1,7 @@
 package com.team4099.robot2023.commands.drivetrain
 
 import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
+import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest
 import edu.wpi.first.wpilibj2.command.Command
 import org.team4099.lib.units.base.meters
 import org.team4099.lib.units.derived.radians
@@ -12,7 +13,7 @@ class DriveBrakeModeCommand(val drivetrain: Drivetrain) : Command() {
   }
 
   override fun execute() {
-    drivetrain.setOpenLoop(
+    drivetrain.currentRequest = DrivetrainRequest.OpenLoop(
         0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond)
       )
     drivetrain.swerveModules.forEach() { it.setDriveBrakeMode(true) }
diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt
index 51438b3b..9c6fea93 100644
--- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt
+++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt
@@ -6,6 +6,7 @@ import com.team4099.lib.trajectory.CustomTrajectoryGenerator
 import com.team4099.lib.trajectory.Waypoint
 import com.team4099.robot2023.config.constants.DrivetrainConstants
 import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
+import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest
 import com.team4099.robot2023.util.AllianceFlipUtil
 import com.team4099.robot2023.util.Velocity2d
 import edu.wpi.first.math.kinematics.ChassisSpeeds
@@ -215,7 +216,7 @@ class DrivePathCommand(
       Pose2dWPILIB(desiredState.poseMeters.translation, desiredRotation.position)
     )
 
-    drivetrain.setClosedLoop(
+    drivetrain.currentRequest = DrivetrainRequest.ClosedLoop(
         nextDriveState,
         ChassisAccels(xAccel, yAccel, 0.0.radians.perSecond.perSecond).chassisAccelsWPILIB
       )
@@ -280,16 +281,16 @@ class DrivePathCommand(
     Logger.recordOutput("ActiveCommands/DrivePathCommand", false)
     if (interrupted) {
       // Stop where we are if interrupted
-      drivetrain.setOpenLoop(
-          0.degrees.perSecond, Pair(0.meters.perSecond, 0.meters.perSecond)
-        )
+      drivetrain.currentRequest = 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.setOpenLoop(
-          0.degrees.perSecond, Pair(0.meters.perSecond, 0.meters.perSecond)
-        )
+      drivetrain.currentRequest = DrivetrainRequest.OpenLoop(
+        0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond)
+      )
     }
   }
 }
diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GoToAngle.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GoToAngle.kt
index 0779ea8f..30d1a3f2 100644
--- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GoToAngle.kt
+++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GoToAngle.kt
@@ -3,6 +3,7 @@ package com.team4099.robot2023.commands.drivetrain
 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.DrivetrainRequest as DrivetrainRequest
 import edu.wpi.first.wpilibj.RobotBase
 import edu.wpi.first.wpilibj2.command.Command
 import org.littletonrobotics.junction.Logger
@@ -81,7 +82,7 @@ class GoToAngle(val drivetrain: Drivetrain) : Command() {
 
     val thetaFeedback = thetaPID.calculate(drivetrain.odometryPose.rotation, desiredAngle)
 
-    drivetrain.setOpenLoop(
+    drivetrain.currentRequest = DrivetrainRequest.OpenLoop(
         thetaFeedback, Pair(0.0.meters.perSecond, 0.0.meters.perSecond), fieldOriented = true
       )
 
@@ -97,8 +98,8 @@ class GoToAngle(val drivetrain: Drivetrain) : Command() {
   }
 
   override fun end(interrupted: Boolean) {
-    drivetrain.setOpenLoop(
-        0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond)
-      )
+    drivetrain.currentRequest = DrivetrainRequest.OpenLoop(
+      0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond)
+    )
   }
 }
diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GyroAutoLevel.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GyroAutoLevel.kt
index 5d22049e..e49f994f 100644
--- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GyroAutoLevel.kt
+++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GyroAutoLevel.kt
@@ -4,6 +4,7 @@ import com.team4099.lib.hal.Clock
 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.DrivetrainRequest as DrivetrainRequest
 import edu.wpi.first.wpilibj2.command.Command
 import org.littletonrobotics.junction.Logger
 import org.team4099.lib.controller.ProfiledPIDController
@@ -91,7 +92,7 @@ class GyroAutoLevel(val drivetrain: Drivetrain) : Command() {
   override fun execute() {
     Logger.recordOutput("ActiveCommands/AutoLevelCommand", true)
 
-    drivetrain.setOpenLoop(
+    drivetrain.currentRequest = DrivetrainRequest.OpenLoop(
         0.0.radians.perSecond, gyroFeedback, fieldOriented = true
       )
 
@@ -119,8 +120,8 @@ class GyroAutoLevel(val drivetrain: Drivetrain) : Command() {
   }
 
   override fun end(interrupted: Boolean) {
-    drivetrain.setOpenLoop(
-        0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond)
-      )
+    drivetrain.currentRequest = DrivetrainRequest.OpenLoop(
+      0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond)
+    )
   }
 }
diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/OpenLoopReverseCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/OpenLoopReverseCommand.kt
index c4847a4d..6f4fa148 100644
--- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/OpenLoopReverseCommand.kt
+++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/OpenLoopReverseCommand.kt
@@ -1,6 +1,7 @@
 package com.team4099.robot2023.commands.drivetrain
 
 import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
+import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest
 import edu.wpi.first.wpilibj2.command.Command
 import org.team4099.lib.units.base.feet
 import org.team4099.lib.units.derived.degrees
@@ -12,7 +13,7 @@ class OpenLoopReverseCommand(val drivetrain: Drivetrain) : Command() {
   }
 
   override fun execute() {
-    drivetrain.setOpenLoop(
+    drivetrain.currentRequest = DrivetrainRequest.OpenLoop(
         0.degrees.perSecond,
         Pair(-5.0.feet.perSecond, 0.0.feet.perSecond),
         fieldOriented = false
diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt
index 849c0b28..18f554e4 100644
--- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt
+++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt
@@ -1,6 +1,7 @@
 package com.team4099.robot2023.commands.drivetrain
 
 import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
+import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest
 import com.team4099.robot2023.util.driver.DriverProfile
 import edu.wpi.first.wpilibj2.command.Command
 import org.littletonrobotics.junction.Logger
@@ -23,7 +24,7 @@ class TeleopDriveCommand(
   override fun execute() {
     val speed = driver.driveSpeedClampedSupplier(driveX, driveY, slowMode)
     val rotation = driver.rotationSpeedClampedSupplier(turn, slowMode)
-    drivetrain.setOpenLoop(rotation, speed)
+    drivetrain.currentRequest = DrivetrainRequest.OpenLoop(rotation, speed)
     Logger.recordOutput("ActiveCommands/TeleopDriveCommand", true)
   }
   override fun isFinished(): Boolean {
diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ZeroSensorsCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ZeroSensorsCommand.kt
index 8654eba7..d3b6d267 100644
--- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ZeroSensorsCommand.kt
+++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ZeroSensorsCommand.kt
@@ -1,6 +1,7 @@
 package com.team4099.robot2023.commands.drivetrain
 
 import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
+import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest
 import edu.wpi.first.wpilibj2.command.Command
 import org.littletonrobotics.junction.Logger
 
@@ -11,7 +12,7 @@ class ZeroSensorsCommand(val drivetrain: Drivetrain) : Command() {
   }
 
   override fun initialize() {
-    drivetrain.zeroSensors()
+    drivetrain.currentRequest = DrivetrainRequest.ZeroSensors()
   }
 
   override fun isFinished(): Boolean {
diff --git a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt
index d1383fad..2a3446ac 100644
--- a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt
+++ b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt
@@ -97,6 +97,4 @@ object ControlBoard {
 
   // for testing
   val setArmCommand = Trigger { technician.yButton }
-
-  val groundIntakeTest = Trigger { driver.aButton }
 }
diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt
index 00b49039..6e4de373 100644
--- a/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt
+++ b/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt
@@ -12,6 +12,6 @@ object IntakeConstants {
   const val ROLLER_MOTOR_INVERTED = true
   const val ROLLER_GEAR_RATIO = 24.0 / 12.0 // this one has been updated
 
-  // TODO: Update value
-  val IDLE_ROLLER_VOLTAGE = 2.0.volts
+  // TODO: Update the idle roller voltage later
+  val IDLE_ROLLER_VOLTAGE = 1.0.volts
 }
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt
index eae04997..870ddbdc 100644
--- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt
@@ -5,6 +5,7 @@ import com.team4099.robot2023.config.constants.Constants
 import com.team4099.robot2023.config.constants.DrivetrainConstants
 import com.team4099.robot2023.config.constants.VisionConstants
 import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIO
+import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest
 import com.team4099.robot2023.util.Alert
 import com.team4099.robot2023.util.FMSData
 import com.team4099.robot2023.util.PoseEstimator
@@ -168,6 +169,38 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB
 
     var lastGyroYaw = 0.0.radians
 
+    var velocityTarget = 0.degrees.perSecond
+
+    var targetedDriveVector = Pair(0.meters.perSecond, 0.meters.perSecond)
+
+    var fieldOrientation = true
+
+    var targetedChassisSpeeds = edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0)
+
+    var targetedChassisAccels = edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0)
+
+    var currentState: DrivetrainState = DrivetrainState.UNINITIALIZED
+    var currentRequest: DrivetrainRequest = DrivetrainRequest.ZeroSensors()
+        set(value) {
+            when (value) {
+                is DrivetrainRequest.OpenLoop -> {
+                    velocityTarget = value.angularVelocity
+                    targetedDriveVector = value.driveVector
+                    fieldOrientation = value.fieldOriented
+                }
+                is DrivetrainRequest.ClosedLoop -> {
+                    targetedChassisSpeeds = value.chassisSpeeds
+                    targetedChassisAccels = value.chassisAccels
+                }
+                else -> {}
+            }
+            field = value
+        }
+
+    init {
+        zeroSteering()
+    }
+
     override fun periodic() {
         val startTime = Clock.realTimestamp
         gyroNotConnectedAlert.set(!gyroInputs.gyroConnected)
@@ -240,6 +273,44 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB
             "LoggedRobot/Subsystems/DrivetrainLoopTimeMS",
             (Clock.realTimestamp - startTime).inMilliseconds
         )
+
+        var nextState = currentState
+
+        when (currentState) {
+            DrivetrainState.UNINITIALIZED -> {
+                // Transitions
+                nextState = DrivetrainState.ZEROING_SENSORS
+            }
+            DrivetrainState.ZEROING_SENSORS -> {
+                zeroSensors()
+
+                // Transitions
+                currentRequest = DrivetrainRequest.Idle()
+                nextState = fromRequestToState(currentRequest)
+            }
+            DrivetrainState.OPEN_LOOP -> {
+                // Outputs
+                setOpenLoop(velocityTarget, targetedDriveVector, fieldOrientation)
+
+                // Transitions
+                nextState = fromRequestToState(currentRequest)
+            }
+            DrivetrainState.CLOSED_LOOP -> {
+                // Outputs
+                setClosedLoop(targetedChassisSpeeds, targetedChassisAccels)
+
+                // Transitions
+                nextState = fromRequestToState(currentRequest)
+            }
+            DrivetrainState.IDLE -> {
+                nextState = fromRequestToState(currentRequest)
+            }
+        }
+
+        currentState = nextState
+
+        // Log the current state
+        Logger.recordOutput("Drivetrain/requestedState", currentState.toString())
     }
 
     private fun updateOdometry() {
@@ -553,4 +624,32 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB
     fun addVisionData(visionData: List<PoseEstimator.TimestampedVisionUpdate>) {
         swerveDrivePoseEstimator.addVisionData(visionData)
     }
+
+    companion object {
+        enum class DrivetrainState {
+            UNINITIALIZED,
+            IDLE,
+            ZEROING_SENSORS,
+            OPEN_LOOP,
+            CLOSED_LOOP;
+
+            inline fun equivalentToRequest(request: DrivetrainRequest): Boolean {
+                return (
+                        (request is DrivetrainRequest.ZeroSensors && this == ZEROING_SENSORS) ||
+                                (request is DrivetrainRequest.OpenLoop && this == OPEN_LOOP) ||
+                                (request is DrivetrainRequest.ClosedLoop && this == CLOSED_LOOP) ||
+                                (request is DrivetrainRequest.Idle && this == IDLE)
+                        )
+            }
+        }
+
+        inline fun fromRequestToState(request: DrivetrainRequest): DrivetrainState {
+            return when (request) {
+                is DrivetrainRequest.OpenLoop -> DrivetrainState.OPEN_LOOP
+                is DrivetrainRequest.ClosedLoop -> DrivetrainState.CLOSED_LOOP
+                is DrivetrainRequest.ZeroSensors -> DrivetrainState.ZEROING_SENSORS
+                is DrivetrainRequest.Idle -> DrivetrainState.IDLE
+            }
+        }
+    }
 }
\ No newline at end of file
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt
index e65f207e..4871b7c0 100644
--- a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt
@@ -18,16 +18,12 @@ class Intake(val io: IntakeIO) : SubsystemBase() {
     var isZeroed = false
     var currentState: IntakeState = IntakeState.UNINITIALIZED
 
-    var currentRequest: Request.IntakeRequest = Request.IntakeRequest.Idle()
+    var currentRequest: Request.IntakeRequest = Request.IntakeRequest.OpenLoop(IntakeConstants.IDLE_ROLLER_VOLTAGE)
         set(value) {
             rollerVoltageTarget = when (value) {
                 is Request.IntakeRequest.OpenLoop -> {
                     value.rollerVoltage
                 }
-
-                is Request.IntakeRequest.Idle -> {
-                    IntakeConstants.IDLE_ROLLER_VOLTAGE
-                }
             }
 
             field = value
@@ -57,13 +53,7 @@ class Intake(val io: IntakeIO) : SubsystemBase() {
                 // loop cycle
 
                 // Transitions
-                nextState = IntakeState.IDLE
-            }
-            IntakeState.IDLE -> {
-                setRollerVoltage(IntakeConstants.IDLE_ROLLER_VOLTAGE)
-
-                // Transitions
-                nextState = fromRequestToState(currentRequest)
+                nextState = IntakeState.OPEN_LOOP
             }
             IntakeState.OPEN_LOOP -> {
                 setRollerVoltage(rollerVoltageTarget)
@@ -85,28 +75,25 @@ class Intake(val io: IntakeIO) : SubsystemBase() {
     }
 
     fun generateIntakeTestCommand(): Command {
-        val returnCommand = runOnce({ currentRequest = Request.IntakeRequest.OpenLoop(12.volts) })
+        val returnCommand = runOnce { currentRequest = Request.IntakeRequest.OpenLoop(12.volts) }
         return returnCommand
     }
 
     companion object {
         enum class IntakeState {
             UNINITIALIZED,
-            IDLE,
             OPEN_LOOP;
 
             fun equivalentToRequest(request: Request.IntakeRequest): Boolean {
                 return (
-                        (request is Request.IntakeRequest.OpenLoop && this == OPEN_LOOP) ||
-                                (request is Request.IntakeRequest.Idle && this == IDLE)
-                        )
+                        (request is Request.IntakeRequest.OpenLoop && this == OPEN_LOOP)
+                )
             }
         }
 
         fun fromRequestToState(request: Request.IntakeRequest): IntakeState {
             return when (request) {
                 is Request.IntakeRequest.OpenLoop -> IntakeState.OPEN_LOOP
-                is Request.IntakeRequest.Idle -> IntakeState.IDLE
             }
         }
     }
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt
index 6ba96d7e..6c0569bd 100644
--- a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt
@@ -15,49 +15,49 @@ import org.team4099.lib.units.perMinute
 import org.team4099.lib.units.perSecond
 
 interface IntakeIO {
-  class IntakeIOInputs : LoggableInputs {
-    var rollerVelocity = 0.0.rotations.perMinute
-    var rollerAppliedVoltage = 0.0.volts
+    class IntakeIOInputs : LoggableInputs {
+        var rollerVelocity = 0.0.rotations.perMinute
+        var rollerAppliedVoltage = 0.0.volts
 
-    var rollerSupplyCurrent = 0.0.amps
-    var rollerStatorCurrent = 0.0.amps
+        var rollerSupplyCurrent = 0.0.amps
+        var rollerStatorCurrent = 0.0.amps
 
-    var rollerTemp = 0.0.celsius
+        var rollerTemp = 0.0.celsius
 
-    var isSimulated = false
+        var isSimulated = false
 
-    override fun toLog(table: LogTable?) {
-      table?.put("rollerVelocityRPM", rollerVelocity.inRotationsPerMinute)
-      table?.put("rollerAppliedVoltage", rollerAppliedVoltage.inVolts)
-      table?.put("rollerSupplyCurrentAmps", rollerSupplyCurrent.inAmperes)
-      table?.put("rollerStatorCurrentAmps", rollerStatorCurrent.inAmperes)
-      table?.put("rollerTempCelsius", rollerTemp.inCelsius)
-    }
+        override fun toLog(table: LogTable?) {
+            table?.put("rollerVelocityRPM", rollerVelocity.inRotationsPerMinute)
+            table?.put("rollerAppliedVoltage", rollerAppliedVoltage.inVolts)
+            table?.put("rollerSupplyCurrentAmps", rollerSupplyCurrent.inAmperes)
+            table?.put("rollerStatorCurrentAmps", rollerStatorCurrent.inAmperes)
+            table?.put("rollerTempCelsius", rollerTemp.inCelsius)
+        }
 
-    override fun fromLog(table: LogTable?) {
-      table?.get("rollerVelocityRPM", rollerVelocity.inRotationsPerMinute)?.let {
-        rollerVelocity = it.rotations.perSecond
-      }
+        override fun fromLog(table: LogTable?) {
+            table?.get("rollerVelocityRPM", rollerVelocity.inRotationsPerMinute)?.let {
+                rollerVelocity = it.rotations.perSecond
+            }
 
-      table?.get("rollerAppliedVoltage", rollerAppliedVoltage.inVolts)?.let {
-        rollerAppliedVoltage = it.volts
-      }
+            table?.get("rollerAppliedVoltage", rollerAppliedVoltage.inVolts)?.let {
+                rollerAppliedVoltage = it.volts
+            }
 
-      table?.get("rollerSupplyCurrentAmps", rollerSupplyCurrent.inAmperes)?.let {
-        rollerSupplyCurrent = it.amps
-      }
+            table?.get("rollerSupplyCurrentAmps", rollerSupplyCurrent.inAmperes)?.let {
+                rollerSupplyCurrent = it.amps
+            }
 
-      table?.get("rollerStatorCurrentAmps", rollerStatorCurrent.inAmperes)?.let {
-        rollerStatorCurrent = it.amps
-      }
+            table?.get("rollerStatorCurrentAmps", rollerStatorCurrent.inAmperes)?.let {
+                rollerStatorCurrent = it.amps
+            }
 
-      table?.get("rollerTempCelsius", rollerTemp.inCelsius)?.let { rollerTemp = it.celsius }
+            table?.get("rollerTempCelsius", rollerTemp.inCelsius)?.let { rollerTemp = it.celsius }
+        }
     }
-  }
 
-  fun updateInputs(io: IntakeIOInputs) {}
+    fun updateInputs(io: IntakeIOInputs) {}
 
-  fun setRollerVoltage(voltage: ElectricalPotential) {}
+    fun setRollerVoltage(voltage: ElectricalPotential) {}
 
-  fun setRollerBrakeMode(brake: Boolean) {}
-}
+    fun setRollerBrakeMode(brake: Boolean) {}
+}
\ No newline at end of file
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIONEO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIONEO.kt
index dadcd0db..5718011c 100644
--- a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIONEO.kt
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIONEO.kt
@@ -19,82 +19,81 @@ import org.team4099.lib.units.sparkMaxAngularMechanismSensor
 import kotlin.math.absoluteValue
 
 object IntakeIONEO : IntakeIO {
-  private val rollerSparkMax =
-    CANSparkMax(Constants.Intake.ROLLER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless)
+    private val rollerSparkMax =
+        CANSparkMax(Constants.Intake.ROLLER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless)
 
-  private val rollerSensor =
-    sparkMaxAngularMechanismSensor(
-      rollerSparkMax, IntakeConstants.ROLLER_GEAR_RATIO, IntakeConstants.VOLTAGE_COMPENSATION
-    )
+    private val rollerSensor =
+        sparkMaxAngularMechanismSensor(
+            rollerSparkMax, IntakeConstants.ROLLER_GEAR_RATIO, IntakeConstants.VOLTAGE_COMPENSATION
+        )
 
-  init {
-    rollerSparkMax.restoreFactoryDefaults()
-    rollerSparkMax.clearFaults()
+    init {
+        rollerSparkMax.restoreFactoryDefaults()
+        rollerSparkMax.clearFaults()
 
-    rollerSparkMax.enableVoltageCompensation(IntakeConstants.VOLTAGE_COMPENSATION.inVolts)
-    rollerSparkMax.setSmartCurrentLimit(IntakeConstants.ROLLER_CURRENT_LIMIT.inAmperes.toInt())
-    rollerSparkMax.inverted = IntakeConstants.ROLLER_MOTOR_INVERTED
+        rollerSparkMax.enableVoltageCompensation(IntakeConstants.VOLTAGE_COMPENSATION.inVolts)
+        rollerSparkMax.setSmartCurrentLimit(IntakeConstants.ROLLER_CURRENT_LIMIT.inAmperes.toInt())
+        rollerSparkMax.inverted = IntakeConstants.ROLLER_MOTOR_INVERTED
 
-    rollerSparkMax.idleMode = CANSparkMax.IdleMode.kCoast
+        rollerSparkMax.idleMode = CANSparkMax.IdleMode.kCoast
 
-    rollerSparkMax.openLoopRampRate = 0.0
-    rollerSparkMax.burnFlash()
+        rollerSparkMax.burnFlash()
 
-    MotorChecker.add(
-      "Ground Intake",
-      "Roller",
-      MotorCollection(
-        mutableListOf(Neo(rollerSparkMax, "Roller Motor")),
-        IntakeConstants.ROLLER_CURRENT_LIMIT,
-        70.celsius,
-        IntakeConstants.ROLLER_CURRENT_LIMIT - 0.amps,
-        90.celsius
-      ),
-    )
-  }
+        MotorChecker.add(
+            "Ground Intake",
+            "Roller",
+            MotorCollection(
+                mutableListOf(Neo(rollerSparkMax, "Roller Motor")),
+                IntakeConstants.ROLLER_CURRENT_LIMIT,
+                70.celsius,
+                30.amps,
+                90.celsius
+            ),
+        )
+    }
 
-  override fun updateInputs(inputs: IntakeIO.IntakeIOInputs) {
-    inputs.rollerVelocity = rollerSensor.velocity
-    inputs.rollerAppliedVoltage = rollerSparkMax.busVoltage.volts * rollerSparkMax.appliedOutput
-    inputs.rollerStatorCurrent = rollerSparkMax.outputCurrent.amps
+    override fun updateInputs(inputs: IntakeIO.IntakeIOInputs) {
+        inputs.rollerVelocity = rollerSensor.velocity
+        inputs.rollerAppliedVoltage = rollerSparkMax.busVoltage.volts * rollerSparkMax.appliedOutput
+        inputs.rollerStatorCurrent = rollerSparkMax.outputCurrent.amps
 
-    // BusVoltage * SupplyCurrent = AppliedVoltage * StatorCurrent
-    // AppliedVoltage = percentOutput * BusVoltage
-    // SupplyCurrent = (percentOutput * BusVoltage / BusVoltage) * StatorCurrent =
-    // percentOutput * statorCurrent
-    inputs.rollerSupplyCurrent =
-      inputs.rollerStatorCurrent * rollerSparkMax.appliedOutput.absoluteValue
-    inputs.rollerTemp = rollerSparkMax.motorTemperature.celsius
+        // BusVoltage * SupplyCurrent = AppliedVoltage * StatorCurrent
+        // AppliedVoltage = percentOutput * BusVoltage
+        // SupplyCurrent = (percentOutput * BusVoltage / BusVoltage) * StatorCurrent =
+        // percentOutput * statorCurrent
+        inputs.rollerSupplyCurrent =
+            inputs.rollerStatorCurrent * rollerSparkMax.appliedOutput.absoluteValue
+        inputs.rollerTemp = rollerSparkMax.motorTemperature.celsius
 
-    Logger.recordOutput(
-      "Intake/rollerMotorOvercurrentFault",
-      rollerSparkMax.getFault(CANSparkMax.FaultID.kOvercurrent)
-    )
-    Logger.recordOutput("Intake/busVoltage", rollerSparkMax.busVoltage)
-  }
+        Logger.recordOutput(
+            "Intake/rollerMotorOvercurrentFault",
+            rollerSparkMax.getFault(CANSparkMax.FaultID.kOvercurrent)
+        )
+        Logger.recordOutput("Intake/busVoltage", rollerSparkMax.busVoltage)
+    }
 
-  /**
-   * Sets the roller motor voltage, ensures the voltage is limited to battery voltage compensation
-   *
-   * @param voltage the voltage to set the roller motor to
-   */
-  override fun setRollerVoltage(voltage: ElectricalPotential) {
-    rollerSparkMax.setVoltage(
-      clamp(voltage, -IntakeConstants.VOLTAGE_COMPENSATION, IntakeConstants.VOLTAGE_COMPENSATION)
-        .inVolts
-    )
-  }
+    /**
+     * Sets the roller motor voltage, ensures the voltage is limited to battery voltage compensation
+     *
+     * @param voltage the voltage to set the roller motor to
+     */
+    override fun setRollerVoltage(voltage: ElectricalPotential) {
+        rollerSparkMax.setVoltage(
+            clamp(voltage, -IntakeConstants.VOLTAGE_COMPENSATION, IntakeConstants.VOLTAGE_COMPENSATION)
+                .inVolts
+        )
+    }
 
-  /**
-   * Sets the roller motor brake mode
-   *
-   * @param brake if it brakes
-   */
-  override fun setRollerBrakeMode(brake: Boolean) {
-    if (brake) {
-      rollerSparkMax.idleMode = CANSparkMax.IdleMode.kBrake
-    } else {
-      rollerSparkMax.idleMode = CANSparkMax.IdleMode.kCoast
+    /**
+     * Sets the roller motor brake mode
+     *
+     * @param brake if it brakes
+     */
+    override fun setRollerBrakeMode(brake: Boolean) {
+        if (brake) {
+            rollerSparkMax.idleMode = CANSparkMax.IdleMode.kBrake
+        } else {
+            rollerSparkMax.idleMode = CANSparkMax.IdleMode.kCoast
+        }
     }
-  }
 }
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 0f144610..9c77f65b 100644
--- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt
@@ -60,7 +60,6 @@ sealed interface Request {
 
   sealed interface IntakeRequest : Request {
     class OpenLoop(val rollerVoltage: ElectricalPotential) : IntakeRequest
-    class Idle() : IntakeRequest
   }
 
   sealed interface DrivetrainRequest : Request {