diff --git a/simgui-ds.json b/simgui-ds.json
index 97f4895e..4726e2df 100644
--- a/simgui-ds.json
+++ b/simgui-ds.json
@@ -1,9 +1,4 @@
 {
-  "System Joysticks": {
-    "window": {
-      "visible": false
-    }
-  },
   "keyboardJoysticks": [
     {
       "axisConfig": [
@@ -97,6 +92,9 @@
   "robotJoysticks": [
     {
       "guid": "Keyboard0"
+    },
+    {
+      "guid": "Keyboard1"
     }
   ]
 }
diff --git a/simgui.json b/simgui.json
index a5719869..14555702 100644
--- a/simgui.json
+++ b/simgui.json
@@ -5,9 +5,9 @@
         "visible": false
       }
     },
-    "Timing": {
+    "RoboRIO": {
       "window": {
-        "visible": false
+        "visible": true
       }
     }
   },
diff --git a/src/main/kotlin/com/team4099/lib/logging/DoubleArrayConversions.kt b/src/main/kotlin/com/team4099/lib/logging/DoubleArrayConversions.kt
new file mode 100644
index 00000000..b6c9453e
--- /dev/null
+++ b/src/main/kotlin/com/team4099/lib/logging/DoubleArrayConversions.kt
@@ -0,0 +1,86 @@
+package com.team4099.lib.logging
+
+import edu.wpi.first.math.kinematics.SwerveModulePosition
+import edu.wpi.first.math.kinematics.SwerveModuleState
+import org.team4099.lib.geometry.Pose2d
+import org.team4099.lib.geometry.Pose3d
+import org.team4099.lib.geometry.Quaternion
+import org.team4099.lib.geometry.Rotation3d
+import org.team4099.lib.geometry.Transform2d
+import org.team4099.lib.geometry.Transform3d
+import org.team4099.lib.geometry.Translation2d
+import org.team4099.lib.geometry.Translation3d
+import org.team4099.lib.geometry.Twist2d
+import org.team4099.lib.geometry.Twist3d
+import org.team4099.lib.units.base.inMeters
+import org.team4099.lib.units.derived.inRadians
+
+fun Pose2d.toDoubleArray(): DoubleArray {
+    return doubleArrayOf(this.x.inMeters, this.y.inMeters, this.rotation.inRadians)
+}
+
+fun Pose3d.toDoubleArray(): DoubleArray {
+    return doubleArrayOf(
+        this.x.inMeters,
+        this.y.inMeters,
+        this.z.inMeters,
+        this.rotation.rotation3d.quaternion.w,
+        this.rotation.rotation3d.quaternion.x,
+        this.rotation.rotation3d.quaternion.y,
+        this.rotation.rotation3d.quaternion.z
+    )
+}
+
+fun Quaternion.toDoubleArray(): Array<Double> {
+    return arrayOf(this.w.inRadians, this.x.inMeters, this.y.inMeters, this.z.inMeters)
+}
+
+fun Rotation3d.toDoubleArray(): Array<Double> {
+    return arrayOf(this.x.inRadians, this.y.inRadians, this.z.inRadians)
+}
+
+fun Transform2d.toDoubleArray(): Array<Double> {
+    return arrayOf(this.translation.x.inMeters, this.translation.y.inMeters, this.rotation.inRadians)
+}
+
+fun Transform3d.toDoubleArray(): Array<Double> {
+    return arrayOf(
+        this.translation.x.inMeters,
+        this.translation.y.inMeters,
+        this.translation.z.inMeters,
+        this.rotation.x.inRadians,
+        this.rotation.y.inRadians,
+        this.rotation.z.inRadians
+    )
+}
+
+fun Translation2d.toDoubleArray(): Array<Double> {
+    return arrayOf(this.x.inMeters, this.y.inMeters)
+}
+
+fun Translation3d.toDoubleArray(): Array<Double> {
+    return arrayOf(this.x.inMeters, this.y.inMeters, this.z.inMeters)
+}
+
+fun Twist2d.toDoubleArray(): Array<Double> {
+    return arrayOf(this.dx.inMeters, this.dy.inMeters, this.dtheta.inRadians)
+}
+
+fun Twist3d.toDoubleArray(): Array<Double> {
+    return arrayOf(
+        this.dx.inMeters,
+        this.dy.inMeters,
+        this.dz.inMeters,
+        this.rx.inRadians,
+        this.ry.inRadians,
+        this.rz.inRadians
+    )
+}
+
+fun SwerveModulePosition.toDoubleArray(): Array<Double> {
+    return arrayOf(this.distanceMeters, this.angle.radians)
+}
+
+fun SwerveModuleState.toDoubleArray(): Array<Double> {
+    return arrayOf(this.speedMetersPerSecond, this.angle.radians)
+}
\ 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 7b3aaa47..61286adf 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 = 258
-const val GIT_SHA = "0a3451d3fae466b43823feb361ab59009e458cc1"
-const val GIT_DATE = "2024-02-14T20:51:17Z"
-const val GIT_BRANCH = "main"
-const val BUILD_DATE = "2024-02-15T21:03:56Z"
-const val BUILD_UNIX_TIME = 1708049036263L
+const val GIT_REVISION = 260
+const val GIT_SHA = "851ecf84ee0e858110dc9ab517f2015eb3654d88"
+const val GIT_DATE = "2024-02-19T15:19:59Z"
+const val GIT_BRANCH = "AutoAim"
+const val BUILD_DATE = "2024-02-20T01:25:57Z"
+const val BUILD_UNIX_TIME = 1708410357413L
 const val DIRTY = 1
diff --git a/src/main/kotlin/com/team4099/robot2023/Robot.kt b/src/main/kotlin/com/team4099/robot2023/Robot.kt
index 568a0451..493bbac5 100644
--- a/src/main/kotlin/com/team4099/robot2023/Robot.kt
+++ b/src/main/kotlin/com/team4099/robot2023/Robot.kt
@@ -161,6 +161,7 @@ object Robot : LoggedRobot() {
     // motor checker stuff
     val motorCheckerStartTime = Clock.realTimestamp
     MotorChecker.periodic()
+
     Logger.recordOutput(
       "LoggedRobot/Subsystems/MotorCheckerLoopTimeMS",
       (Clock.realTimestamp - motorCheckerStartTime).inMilliseconds
diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
index 3cecfa04..962e0c33 100644
--- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
+++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
@@ -36,7 +36,14 @@ import org.team4099.lib.smoothDeadband
 import org.team4099.lib.units.derived.Angle
 import org.team4099.lib.units.derived.degrees
 import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest
+import FieldConstants
+import com.team4099.robot2023.commands.drivetrain.TargetPoseCommand
 import com.team4099.robot2023.subsystems.wrist.WristIO
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup
+import org.team4099.lib.geometry.Pose2d
+import org.team4099.lib.geometry.Pose3d
+import org.team4099.lib.geometry.Rotation3d
+import javax.swing.GroupLayout
 
 object RobotContainer {
   private val drivetrain: Drivetrain
@@ -88,7 +95,7 @@ object RobotContainer {
       wrist = Wrist(WristIOSim)
     }
 
-    superstructure = Superstructure(intake, feeder, elevator, wrist, flywheel)
+    superstructure = Superstructure(drivetrain, intake, feeder, elevator, wrist, flywheel)
     vision.setDataInterfaces({ drivetrain.odometryPose }, { drivetrain.addVisionData(it) })
     limelight.poseSupplier = { drivetrain.odometryPose }
   }
@@ -148,6 +155,24 @@ object RobotContainer {
 
     ControlBoard.resetGyro.whileTrue(ResetGyroYawCommand(drivetrain, toAngle = 180.degrees))
 
+    ControlBoard.intake.whileTrue(superstructure.groundIntakeCommand())
+    ControlBoard.ampPrep.whileTrue(superstructure.prepAmpCommand())
+    ControlBoard.scoreAmp.whileTrue(superstructure.scoreAmpCommand())
+    ControlBoard.autoAim.whileTrue(
+      ParallelCommandGroup(
+        TargetPoseCommand(
+          drivetrain,
+          Pose2d(
+            FieldConstants.Speaker.centerSpeakerOpening.x,
+            FieldConstants.Speaker.centerSpeakerOpening.y,
+            0.0.degrees
+          )
+        )
+      , superstructure.autoAimCommand())
+    )
+    ControlBoard.scoreSpeaker.whileTrue(superstructure.scoreSpeakerLowCommand())
+
+    /*
     ControlBoard.runGroundIntake.whileTrue(superstructure.groundIntakeCommand())
     ControlBoard.ejectGamePiece.whileTrue(superstructure.ejectGamePieceCommand())
     ControlBoard.prepAmpScore.whileTrue(superstructure.prepAmpCommand())
@@ -158,6 +183,7 @@ object RobotContainer {
     ControlBoard.climbExtend.whileTrue(superstructure.climbExtendCommand())
     ControlBoard.climbRetract.whileTrue(superstructure.climbRetractCommand())
     ControlBoard.requestIdle.whileTrue(superstructure.requestIdleCommand())
+     */
 
     /*
     TUNING COMMANDS
diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt
new file mode 100644
index 00000000..4a929455
--- /dev/null
+++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt
@@ -0,0 +1,123 @@
+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
+import edu.wpi.first.wpilibj.RobotBase
+import edu.wpi.first.wpilibj2.command.Command
+import org.littletonrobotics.junction.Logger
+import org.team4099.lib.controller.ProfiledPIDController
+import org.team4099.lib.controller.TrapezoidProfile
+import org.team4099.lib.geometry.Pose2d
+import org.team4099.lib.units.Velocity
+import org.team4099.lib.units.base.inMeters
+import org.team4099.lib.units.derived.Angle
+import org.team4099.lib.units.derived.Radian
+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.inRadians
+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.radians
+import org.team4099.lib.units.inDegreesPerSecond
+import org.team4099.lib.units.perSecond
+import kotlin.math.PI
+import kotlin.math.atan2
+
+class TargetPoseCommand(val drivetrain: Drivetrain, val targetPose: Pose2d) : Command() {
+    private val thetaPID: ProfiledPIDController<Radian, Velocity<Radian>>
+
+    val thetakP =
+        LoggedTunableValue(
+            "Pathfollow/thetakP",
+            Pair({ it.inDegreesPerSecondPerDegree }, { it.degrees.perSecond.perDegree })
+        )
+    val thetakI =
+        LoggedTunableValue(
+            "Pathfollow/thetakI",
+            Pair(
+                { it.inDegreesPerSecondPerDegreeSeconds }, { it.degrees.perSecond.perDegreeSeconds }
+            )
+        )
+    val thetakD =
+        LoggedTunableValue(
+            "Pathfollow/thetakD",
+            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)
+    var desiredAngle: Angle = 0.0.degrees
+
+    init {
+        addRequirements(drivetrain)
+
+        Logger.recordOutput("Odometry/targetedPose", doubleArrayOf(targetPose.x.inMeters, targetPose.y.inMeters, targetPose.rotation.inRadians))
+
+        if (RobotBase.isReal()) {
+            thetakP.initDefault(DrivetrainConstants.PID.AUTO_THETA_PID_KP)
+            thetakI.initDefault(DrivetrainConstants.PID.AUTO_THETA_PID_KI)
+            thetakD.initDefault(DrivetrainConstants.PID.AUTO_THETA_PID_KD)
+        } else {
+            thetakP.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KP)
+            thetakI.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KI)
+            thetakD.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KD)
+        }
+
+        thetaPID =
+            ProfiledPIDController(
+                thetakP.get(),
+                thetakI.get(),
+                thetakD.get(),
+                TrapezoidProfile.Constraints(thetaMaxVel.get(), thetaMaxAccel.get())
+            )
+        thetaPID.enableContinuousInput(-PI.radians, PI.radians)
+    }
+
+    override fun initialize() {
+        thetaPID.reset(drivetrain.odometryPose.rotation)
+    }
+
+    override fun execute() {
+        Logger.recordOutput("ActiveCommands/TargetPoseCommand", true)
+
+        val currentPose = drivetrain.odometryPose
+        val relativeToRobotPose = targetPose.relativeTo(currentPose)
+        desiredAngle = currentPose.rotation + atan2(relativeToRobotPose.y.inMeters, relativeToRobotPose.x.inMeters).radians
+
+        val thetaFeedback = thetaPID.calculate(currentPose.rotation, desiredAngle)
+
+        drivetrain.currentRequest =
+            Request.DrivetrainRequest.OpenLoop(
+                thetaFeedback,
+                Pair(drivetrain.fieldVelocity.x, drivetrain.fieldVelocity.y),
+                fieldOriented = true
+            )
+
+        Logger.recordOutput("AutoLevel/CurrentYawDegrees", drivetrain.odometryPose.rotation.inDegrees)
+        Logger.recordOutput("AutoLevel/DesiredYawDegrees", desiredAngle.inDegrees)
+        Logger.recordOutput("AutoLevel/thetaFeedbackDPS", thetaFeedback.inDegreesPerSecond)
+    }
+
+    override fun isFinished(): Boolean {
+        return (drivetrain.odometryPose.rotation - desiredAngle).absoluteValue <
+                DrivetrainConstants.PID.AUTO_THETA_ALLOWED_ERROR
+    }
+
+    override fun end(interrupted: Boolean) {
+        drivetrain.currentRequest =
+            Request.DrivetrainRequest.OpenLoop(
+                0.0.radians.perSecond, Pair(drivetrain.fieldVelocity.x, drivetrain.fieldVelocity.y)
+            )
+    }
+}
\ No newline at end of file
diff --git a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt
index fd81ca69..26229008 100644
--- a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt
+++ b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt
@@ -27,13 +27,14 @@ object ControlBoard {
     get() = -driver.leftYAxis
 
   val turn: Double
-    get() = driver.rightXAxis
+    get() = operator.leftXAxis
 
   val slowMode: Boolean
     get() = driver.leftShoulderButton
 
   val resetGyro = Trigger { driver.startButton && driver.selectButton }
 
+  /*
   val shooterUp = Trigger { driver.bButton }
   val shooterDown = Trigger { driver.xButton }
   val wristTestUp = Trigger { driver.yButton }
@@ -64,4 +65,13 @@ object ControlBoard {
   val testWrist = Trigger { operator.aButton }
   val testElevator = Trigger { operator.bButton }
   val setTuningMode = Trigger { driver.rightShoulderButton }
+
+   */
+
+  val intake = Trigger {driver.aButton}
+  val scoreAmp = Trigger {driver.bButton}
+  val scoreSpeaker = Trigger {driver.xButton}
+
+  val autoAim = Trigger {operator.aButton}
+  val ampPrep = Trigger {operator.bButton}
 }
diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt
index e5c2f98d..026a342b 100644
--- a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt
+++ b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt
@@ -1,7 +1,9 @@
 package com.team4099.robot2023.config.constants
 
+import org.team4099.lib.units.base.meters
 import org.team4099.lib.units.base.seconds
 import org.team4099.lib.units.milli
+import org.team4099.lib.units.perSecond
 
 typealias GamePiece = Constants.Universal.GamePiece
 
@@ -11,6 +13,7 @@ typealias NodeTier = Constants.Universal.NodeTier
 
 object Constants {
   object Universal {
+    val gravity = -9.807.meters.perSecond.perSecond
     val SIM_MODE = Tuning.SimType.SIM
     const val REAL_FIELD = false
 
diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt
index 9519e3e9..6f110ff6 100644
--- a/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt
+++ b/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt
@@ -1,7 +1,96 @@
-package com.team4099.robot2023.config.constants
+import org.team4099.lib.apriltag.AprilTagFieldLayout
+import org.team4099.lib.geometry.Pose2d
+import org.team4099.lib.geometry.Pose3d
+import org.team4099.lib.geometry.Translation2d
+import org.team4099.lib.geometry.Translation3d
+import org.team4099.lib.units.base.inches
+import org.team4099.lib.units.derived.degrees
 
-import org.team4099.lib.units.base.feet
+// Copyright (c) 2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
 
+
+/**
+ * Contains various field dimensions and useful reference points. Dimensions are in meters, and sets
+ * of corners start in the lower left moving clockwise. **All units in Meters** <br></br>
+ * <br></br>
+ *
+ *
+ * All translations and poses are stored with the origin at the rightmost point on the BLUE
+ * ALLIANCE wall.<br></br>
+ * <br></br>
+ * Length refers to the *x* direction (as described by wpilib) <br></br>
+ * Width refers to the *y* direction (as described by wpilib)
+ */
 object FieldConstants {
-  val fieldLength = 54.feet
-}
+  var fieldLength = 651.223.inches
+  var fieldWidth =  323.277.inches
+  var wingX = 229.201.inches
+  var podiumX = 126.75.inches
+  var startingLineX = 74.111.inches
+
+  var ampCenter = Translation2d(72.455.inches, 322.996.inches)
+
+  var aprilTagWidth = 6.50.inches
+
+  var noteThickness = 2.inches
+
+  /** Staging locations for each note  */
+  object StagingLocations {
+    var centerlineX = fieldLength / 2.0
+
+    // need to update
+    var centerlineFirstY = 29.638.inches
+    var centerlineSeparationY = 66.0.inches
+    var spikeX = 114.0.inches
+
+    // need
+    var spikeFirstY = 161.638.inches
+    var spikeSeparationY = 57.0.inches
+
+    var centerlineTranslations: Array<Translation2d?> = arrayOfNulls(5)
+    var spikeTranslations: Array<Translation2d?> = arrayOfNulls(3)
+
+    init {
+      for (i in centerlineTranslations.indices) {
+        centerlineTranslations[i] =
+          Translation2d(centerlineX, centerlineFirstY + (centerlineSeparationY * i))
+      }
+    }
+
+    init {
+      for (i in spikeTranslations.indices) {
+        spikeTranslations[i] = Translation2d(spikeX, spikeFirstY + (spikeSeparationY * i))
+      }
+    }
+  }
+
+  /** Each corner of the speaker *  */
+  object Speaker {
+    // corners (blue alliance origin)
+    var topRightSpeaker = Translation3d(
+      18.055.inches,
+      238.815.inches,
+      83.091.inches
+    )
+
+    var topLeftSpeaker = Translation3d(
+      18.055.inches,
+      197.765.inches,
+      83.091.inches
+    )
+
+    var bottomRightSpeaker: Translation3d =
+      Translation3d(0.0.inches, 238.815.inches, 78.324.inches)
+    var bottomLeftSpeaker: Translation3d =
+      Translation3d(0.0.inches, 197.765.inches, 78.324.inches)
+
+    /** Center of the speaker opening (blue alliance)  */
+    var centerSpeakerOpening: Translation3d = (bottomLeftSpeaker + topRightSpeaker) / 2.0
+    var speakerTargetPose = Pose2d(centerSpeakerOpening.x, centerSpeakerOpening.y, 0.0.degrees)
+  }
+}
\ No newline at end of file
diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt
index 8afbd68d..2bd728f6 100644
--- a/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt
+++ b/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt
@@ -3,6 +3,7 @@ package com.team4099.robot2023.config.constants
 import org.team4099.lib.units.Velocity
 import org.team4099.lib.units.base.amps
 import org.team4099.lib.units.base.grams
+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.DerivativeGain
@@ -24,6 +25,9 @@ object FlywheelConstants {
 
   val VOLTAGE_COMPENSATION = 12.volts
 
+  val FLYWHEEL_SPEED_TRANSFER_PERCENTAGE = 0.475
+  val FLYWHEEL_RADIUS = 2.inches
+
   val INERTIA = 0.0014550597.kilo.grams * 1.0.meters.squared
 
   val RIGHT_FLYWHEEL_SUPPLY_CURRENT_LIMIT = 50.0.amps
@@ -62,7 +66,7 @@ object FlywheelConstants {
   val IDLE_VELOCITY = 0.0.rotations.perMinute
   val SPEAKER_VELOCITY = 10_000.rotations.perMinute
   val AMP_VELOCITY = 5_000.rotations.perMinute
-  val AMP_SCORE_TIME = 1.seconds
-  val SPEAKER_SCORE_TIME = 1.seconds
+  val AMP_SCORE_TIME = 0.5.seconds
+  val SPEAKER_SCORE_TIME = 0.5.seconds
   val EJECT_VELOCITY = 5_000.rotations.perMinute
 }
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 9f3baa5e..0766fd2b 100644
--- a/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt
+++ b/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt
@@ -1,6 +1,10 @@
 package com.team4099.robot2023.config.constants
 
+import org.team4099.lib.geometry.Transform2d
+import org.team4099.lib.geometry.Translation2d
 import org.team4099.lib.units.base.amps
+import org.team4099.lib.units.base.inches
+import org.team4099.lib.units.derived.degrees
 import org.team4099.lib.units.derived.volts
 
 object IntakeConstants {
@@ -8,6 +12,9 @@ object IntakeConstants {
   val CENTER_WHEEL_INERTIA = 0.002459315
   val VOLTAGE_COMPENSATION = 12.0.volts
 
+  val INTAKE_TRANSFORM = Transform2d(Translation2d(-18.0.inches, 0.0.inches), 0.0.degrees)
+  val SIM_INTAKE_DISTANCE = 8.inches
+
   // TODO: Change gear ratio according to robot
   val ROLLER_CURRENT_LIMIT = 50.0.amps
   const val ROLLER_MOTOR_INVERTED = true
diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/SuperstructureConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/SuperstructureConstants.kt
new file mode 100644
index 00000000..75ab426f
--- /dev/null
+++ b/src/main/kotlin/com/team4099/robot2023/config/constants/SuperstructureConstants.kt
@@ -0,0 +1,32 @@
+package com.team4099.robot2023.config.constants
+
+import org.team4099.lib.units.base.inches
+import org.team4099.lib.units.base.meters
+import org.team4099.lib.units.derived.degrees
+import org.team4099.lib.units.derived.rotations
+import org.team4099.lib.units.perMinute
+
+object SuperstructureConstants {
+
+    val distanceFlywheelSpeedTable = listOf(
+        Pair(1.meters, 3000.rotations.perMinute),
+        Pair(2.meters, 3500.rotations.perMinute),
+        Pair(3.meters, 4000.rotations.perMinute),
+        Pair(4.meters, 4500.rotations.perMinute),
+        Pair(5.meters, 5000.rotations.perMinute),
+        Pair(6.meters, 5500.rotations.perMinute),
+        Pair(7.meters, 6000.rotations.perMinute),
+        Pair(500.inches, 8000.rotations.perMinute)
+    )
+
+    val distanceWristAngleTable = listOf(
+        Pair(46.5.inches, -35.degrees),
+        Pair(75.inches, -22.degrees),
+        Pair(117.inches, -11.5.degrees),
+        Pair(149.inches, -6.5.degrees),
+        Pair(191.inches, -2.degrees),
+        Pair(253.inches, 1.5.degrees),
+        Pair(321.inches, 4.degrees),
+        Pair(500.inches , 10.degrees)
+    )
+}
\ No newline at end of file
diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt
index 97d4c4f0..ef5c6a8c 100644
--- a/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt
+++ b/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt
@@ -37,6 +37,9 @@ object WristConstants {
   val WRIST_MIN_ROTATION = (-37.0).degrees
 
   val WRIST_ZERO_SIM_OFFSET = 27.5.degrees
+  val NOTE_ANGLE_SIM_OFFSET = -24.degrees
+  val WRIST_AXIS_TO_NOTE_HOLD_POSITION = 14.5.inches
+  val WRIST_AXIS_TO_NOTE_LAUNCH_POSITION = 10.inches
 
   val MAX_WRIST_VELOCITY = 300.degrees.perSecond
   val MAX_WRIST_ACCELERATION = 600.degrees.perSecond.perSecond
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt
index 5826a581..64b72e5b 100644
--- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt
@@ -97,11 +97,8 @@ class Feeder(val io: FeederIO) : SubsystemBase() {
   var lastBeamState = false
   val hasNote: Boolean
     get() {
-      return false
-      /*
       return (inputs.beamBroken &&
-        Clock.fpgaTime - firstTripBeamBreakTime > FeederConstants.BEAM_BREAK_WAIT_TIME) || inputs.isSimulated
-       */
+        Clock.fpgaTime - firstTripBeamBreakTime > FeederConstants.BEAM_BREAK_WAIT_TIME)
     }
 
   private var timeProfileGeneratedAt = Clock.fpgaTime
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/AutoAim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/AutoAim.kt
new file mode 100644
index 00000000..ee94bff2
--- /dev/null
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/AutoAim.kt
@@ -0,0 +1,103 @@
+package com.team4099.robot2023.subsystems.superstructure
+
+import FieldConstants
+import com.ctre.phoenix6.controls.VelocityDutyCycle
+import com.team4099.lib.logging.LoggedTunableValue
+import com.team4099.robot2023.config.constants.SuperstructureConstants
+import com.team4099.robot2023.util.PoseEstimator
+import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap
+import edu.wpi.first.math.interpolation.InterpolatingTreeMap
+import edu.wpi.first.math.interpolation.Interpolator
+import java.util.function.Consumer
+import org.littletonrobotics.junction.Logger
+import org.team4099.lib.geometry.Pose2d
+import org.team4099.lib.units.AngularVelocity
+import org.team4099.lib.units.base.Length
+import org.team4099.lib.units.base.inInches
+import org.team4099.lib.units.base.inMeters
+import org.team4099.lib.units.base.inches
+import org.team4099.lib.units.base.meters
+import org.team4099.lib.units.derived.Angle
+import org.team4099.lib.units.derived.Radian
+import org.team4099.lib.units.derived.degrees
+import org.team4099.lib.units.derived.inDegrees
+import org.team4099.lib.units.derived.meterSquared
+import org.team4099.lib.units.derived.rotations
+import org.team4099.lib.units.inRotationsPerMinute
+import org.team4099.lib.units.perMinute
+
+class AutoAim {
+    var poseSupplier: () -> Pose2d = { Pose2d() }
+    val flywheelSpeedRPMInterpolationTable: InterpolatingDoubleTreeMap = InterpolatingDoubleTreeMap()
+    val wristAngleDegreesInterpolationTable: InterpolatingDoubleTreeMap = InterpolatingDoubleTreeMap()
+
+    val tunableFlywheelInterpolationTable = SuperstructureConstants.distanceFlywheelSpeedTable.mapIndexed {i, it ->
+                Pair(
+                    LoggedTunableValue("AutoAim/FlywheelInterpolation/${i}/Distance", it.first, Pair({ it.inInches }, { it.inches })),
+                    LoggedTunableValue("AutoAim/FlywheelInterpolation/${i}/SpeedRPM", it.second, Pair({ it.inRotationsPerMinute }, { it.rotations.perMinute }))
+                )
+            }
+
+    val tunableWristInterpolationTable = SuperstructureConstants.distanceWristAngleTable.mapIndexed {i, it ->
+            Pair(
+                LoggedTunableValue("AutoAim/WristInterpolation/${i}/Distance", it.first, Pair({ it.inInches }, { it.inches })),
+                LoggedTunableValue("AutoAim/WristInterpolation/${i}/AngleDegrees", it.second, Pair({ it.inDegrees }, { it.degrees }))
+            )
+        }
+
+    val interpolationTestDistance = LoggedTunableValue("AutoAim/TestDistance", 0.0.meters, Pair({it.inInches}, {it.inches}))
+
+    init {
+        updateFlywheelInterpolationTable()
+        updateWristInterpolationTable()
+    }
+
+    fun periodic() {
+        for (point in tunableFlywheelInterpolationTable) {
+            if (point.first.hasChanged() || point.second.hasChanged()) {
+                updateFlywheelInterpolationTable()
+                break
+            }
+        }
+
+        for (point in tunableWristInterpolationTable) {
+            if (point.first.hasChanged() || point.second.hasChanged()) {
+                updateWristInterpolationTable()
+                break
+            }
+        }
+
+        Logger.recordOutput("AutoAim/InterpolatedFlywheelSpeed", flywheelSpeedRPMInterpolationTable.get(interpolationTestDistance.get().inMeters))
+
+        Logger.recordOutput("AutoAim/InterpolatedWristAngle", wristAngleDegreesInterpolationTable.get(interpolationTestDistance.get().inMeters))
+    }
+
+
+    fun calculateDistanceFromSpeaker(): Length {
+        val distance = (poseSupplier() - FieldConstants.Speaker.speakerTargetPose).translation.magnitude.meters
+        Logger.recordOutput("AutoAim/currentDistanceInhces", distance.inInches)
+        return distance
+    }
+    fun calculateFlywheelSpeed(): AngularVelocity {
+        return flywheelSpeedRPMInterpolationTable.get(calculateDistanceFromSpeaker().inMeters).rotations.perMinute
+    }
+
+    fun calculateWristAngle(): Angle {
+        return wristAngleDegreesInterpolationTable.get(calculateDistanceFromSpeaker().inMeters).degrees
+    }
+
+    fun updateFlywheelInterpolationTable() {
+        flywheelSpeedRPMInterpolationTable.clear()
+        tunableFlywheelInterpolationTable.forEach {
+            flywheelSpeedRPMInterpolationTable.put(it.first.get().inMeters, it.second.get().inRotationsPerMinute)
+        }
+    }
+
+    fun updateWristInterpolationTable() {
+        wristAngleDegreesInterpolationTable.clear()
+        tunableWristInterpolationTable.forEach {
+            wristAngleDegreesInterpolationTable.put(it.first.get().inMeters, it.second.get().inDegrees)
+        }
+    }
+
+}
\ No newline at end of file
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..9894b375 100644
--- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt
@@ -15,6 +15,8 @@ sealed interface Request {
 
     class GroundIntake() : SuperstructureRequest
 
+    class AutoAim() : SuperstructureRequest
+
     class EjectGamePiece() : SuperstructureRequest
 
     class PrepScoreAmp() : SuperstructureRequest
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt
index 0204c6f0..59542944 100644
--- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt
@@ -1,12 +1,18 @@
 package com.team4099.robot2023.subsystems.superstructure
 
+import FieldConstants
 import com.team4099.lib.hal.Clock
+import com.team4099.lib.logging.toDoubleArray
+import com.team4099.robot2023.RobotContainer
 import com.team4099.robot2023.config.constants.WristConstants
+import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
 import com.team4099.robot2023.subsystems.elevator.Elevator
 import com.team4099.robot2023.subsystems.feeder.Feeder
 import com.team4099.robot2023.subsystems.flywheel.Flywheel
 import com.team4099.robot2023.subsystems.intake.Intake
 import com.team4099.robot2023.subsystems.wrist.Wrist
+import com.team4099.robot2023.util.NoteSimulation
+import edu.wpi.first.wpilibj.RobotBase
 import edu.wpi.first.wpilibj2.command.Command
 import edu.wpi.first.wpilibj2.command.SubsystemBase
 import org.littletonrobotics.junction.Logger
@@ -20,9 +26,12 @@ 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.derived.degrees
+import org.team4099.lib.units.derived.inDegrees
 import org.team4099.lib.units.derived.volts
+import org.team4099.lib.units.inRotationsPerMinute
 
 class Superstructure(
+  private val drivetrain: Drivetrain,
   private val intake: Intake,
   private val feeder: Feeder,
   private val elevator: Elevator,
@@ -30,6 +39,8 @@ class Superstructure(
   private val flywheel: Flywheel
 ) : SubsystemBase() {
 
+  val aimer: AutoAim = AutoAim()
+
   var currentRequest: Request.SuperstructureRequest = Request.SuperstructureRequest.Idle()
 
   var currentState: SuperstructureStates = SuperstructureStates.UNINITIALIZED
@@ -42,36 +53,49 @@ class Superstructure(
 
   var lastTransitionTime = Clock.fpgaTime
 
-  fun toDoubleArray(somePose: Pose3d): DoubleArray {
-    return doubleArrayOf(
-      somePose.x.inMeters,
-      somePose.y.inMeters,
-      somePose.z.inMeters,
-      somePose.rotation.rotation3d.quaternion.w,
-      somePose.rotation.rotation3d.quaternion.x,
-      somePose.rotation.rotation3d.quaternion.y,
-      somePose.rotation.rotation3d.quaternion.z
-    )
+  var noteHoldingID = -1
+
+  var notes = mutableListOf<NoteSimulation>()
+
+
+  init {
+    FieldConstants.StagingLocations.spikeTranslations.forEach {
+      notes.add(NoteSimulation(notes.size, Pose3d(it?.x ?: 0.0.inches, it?.y ?: 0.0.inches, FieldConstants.noteThickness/2.0, Rotation3d())))
+    }
+
+    FieldConstants.StagingLocations.centerlineTranslations.forEach {
+      notes.add(NoteSimulation(notes.size, Pose3d(it?.x ?: 0.0.inches, it?.y ?: 0.0.inches, FieldConstants.noteThickness/2.0, Rotation3d())))
+    }
+
+    notes.forEach {it.poseSupplier = {drivetrain.odometryPose}}
+    notes.forEach {it.wristAngleSupplier = {wrist.inputs.wristPosition}}
+    notes.forEach {it.elevatorHeightSupplier = {elevator.inputs.elevatorPosition}}
+    notes.forEach {it.flywheelAngularVelocitySupplier = {flywheel.inputs.rightFlywheelVelocity}}
+    notes[0].currentState = NoteSimulation.NoteStates.IN_ROBOT
+
+    aimer.poseSupplier = { drivetrain.odometryPose }
+
   }
 
   override fun periodic() {
+    notes.forEach {it.periodic()}
+    notes.forEach { Logger.recordOutput("NoteSimulation/${it.id}", it.currentPose.toDoubleArray())}
+
+    aimer.periodic()
 
     Logger.recordOutput(
       "SimulatedMechanisms/0",
-      toDoubleArray(
         Pose3d()
           .transformBy(
             Transform3d(
               Translation3d(0.0.inches, 0.0.inches, elevator.inputs.elevatorPosition),
               Rotation3d()
             )
-          )
+          ).toDoubleArray()
       )
-    )
 
     Logger.recordOutput(
       "SimulatedMechanisms/1",
-      toDoubleArray(
         Pose3d(0.0.meters, 0.0.meters, 0.0.meters, Rotation3d())
           .transformBy(
             Transform3d(
@@ -86,9 +110,8 @@ class Superstructure(
                 0.0.degrees
               )
             )
-          )
+          ).toDoubleArray()
       )
-    )
 
     val intakeLoopStartTime = Clock.realTimestamp
     intake.periodic()
@@ -157,6 +180,7 @@ class Superstructure(
         }
       }
       SuperstructureStates.IDLE -> {
+        noteHoldingID = 0
         intake.currentRequest =
           Request.IntakeRequest.OpenLoop(
             Intake.TunableIntakeStates.idleRollerVoltage.get(),
@@ -215,6 +239,10 @@ class Superstructure(
           is Request.SuperstructureRequest.Tuning -> {
             nextState = SuperstructureStates.TUNING
           }
+
+          is Request.SuperstructureRequest.AutoAim -> {
+            nextState = SuperstructureStates.AUTO_AIM
+          }
         }
       }
       SuperstructureStates.GROUND_INTAKE_PREP -> {
@@ -239,7 +267,18 @@ class Superstructure(
         feeder.currentRequest =
           Request.FeederRequest.OpenLoopIntake(Feeder.TunableFeederStates.intakeVoltage.get())
         flywheel.currentRequest = Request.FlywheelRequest.OpenLoop(2.volts)
-        if (feeder.hasNote) {
+
+        if (noteHoldingID == -1) {
+          for (note in notes) {
+            if (note.canIntake()) {
+              noteHoldingID = note.id
+              note.currentState = NoteSimulation.NoteStates.IN_ROBOT
+              break;
+            }
+          }
+        }
+
+        if (feeder.hasNote || (!RobotBase.isReal() && noteHoldingID != -1)) {
           currentRequest = Request.SuperstructureRequest.Idle()
           nextState = SuperstructureStates.IDLE
         }
@@ -276,11 +315,19 @@ class Superstructure(
         }
       }
       SuperstructureStates.SCORE_AMP -> {
+
+        if (noteHoldingID != -1) {
+          notes[noteHoldingID].currentState = NoteSimulation.NoteStates.IN_FLIGHT
+          noteHoldingID = -1
+        }
+
         feeder.currentRequest =
           Request.FeederRequest.OpenLoopShoot(Feeder.TunableFeederStates.shootVoltage.get())
         if (!feeder.hasNote &&
           Clock.fpgaTime - shootStartTime > Flywheel.TunableFlywheelStates.ampScoreTime.get()
         ) {
+
+          currentRequest = Request.SuperstructureRequest.Idle()
           nextState = SuperstructureStates.IDLE
         }
 
@@ -290,18 +337,54 @@ class Superstructure(
           }
         }
       }
+      SuperstructureStates.AUTO_AIM -> {
+        val targetFlywheelSpeed = aimer.calculateFlywheelSpeed()
+        val targetWristAngle = aimer.calculateWristAngle()
+
+        Logger.recordOutput("AutoAim/FlywheelSpeed", targetFlywheelSpeed.inRotationsPerMinute)
+        Logger.recordOutput("AutoAim/WristAngle", targetWristAngle.inDegrees)
+
+        flywheel.currentRequest =
+          Request.FlywheelRequest.TargetingVelocity(
+            targetFlywheelSpeed
+          )
+        wrist.currentRequest =
+          Request.WristRequest.TargetingPosition(
+            targetWristAngle
+          )
+
+        when (currentRequest) {
+          is Request.SuperstructureRequest.Idle -> {
+            nextState = SuperstructureStates.IDLE
+          }
+          is Request.SuperstructureRequest.ScoreSpeakerLow -> {
+            if (wrist.isAtTargetedPosition && flywheel.isAtTargetedVelocity) {
+              nextState = SuperstructureStates.SCORE_SPEAKER_LOW
+              shootStartTime = Clock.fpgaTime
+            }
+          }
+        }
+      }
+
       SuperstructureStates.SCORE_SPEAKER_LOW_PREP -> {
         elevator.currentRequest =
           Request.ElevatorRequest.TargetingPosition(
             Elevator.TunableElevatorHeights.shootSpeakerLow.get()
           )
+
+        val targetFlywheelSpeed = aimer.calculateFlywheelSpeed()
+        val targetWristAngle = aimer.calculateWristAngle()
+
+        Logger.recordOutput("AutoAim/FlywheelSpeed", targetFlywheelSpeed.inRotationsPerMinute)
+        Logger.recordOutput("AutoAim/WristAngle", targetWristAngle.inDegrees)
+
         flywheel.currentRequest =
           Request.FlywheelRequest.TargetingVelocity(
-            Flywheel.TunableFlywheelStates.speakerVelocity.get()
+            targetFlywheelSpeed
           )
         wrist.currentRequest =
           Request.WristRequest.TargetingPosition(
-            Wrist.TunableWristStates.subwooferSpeakerShotAngleLow.get()
+            targetWristAngle
           )
         if (wrist.isAtTargetedPosition &&
           flywheel.isAtTargetedVelocity &&
@@ -317,13 +400,18 @@ class Superstructure(
         }
       }
       SuperstructureStates.SCORE_SPEAKER_LOW -> {
+        if (noteHoldingID != -1) {
+          notes[noteHoldingID].currentState = NoteSimulation.NoteStates.IN_FLIGHT
+          noteHoldingID = -1
+        }
+
         feeder.currentRequest =
           Request.FeederRequest.OpenLoopShoot(Feeder.TunableFeederStates.shootVoltage.get())
         if (!feeder.hasNote &&
           Clock.fpgaTime - shootStartTime >
           Flywheel.TunableFlywheelStates.speakerScoreTime.get()
         ) {
-          nextState = SuperstructureStates.IDLE
+          currentRequest = Request.SuperstructureRequest.Idle()
         }
 
         when (currentRequest) {
@@ -359,6 +447,12 @@ class Superstructure(
         }
       }
       SuperstructureStates.SCORE_SPEAKER_MID -> {
+
+        if (noteHoldingID != -1) {
+          notes[noteHoldingID].currentState = NoteSimulation.NoteStates.IN_FLIGHT
+          noteHoldingID = -1
+        }
+
         feeder.currentRequest =
           Request.FeederRequest.OpenLoopShoot(Feeder.TunableFeederStates.shootVoltage.get())
         if (!feeder.hasNote &&
@@ -400,6 +494,12 @@ class Superstructure(
         }
       }
       SuperstructureStates.SCORE_SPEAKER_HIGH -> {
+
+        if (noteHoldingID != -1) {
+          notes[noteHoldingID].currentState = NoteSimulation.NoteStates.IN_FLIGHT
+          noteHoldingID = -1
+        }
+
         feeder.currentRequest =
           Request.FeederRequest.OpenLoopShoot(Feeder.TunableFeederStates.shootVoltage.get())
         if (!feeder.hasNote &&
@@ -674,6 +774,14 @@ class Superstructure(
     return returnCommand
   }
 
+  fun autoAimCommand(): Command  {
+    val returnCommand = runOnce {
+      currentRequest = Request.SuperstructureRequest.AutoAim()
+    }
+    returnCommand.name = "Auto"
+    return returnCommand
+  }
+
   companion object {
     enum class SuperstructureStates {
       UNINITIALIZED,
@@ -683,6 +791,7 @@ class Superstructure(
       HOME,
       GROUND_INTAKE_PREP,
       GROUND_INTAKE,
+      AUTO_AIM,
       SCORE_AMP_PREP,
       SCORE_AMP,
       SCORE_SPEAKER_LOW_PREP,
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt
index 999966b2..f893660e 100644
--- a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt
@@ -256,8 +256,13 @@ class Wrist(val io: WristIO) : SubsystemBase() {
       }
       WristStates.TARGETING_POSITION -> {
         Logger.recordOutput("Wrist/RequestedPosition", wristPositionTarget.inDegrees)
-
-        if (wristPositionTarget != lastWristPositionTarget) {
+        if ((wristPositionTarget - lastWristPositionTarget).absoluteValue < 5.degrees) {
+          wristProfile =
+            TrapezoidProfile(wristConstraints,
+            TrapezoidProfile.State(wristPositionTarget, 0.0.radians.perSecond),
+              wristProfile.initial)
+        }
+        else {
           val preProfileGenerate = Clock.fpgaTime
           wristProfile =
             TrapezoidProfile(
diff --git a/src/main/kotlin/com/team4099/robot2023/util/AllianceFlipUtil.kt b/src/main/kotlin/com/team4099/robot2023/util/AllianceFlipUtil.kt
index f2d3f810..6245cc59 100644
--- a/src/main/kotlin/com/team4099/robot2023/util/AllianceFlipUtil.kt
+++ b/src/main/kotlin/com/team4099/robot2023/util/AllianceFlipUtil.kt
@@ -3,7 +3,6 @@ package com.team4099.robot2023.util
 import com.team4099.lib.math.Zone2d
 import com.team4099.lib.trajectory.RotationSequence
 import com.team4099.lib.trajectory.Waypoint
-import com.team4099.robot2023.config.constants.FieldConstants
 import edu.wpi.first.math.geometry.Rotation2d
 import edu.wpi.first.math.trajectory.Trajectory
 import edu.wpi.first.wpilibj.DriverStation
diff --git a/src/main/kotlin/com/team4099/robot2023/util/NoteSimulation.kt b/src/main/kotlin/com/team4099/robot2023/util/NoteSimulation.kt
new file mode 100644
index 00000000..532d3768
--- /dev/null
+++ b/src/main/kotlin/com/team4099/robot2023/util/NoteSimulation.kt
@@ -0,0 +1,196 @@
+package com.team4099.robot2023.util
+
+import FieldConstants
+import com.team4099.lib.hal.Clock
+import org.littletonrobotics.junction.Logger
+import com.team4099.lib.logging.toDoubleArray
+import com.team4099.robot2023.config.constants.Constants
+import com.team4099.robot2023.config.constants.FlywheelConstants
+import com.team4099.robot2023.config.constants.IntakeConstants
+import com.team4099.robot2023.config.constants.WristConstants
+import org.team4099.lib.geometry.Pose2d
+import org.team4099.lib.geometry.Pose3d
+import org.team4099.lib.geometry.Rotation3d
+import org.team4099.lib.geometry.Transform3d
+import org.team4099.lib.geometry.Translation3d
+import org.team4099.lib.units.AngularVelocity
+import org.team4099.lib.units.base.Length
+import org.team4099.lib.units.base.Meter
+import org.team4099.lib.units.base.Second
+import org.team4099.lib.units.base.Time
+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.Angle
+import org.team4099.lib.units.derived.Radian
+import org.team4099.lib.units.derived.cos
+import org.team4099.lib.units.derived.degrees
+import org.team4099.lib.units.derived.inDegrees
+import org.team4099.lib.units.derived.radians
+import org.team4099.lib.units.derived.rotations
+import org.team4099.lib.units.derived.sin
+import org.team4099.lib.units.inMetersPerSecond
+import org.team4099.lib.units.inMetersPerSecondPerSecond
+import org.team4099.lib.units.inRadiansPerSecond
+import org.team4099.lib.units.perMinute
+import org.team4099.lib.units.perSecond
+import kotlin.math.absoluteValue
+import kotlin.math.max
+
+class NoteSimulation(val id: Int = 0, val offFieldPose: Pose3d = Pose3d(), val stagedPose: Pose3d = Pose3d()) {
+
+
+    var currentPose = Pose3d()
+
+    var xVel = 0.0.meters.perSecond
+    var yVel = 0.0.meters.perSecond
+    var zVel = 0.0.meters.perSecond
+
+    var xAccel = 0.0.meters.perSecond.perSecond
+    var yAccel = 0.0.meters.perSecond.perSecond
+    var zAccel = 0.0.meters.perSecond.perSecond
+
+    var lastXPos = 0.0.meters
+    var lastYPos = 0.0.meters
+    var lastZPos = 0.0.meters
+
+    var lastXVel = 0.0.meters.perSecond
+    var lastYVel = 0.0.meters.perSecond
+    var lastZVel = 0.0.meters.perSecond
+
+    var launchStartPose = Pose3d()
+    var lastUpdateTime = Clock.fpgaTime
+    var inclinationAngleTheta = 0.0.degrees
+    var azimuthalAnglePhi = 0.0.degrees
+    var launchVelocity = 0.meters.perSecond
+
+    var currentState = NoteStates.OFF_FIELD
+        set(value) {
+            if (currentState == NoteStates.IN_ROBOT && value == NoteStates.IN_FLIGHT) {
+                launchStartPose = getNoteInRobotPose()
+
+                lastUpdateTime = Clock.fpgaTime
+                inclinationAngleTheta = 90.degrees + (wristAngleSupplier() + WristConstants.NOTE_ANGLE_SIM_OFFSET)
+                azimuthalAnglePhi =  poseSupplier().rotation
+
+                lastXPos = launchStartPose.x
+                lastYPos = launchStartPose.y
+                lastZPos = launchStartPose.z
+
+                launchVelocity = (FlywheelConstants.FLYWHEEL_SPEED_TRANSFER_PERCENTAGE * flywheelAngularVelocitySupplier().inRadiansPerSecond * FlywheelConstants.FLYWHEEL_RADIUS.inMeters).meters.perSecond
+
+                lastXVel = launchVelocity * inclinationAngleTheta.sin * azimuthalAnglePhi.cos
+                lastYVel = launchVelocity * inclinationAngleTheta.sin * azimuthalAnglePhi.sin
+                lastZVel = launchVelocity * inclinationAngleTheta.cos
+
+
+            }
+            field = value
+        }
+
+    var poseSupplier: () -> Pose2d = { Pose2d() }
+
+    var wristAngleSupplier: () -> Angle = {0.0.degrees}
+
+    var elevatorHeightSupplier: () -> Length =  { 0.0.inches}
+
+    var flywheelAngularVelocitySupplier: () -> AngularVelocity = {0.0.rotations.perMinute}
+
+    fun periodic() {
+        Logger.recordOutput("NoteData/${id}/inclinationAngle", inclinationAngleTheta.inDegrees)
+        Logger.recordOutput("NoteData/${id}/azimuthalAngle", azimuthalAnglePhi.inDegrees)
+        Logger.recordOutput("NoteData/${id}/launchVelocityMetersPerSecond", launchVelocity.inMetersPerSecond)
+        Logger.recordOutput("NoteData/${id}/startPose", launchStartPose.toDoubleArray())
+        Logger.recordOutput("NoteData/${id}/state", currentState.name)
+        Logger.recordOutput("NoteSimulation/IntakePose", poseSupplier().transformBy(IntakeConstants.INTAKE_TRANSFORM).toDoubleArray())
+        when (currentState) {
+            NoteStates.OFF_FIELD -> currentPose = offFieldPose
+            NoteStates.STAGED -> currentPose = stagedPose
+            NoteStates.ON_FIELD -> {}
+            NoteStates.IN_ROBOT -> {
+                currentPose = getNoteInRobotPose()
+            }
+            NoteStates.IN_FLIGHT -> {
+                xAccel = if (lastZPos > FieldConstants.noteThickness/2) 0.0.meters.perSecond.perSecond else -6.meters.perSecond.perSecond * azimuthalAnglePhi.cos.absoluteValue * lastXVel.sign
+                yAccel = if (lastZPos > FieldConstants.noteThickness/2) 0.0.meters.perSecond.perSecond else -6.meters.perSecond.perSecond * azimuthalAnglePhi.sin.absoluteValue * lastYVel.sign
+                zAccel = (Constants.Universal.gravity)
+
+                if (lastZPos <= FieldConstants.noteThickness/2) {
+                    currentState = NoteStates.IN_ROBOT
+                }
+                update()
+
+                if (xVel.absoluteValue < 0.1.meters.perSecond && yVel.absoluteValue < 0.1.meters.perSecond) {
+                    currentState = NoteStates.ON_FIELD
+                }
+            }
+        }
+    }
+
+    fun update() {
+        val currentTime = Clock.fpgaTime
+        val dt = (Clock.fpgaTime - lastUpdateTime)
+
+        xVel = lastXVel + dt * xAccel
+        yVel = lastYVel + dt * yAccel
+        zVel = lastZVel + dt * zAccel
+
+        val xPos = lastXPos + dt * (xVel + lastXVel) / 2
+        val yPos = lastYPos + dt * (yVel + lastYVel) / 2
+        val zPos = max((lastZPos + dt * (zVel + lastZVel)/2).inMeters, FieldConstants.noteThickness.inMeters/2).meters
+
+        val rotation = if (zPos > FieldConstants.noteThickness / 2 ) launchStartPose.rotation else Rotation3d()
+
+        lastUpdateTime = currentTime
+
+        lastXPos = xPos
+        lastYPos = yPos
+        lastZPos = zPos
+
+        lastXVel = xVel
+        lastYVel = yVel
+        lastZVel = zVel
+
+        currentPose = Pose3d(xPos, yPos, zPos, rotation)
+
+        Logger.recordOutput("NoteData/${id}/xVel", xVel.inMetersPerSecond)
+        Logger.recordOutput("NoteData/${id}/yVel", yVel.inMetersPerSecond)
+        Logger.recordOutput("NoteData/${id}/zVel", zVel.inMetersPerSecond)
+        Logger.recordOutput("NoteData/${id}/xAccel", xAccel.inMetersPerSecondPerSecond)
+        Logger.recordOutput("NoteData/${id}/yAccel", yAccel.inMetersPerSecondPerSecond)
+        Logger.recordOutput("NoteData/${id}/zAccel", zAccel.inMetersPerSecondPerSecond)
+    }
+
+    fun getNoteInRobotPose(): Pose3d{
+        return poseSupplier().toPose3d()
+            .transformBy(
+                Transform3d(
+                    Translation3d(
+                        0.013.meters - WristConstants.WRIST_AXIS_TO_NOTE_HOLD_POSITION * wristAngleSupplier().cos,
+                        0.0.inches,
+                        elevatorHeightSupplier() + 0.58.meters + WristConstants.WRIST_AXIS_TO_NOTE_HOLD_POSITION * wristAngleSupplier().sin
+                    ),
+                    Rotation3d(
+                        0.0.degrees,
+                        wristAngleSupplier() + WristConstants.NOTE_ANGLE_SIM_OFFSET,
+                        0.0.degrees
+                    )
+                )
+            )
+    }
+
+    fun canIntake(): Boolean {
+        return (poseSupplier().transformBy(IntakeConstants.INTAKE_TRANSFORM) - currentPose.toPose2d()).translation.magnitude.meters < IntakeConstants.SIM_INTAKE_DISTANCE
+    }
+
+
+    enum class NoteStates {
+            OFF_FIELD,
+            STAGED,
+            ON_FIELD,
+            IN_ROBOT,
+            IN_FLIGHT;
+    }
+}
\ No newline at end of file