From 6a4c6476085e4be10d0766a9591aa6104e36c195 Mon Sep 17 00:00:00 2001
From: Sachet Korada <sachet.korada@gmail.com>
Date: Fri, 13 Dec 2024 10:54:51 -0500
Subject: [PATCH] Use Drive Subsystem object over Mecanum Drive

---
 .../ftc/teamcode/MecanumDrive.kt              | 515 -----------------
 .../firstinspires/ftc/teamcode/TankDrive.kt   | 509 -----------------
 .../ftc/teamcode/TwoDeadWheelLocalizer.kt     | 138 -----
 .../ftc/teamcode/tuning/LocalizationTest.kt   |  79 ---
 .../teamcode/tuning/ManualFeedbackTuner.kt    |  66 ---
 .../auto/blue/basket/left/Blue1Plus1.kt       | 125 +++++
 .../auto/blue/basket/left/Blue1Plus2.kt       | 197 +++++++
 .../auto/blue/basket/left/Blue1Plus2V2.kt     | 185 +++++++
 .../auto/blue/basket/left/Blue1Plus1.kt       | 140 -----
 .../auto/blue/basket/left/Blue1Plus2.kt       | 212 -------
 .../auto/blue/basket/left/Blue1Plus2V2.kt     | 201 -------
 .../ftc/teamcode/opModes/teleOp/MainTeleOp.kt |   8 +-
 .../teamcode/opModes/tuning/IntakeTuner.kt    |   2 +-
 .../opModes/tuning/slides/SlidesPIDTuner.kt   |   6 +-
 .../ftc/teamcode/roadrunner}/Drawing.kt       |   2 +-
 .../ftc/teamcode/roadrunner}/Localizer.kt     |   2 +-
 .../roadrunner}/ThreeDeadWheelLocalizer.kt    |  22 +-
 .../messages/DriveCommandMessage.kt           |   2 +-
 .../messages/MecanumCommandMessage.kt         |   2 +-
 .../messages/MecanumLocalizerInputsMessage.kt |   2 +-
 .../roadrunner}/messages/PoseMessage.kt       |   2 +-
 .../messages/TankCommandMessage.kt            |   2 +-
 .../messages/TankLocalizerInputsMessage.kt    |   2 +-
 .../messages/ThreeDeadWheelInputsMessage.kt   |   2 +-
 .../messages/TwoDeadWheelInputsMessage.kt     |   2 +-
 .../roadrunner/tuning/LocalizationTest.kt     |  55 ++
 .../roadrunner/tuning/ManualFeedbackTuner.kt  |  44 ++
 .../teamcode/roadrunner}/tuning/SplineTest.kt |  22 +-
 .../roadrunner}/tuning/TuningOpModes.kt       | 128 ++---
 .../subsystems/arm/OpenArmSubsystem.kt        |   3 +-
 .../subsystems/drive/DriveSubsystem.kt        | 524 +++++++++++++++++-
 .../slides/OpenElevatorSubsystem.kt           |  31 +-
 .../subsystems/slides/OpenSlidesSubsystem.kt  |  42 --
 33 files changed, 1218 insertions(+), 2056 deletions(-)
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.kt
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.kt
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoDeadWheelLocalizer.kt
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.kt
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedbackTuner.kt
 create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/blue/basket/left/Blue1Plus1.kt
 create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/blue/basket/left/Blue1Plus2.kt
 create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/blue/basket/left/Blue1Plus2V2.kt
 delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/Blue1Plus1.kt
 delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/Blue1Plus2.kt
 delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/Blue1Plus2V2.kt
 rename TeamCode/src/main/{java/org/firstinspires/ftc/teamcode => kotlin/org/firstinspires/ftc/teamcode/roadrunner}/Drawing.kt (90%)
 rename TeamCode/src/main/{java/org/firstinspires/ftc/teamcode => kotlin/org/firstinspires/ftc/teamcode/roadrunner}/Localizer.kt (74%)
 rename TeamCode/src/main/{java/org/firstinspires/ftc/teamcode => kotlin/org/firstinspires/ftc/teamcode/roadrunner}/ThreeDeadWheelLocalizer.kt (89%)
 rename TeamCode/src/main/{java/org/firstinspires/ftc/teamcode => kotlin/org/firstinspires/ftc/teamcode/roadrunner}/messages/DriveCommandMessage.kt (90%)
 rename TeamCode/src/main/{java/org/firstinspires/ftc/teamcode => kotlin/org/firstinspires/ftc/teamcode/roadrunner}/messages/MecanumCommandMessage.kt (79%)
 rename TeamCode/src/main/{java/org/firstinspires/ftc/teamcode => kotlin/org/firstinspires/ftc/teamcode/roadrunner}/messages/MecanumLocalizerInputsMessage.kt (93%)
 rename TeamCode/src/main/{java/org/firstinspires/ftc/teamcode => kotlin/org/firstinspires/ftc/teamcode/roadrunner}/messages/PoseMessage.kt (80%)
 rename TeamCode/src/main/{java/org/firstinspires/ftc/teamcode => kotlin/org/firstinspires/ftc/teamcode/roadrunner}/messages/TankCommandMessage.kt (70%)
 rename TeamCode/src/main/{java/org/firstinspires/ftc/teamcode => kotlin/org/firstinspires/ftc/teamcode/roadrunner}/messages/TankLocalizerInputsMessage.kt (87%)
 rename TeamCode/src/main/{java/org/firstinspires/ftc/teamcode => kotlin/org/firstinspires/ftc/teamcode/roadrunner}/messages/ThreeDeadWheelInputsMessage.kt (81%)
 rename TeamCode/src/main/{java/org/firstinspires/ftc/teamcode => kotlin/org/firstinspires/ftc/teamcode/roadrunner}/messages/TwoDeadWheelInputsMessage.kt (95%)
 create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/tuning/LocalizationTest.kt
 create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/tuning/ManualFeedbackTuner.kt
 rename TeamCode/src/main/{java/org/firstinspires/ftc/teamcode => kotlin/org/firstinspires/ftc/teamcode/roadrunner}/tuning/SplineTest.kt (54%)
 rename TeamCode/src/main/{java/org/firstinspires/ftc/teamcode => kotlin/org/firstinspires/ftc/teamcode/roadrunner}/tuning/TuningOpModes.kt (56%)
 delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/OpenSlidesSubsystem.kt

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.kt
deleted file mode 100644
index 0f3263e..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.kt
+++ /dev/null
@@ -1,515 +0,0 @@
-package org.firstinspires.ftc.teamcode
-
-import com.acmerobotics.dashboard.canvas.Canvas
-import com.acmerobotics.dashboard.config.Config
-import com.acmerobotics.dashboard.telemetry.TelemetryPacket
-import com.acmerobotics.roadrunner.AccelConstraint
-import com.acmerobotics.roadrunner.Action
-import com.acmerobotics.roadrunner.AngularVelConstraint
-import com.acmerobotics.roadrunner.DualNum
-import com.acmerobotics.roadrunner.HolonomicController
-import com.acmerobotics.roadrunner.MecanumKinematics
-import com.acmerobotics.roadrunner.MecanumKinematics.WheelIncrements
-import com.acmerobotics.roadrunner.MinVelConstraint
-import com.acmerobotics.roadrunner.MotorFeedforward
-import com.acmerobotics.roadrunner.Pose2d
-import com.acmerobotics.roadrunner.PoseVelocity2d
-import com.acmerobotics.roadrunner.PoseVelocity2dDual
-import com.acmerobotics.roadrunner.ProfileAccelConstraint
-import com.acmerobotics.roadrunner.ProfileParams
-import com.acmerobotics.roadrunner.Rotation2d
-import com.acmerobotics.roadrunner.Time
-import com.acmerobotics.roadrunner.TimeTrajectory
-import com.acmerobotics.roadrunner.TimeTurn
-import com.acmerobotics.roadrunner.TrajectoryActionBuilder
-import com.acmerobotics.roadrunner.TrajectoryBuilderParams
-import com.acmerobotics.roadrunner.TurnConstraints
-import com.acmerobotics.roadrunner.Twist2dDual
-import com.acmerobotics.roadrunner.Vector2d
-import com.acmerobotics.roadrunner.Vector2dDual
-import com.acmerobotics.roadrunner.VelConstraint
-import com.acmerobotics.roadrunner.ftc.DownsampledWriter
-import com.acmerobotics.roadrunner.ftc.Encoder
-import com.acmerobotics.roadrunner.ftc.FlightRecorder.write
-import com.acmerobotics.roadrunner.ftc.LazyImu
-import com.acmerobotics.roadrunner.ftc.OverflowEncoder
-import com.acmerobotics.roadrunner.ftc.RawEncoder
-import com.acmerobotics.roadrunner.ftc.throwIfModulesAreOutdated
-import com.acmerobotics.roadrunner.now
-import com.acmerobotics.roadrunner.range
-import com.qualcomm.hardware.lynx.LynxModule
-import com.qualcomm.hardware.rev.RevHubOrientationOnRobot
-import com.qualcomm.hardware.rev.RevHubOrientationOnRobot.UsbFacingDirection
-import com.qualcomm.robotcore.hardware.DcMotor
-import com.qualcomm.robotcore.hardware.DcMotorEx
-import com.qualcomm.robotcore.hardware.HardwareMap
-import com.qualcomm.robotcore.hardware.IMU
-import com.qualcomm.robotcore.hardware.VoltageSensor
-import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit
-import org.firstinspires.ftc.teamcode.messages.DriveCommandMessage
-import org.firstinspires.ftc.teamcode.messages.MecanumCommandMessage
-import org.firstinspires.ftc.teamcode.messages.MecanumLocalizerInputsMessage
-import org.firstinspires.ftc.teamcode.messages.PoseMessage
-import java.util.LinkedList
-import kotlin.math.ceil
-import kotlin.math.max
-
-@Config
-class MecanumDrive(hardwareMap: HardwareMap, var pose: Pose2d) {
-    class Params {
-        // IMU orientation
-        // TODO: fill in these values based on
-        //   see https://ftc-docs.firstinspires.org/en/latest/programming_resources/imu/imu.html?highlight=imu#physical-hub-mounting
-        var logoFacingDirection: RevHubOrientationOnRobot.LogoFacingDirection =
-            RevHubOrientationOnRobot.LogoFacingDirection.UP
-        var usbFacingDirection: UsbFacingDirection = UsbFacingDirection.FORWARD
-
-        // drive model parameters
-        var inPerTick: Double = 1.0
-        var lateralInPerTick: Double = inPerTick
-        var trackWidthTicks: Double = 0.0
-
-        // feedforward parameters (in tick units)
-        var kS: Double = 0.0
-        var kV: Double = 0.0
-        var kA: Double = 0.0
-
-        // path profile parameters (in inches)
-        var maxWheelVel: Double = 50.0
-        var minProfileAccel: Double = -30.0
-        var maxProfileAccel: Double = 50.0
-
-        // turn profile parameters (in radians)
-        var maxAngVel: Double = Math.PI // shared with path
-        var maxAngAccel: Double = Math.PI
-
-        // path controller gains
-        var axialGain: Double = 0.0
-        var lateralGain: Double = 0.0
-        var headingGain: Double = 0.0 // shared with turn
-
-        var axialVelGain: Double = 0.0
-        var lateralVelGain: Double = 0.0
-        var headingVelGain: Double = 0.0 // shared with turn
-    }
-
-    val kinematics: MecanumKinematics = MecanumKinematics(
-        PARAMS.inPerTick * PARAMS.trackWidthTicks, PARAMS.inPerTick / PARAMS.lateralInPerTick
-    )
-
-    val defaultTurnConstraints: TurnConstraints = TurnConstraints(
-        PARAMS.maxAngVel, -PARAMS.maxAngAccel, PARAMS.maxAngAccel
-    )
-    val defaultVelConstraint: VelConstraint = MinVelConstraint(
-        listOf(
-            kinematics.WheelVelConstraint(PARAMS.maxWheelVel),
-            AngularVelConstraint(PARAMS.maxAngVel)
-        )
-    )
-    val defaultAccelConstraint: AccelConstraint =
-        ProfileAccelConstraint(PARAMS.minProfileAccel, PARAMS.maxProfileAccel)
-
-    val leftFront: DcMotorEx
-    val leftBack: DcMotorEx
-    val rightBack: DcMotorEx
-    val rightFront: DcMotorEx
-
-    val voltageSensor: VoltageSensor
-
-    val lazyImu: LazyImu
-
-    val localizer: Localizer
-
-    private val poseHistory = LinkedList<Pose2d>()
-
-    private val estimatedPoseWriter = DownsampledWriter("ESTIMATED_POSE", 50000000)
-    private val targetPoseWriter = DownsampledWriter("TARGET_POSE", 50000000)
-    private val driveCommandWriter = DownsampledWriter("DRIVE_COMMAND", 50000000)
-    private val mecanumCommandWriter = DownsampledWriter("MECANUM_COMMAND", 50000000)
-
-    inner class DriveLocalizer : Localizer {
-        val leftFront: Encoder =
-            OverflowEncoder(RawEncoder(this@MecanumDrive.leftFront))
-        val leftBack: Encoder =
-            OverflowEncoder(RawEncoder(this@MecanumDrive.leftBack))
-        val rightBack: Encoder =
-            OverflowEncoder(RawEncoder(this@MecanumDrive.rightBack))
-        val rightFront: Encoder =
-            OverflowEncoder(RawEncoder(this@MecanumDrive.rightFront))
-        val imu: IMU = lazyImu.get()
-
-        private var lastLeftFrontPos = 0
-        private var lastLeftBackPos = 0
-        private var lastRightBackPos = 0
-        private var lastRightFrontPos = 0
-        private var lastHeading: Rotation2d? = null
-        private var initialized = false
-
-        init {
-            // TODO: reverse encoders if needed
-            //   leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
-        }
-
-        override fun update(): Twist2dDual<Time> {
-            val leftFrontPosVel = leftFront.getPositionAndVelocity()
-            val leftBackPosVel = leftBack.getPositionAndVelocity()
-            val rightBackPosVel = rightBack.getPositionAndVelocity()
-            val rightFrontPosVel = rightFront.getPositionAndVelocity()
-
-            val angles = imu.robotYawPitchRollAngles
-
-            write(
-                "MECANUM_LOCALIZER_INPUTS", MecanumLocalizerInputsMessage(
-                    leftFrontPosVel, leftBackPosVel, rightBackPosVel, rightFrontPosVel, angles
-                )
-            )
-
-            val heading = Rotation2d.exp(angles.getYaw(AngleUnit.RADIANS))
-
-            if (!initialized) {
-                initialized = true
-
-                lastLeftFrontPos = leftFrontPosVel.position
-                lastLeftBackPos = leftBackPosVel.position
-                lastRightBackPos = rightBackPosVel.position
-                lastRightFrontPos = rightFrontPosVel.position
-
-                lastHeading = heading
-
-                return Twist2dDual(
-                    Vector2dDual.constant(Vector2d(0.0, 0.0), 2),
-                    DualNum.constant(0.0, 2)
-                )
-            }
-
-            val headingDelta = heading.minus(lastHeading!!)
-            val twist = kinematics.forward(
-                WheelIncrements(
-                    DualNum<Time>(
-                        doubleArrayOf(
-                            (leftFrontPosVel.position - lastLeftFrontPos).toDouble(),
-                            leftFrontPosVel.velocity.toDouble(),
-                        )
-                    ).times(PARAMS.inPerTick),
-                    DualNum<Time>(
-                        doubleArrayOf(
-                            (leftBackPosVel.position - lastLeftBackPos).toDouble(),
-                            leftBackPosVel.velocity.toDouble(),
-                        )
-                    ).times(PARAMS.inPerTick),
-                    DualNum<Time>(
-                        doubleArrayOf(
-                            (rightBackPosVel.position - lastRightBackPos).toDouble(),
-                            rightBackPosVel.velocity.toDouble(),
-                        )
-                    ).times(PARAMS.inPerTick),
-                    DualNum<Time>(
-                        doubleArrayOf(
-                            (rightFrontPosVel.position - lastRightFrontPos).toDouble(),
-                            rightFrontPosVel.velocity.toDouble(),
-                        )
-                    ).times(PARAMS.inPerTick)
-                )
-            )
-
-            lastLeftFrontPos = leftFrontPosVel.position
-            lastLeftBackPos = leftBackPosVel.position
-            lastRightBackPos = rightBackPosVel.position
-            lastRightFrontPos = rightFrontPosVel.position
-
-            lastHeading = heading
-
-            return Twist2dDual(
-                twist.line,
-                DualNum.cons(headingDelta, twist.angle.drop(1))
-            )
-        }
-    }
-
-    init {
-        throwIfModulesAreOutdated(hardwareMap)
-
-        for (module in hardwareMap.getAll(LynxModule::class.java)) {
-            module.bulkCachingMode = LynxModule.BulkCachingMode.AUTO
-        }
-
-        // TODO: make sure your config has motors with these names (or change them)
-        //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
-        leftFront = hardwareMap.get(DcMotorEx::class.java, "leftFront")
-        leftBack = hardwareMap.get(DcMotorEx::class.java, "leftBack")
-        rightBack = hardwareMap.get(DcMotorEx::class.java, "rightBack")
-        rightFront = hardwareMap.get(DcMotorEx::class.java, "rightFront")
-
-        leftFront.zeroPowerBehavior = DcMotor.ZeroPowerBehavior.BRAKE
-        leftBack.zeroPowerBehavior = DcMotor.ZeroPowerBehavior.BRAKE
-        rightBack.zeroPowerBehavior = DcMotor.ZeroPowerBehavior.BRAKE
-        rightFront.zeroPowerBehavior = DcMotor.ZeroPowerBehavior.BRAKE
-
-        // TODO: reverse motor directions if needed
-        //   leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
-
-        // TODO: make sure your config has an IMU with this name (can be BNO or BHI)
-        //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
-        lazyImu = LazyImu(
-            hardwareMap, "imu", RevHubOrientationOnRobot(
-                PARAMS.logoFacingDirection, PARAMS.usbFacingDirection
-            )
-        )
-
-        voltageSensor = hardwareMap.voltageSensor.iterator().next()
-
-        localizer = DriveLocalizer()
-
-        write("MECANUM_PARAMS", PARAMS)
-    }
-
-    fun setDrivePowers(powers: PoseVelocity2d) {
-        val wheelVels: MecanumKinematics.WheelVelocities<Time> = MecanumKinematics(1.0).inverse(
-            PoseVelocity2dDual.constant(powers, 1)
-        )
-
-        var maxPowerMag = 1.0
-        for (power in wheelVels.all()) {
-            maxPowerMag = max(maxPowerMag, power.value())
-        }
-
-        leftFront.power = wheelVels.leftFront[0] / maxPowerMag
-        leftBack.power = wheelVels.leftBack[0] / maxPowerMag
-        rightBack.power = wheelVels.rightBack[0] / maxPowerMag
-        rightFront.power = wheelVels.rightFront[0] / maxPowerMag
-    }
-
-    inner class FollowTrajectoryAction(val timeTrajectory: TimeTrajectory) : Action {
-        private var beginTs = -1.0
-
-        private val xPoints: DoubleArray
-        private val yPoints: DoubleArray
-
-        init {
-            val disps = range(
-                0.0, timeTrajectory.path.length(),
-                max(
-                    2.0,
-                    (ceil(timeTrajectory.path.length() / 2).toInt()).toDouble()
-                ).toInt()
-            )
-            xPoints = DoubleArray(disps.size)
-            yPoints = DoubleArray(disps.size)
-            for (i in disps.indices) {
-                val p = timeTrajectory.path[disps[i], 1].value()
-                xPoints[i] = p.position.x
-                yPoints[i] = p.position.y
-            }
-        }
-
-        override fun run(p: TelemetryPacket): Boolean {
-            val t: Double
-            if (beginTs < 0) {
-                beginTs = now()
-                t = 0.0
-            } else {
-                t = now() - beginTs
-            }
-
-            if (t >= timeTrajectory.duration) {
-                leftFront.power = 0.0
-                leftBack.power = 0.0
-                rightBack.power = 0.0
-                rightFront.power = 0.0
-
-                return false
-            }
-
-            val txWorldTarget = timeTrajectory[t]
-            targetPoseWriter.write(PoseMessage(txWorldTarget.value()))
-
-            val robotVelRobot = updatePoseEstimate()
-
-            val command: PoseVelocity2dDual<Time> = HolonomicController(
-                PARAMS.axialGain, PARAMS.lateralGain, PARAMS.headingGain,
-                PARAMS.axialVelGain, PARAMS.lateralVelGain, PARAMS.headingVelGain
-            )
-                .compute(txWorldTarget, pose, robotVelRobot)
-            driveCommandWriter.write(DriveCommandMessage(command))
-
-            val wheelVels: MecanumKinematics.WheelVelocities<Time> = kinematics.inverse(command)
-            val voltage = voltageSensor.voltage
-
-            val feedforward = MotorFeedforward(
-                PARAMS.kS,
-                PARAMS.kV / PARAMS.inPerTick, PARAMS.kA / PARAMS.inPerTick
-            )
-            val leftFrontPower = feedforward.compute(wheelVels.leftFront) / voltage
-            val leftBackPower = feedforward.compute(wheelVels.leftBack) / voltage
-            val rightBackPower = feedforward.compute(wheelVels.rightBack) / voltage
-            val rightFrontPower = feedforward.compute(wheelVels.rightFront) / voltage
-            mecanumCommandWriter.write(
-                MecanumCommandMessage(
-                    voltage, leftFrontPower, leftBackPower, rightBackPower, rightFrontPower
-                )
-            )
-
-            leftFront.power = leftFrontPower
-            leftBack.power = leftBackPower
-            rightBack.power = rightBackPower
-            rightFront.power = rightFrontPower
-
-            p.put("x", pose.position.x)
-            p.put("y", pose.position.y)
-            p.put("heading (deg)", Math.toDegrees(pose.heading.toDouble()))
-
-            val error = txWorldTarget.value().minusExp(pose)
-            p.put("xError", error.position.x)
-            p.put("yError", error.position.y)
-            p.put("headingError (deg)", Math.toDegrees(error.heading.toDouble()))
-
-            // only draw when active; only one drive action should be active at a time
-            val c = p.fieldOverlay()
-            drawPoseHistory(c)
-
-            c.setStroke("#4CAF50")
-            Drawing.drawRobot(c, txWorldTarget.value())
-
-            c.setStroke("#3F51B5")
-            Drawing.drawRobot(c, pose)
-
-            c.setStroke("#4CAF50FF")
-            c.setStrokeWidth(1)
-            c.strokePolyline(xPoints, yPoints)
-
-            return true
-        }
-
-        override fun preview(fieldOverlay: Canvas) {
-            fieldOverlay.setStroke("#4CAF507A")
-            fieldOverlay.setStrokeWidth(1)
-            fieldOverlay.strokePolyline(xPoints, yPoints)
-        }
-    }
-
-    inner class TurnAction(private val turn: TimeTurn) : Action {
-        private var beginTs = -1.0
-
-        override fun run(p: TelemetryPacket): Boolean {
-            val t: Double
-            if (beginTs < 0) {
-                beginTs = now()
-                t = 0.0
-            } else {
-                t = now() - beginTs
-            }
-
-            if (t >= turn.duration) {
-                leftFront.power = 0.0
-                leftBack.power = 0.0
-                rightBack.power = 0.0
-                rightFront.power = 0.0
-
-                return false
-            }
-
-            val txWorldTarget = turn[t]
-            targetPoseWriter.write(PoseMessage(txWorldTarget.value()))
-
-            val robotVelRobot = updatePoseEstimate()
-
-            val command: PoseVelocity2dDual<Time> = HolonomicController(
-                PARAMS.axialGain, PARAMS.lateralGain, PARAMS.headingGain,
-                PARAMS.axialVelGain, PARAMS.lateralVelGain, PARAMS.headingVelGain
-            )
-                .compute(txWorldTarget, pose, robotVelRobot)
-            driveCommandWriter.write(DriveCommandMessage(command))
-
-            val wheelVels: MecanumKinematics.WheelVelocities<Time> = kinematics.inverse(command)
-            val voltage = voltageSensor.voltage
-            val feedforward = MotorFeedforward(
-                PARAMS.kS,
-                PARAMS.kV / PARAMS.inPerTick, PARAMS.kA / PARAMS.inPerTick
-            )
-            val leftFrontPower = feedforward.compute(wheelVels.leftFront) / voltage
-            val leftBackPower = feedforward.compute(wheelVels.leftBack) / voltage
-            val rightBackPower = feedforward.compute(wheelVels.rightBack) / voltage
-            val rightFrontPower = feedforward.compute(wheelVels.rightFront) / voltage
-            mecanumCommandWriter.write(
-                MecanumCommandMessage(
-                    voltage, leftFrontPower, leftBackPower, rightBackPower, rightFrontPower
-                )
-            )
-
-            leftFront.power = feedforward.compute(wheelVels.leftFront) / voltage
-            leftBack.power = feedforward.compute(wheelVels.leftBack) / voltage
-            rightBack.power = feedforward.compute(wheelVels.rightBack) / voltage
-            rightFront.power = feedforward.compute(wheelVels.rightFront) / voltage
-
-            val c = p.fieldOverlay()
-            drawPoseHistory(c)
-
-            c.setStroke("#4CAF50")
-            Drawing.drawRobot(c, txWorldTarget.value())
-
-            c.setStroke("#3F51B5")
-            Drawing.drawRobot(c, pose)
-
-            c.setStroke("#7C4DFFFF")
-            c.fillCircle(turn.beginPose.position.x, turn.beginPose.position.y, 2.0)
-
-            return true
-        }
-
-        override fun preview(fieldOverlay: Canvas) {
-            fieldOverlay.setStroke("#7C4DFF7A")
-            fieldOverlay.fillCircle(turn.beginPose.position.x, turn.beginPose.position.y, 2.0)
-        }
-    }
-
-    fun updatePoseEstimate(): PoseVelocity2d {
-        val twist = localizer.update()
-        pose = pose.plus(twist.value())
-
-        poseHistory.add(pose)
-        while (poseHistory.size > 100) {
-            poseHistory.removeFirst()
-        }
-
-        estimatedPoseWriter.write(PoseMessage(pose))
-
-        return twist.velocity().value()
-    }
-
-    private fun drawPoseHistory(c: Canvas) {
-        val xPoints = DoubleArray(poseHistory.size)
-        val yPoints = DoubleArray(poseHistory.size)
-
-        var i = 0
-        for ((position) in poseHistory) {
-            xPoints[i] = position.x
-            yPoints[i] = position.y
-
-            i++
-        }
-
-        c.setStrokeWidth(1)
-        c.setStroke("#3F51B5")
-        c.strokePolyline(xPoints, yPoints)
-    }
-
-    fun actionBuilder(beginPose: Pose2d): TrajectoryActionBuilder {
-        return TrajectoryActionBuilder(
-            { turn: TimeTurn -> TurnAction(turn) },
-            { t: TimeTrajectory -> FollowTrajectoryAction(t) },
-            TrajectoryBuilderParams(
-                1e-6,
-                ProfileParams(
-                    0.25, 0.1, 1e-2
-                )
-            ),
-            beginPose, 0.0,
-            defaultTurnConstraints,
-            defaultVelConstraint, defaultAccelConstraint
-        )
-    }
-
-    companion object {
-        var PARAMS: Params = Params()
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.kt
deleted file mode 100644
index 65a0588..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.kt
+++ /dev/null
@@ -1,509 +0,0 @@
-package org.firstinspires.ftc.teamcode
-
-import com.acmerobotics.dashboard.canvas.Canvas
-import com.acmerobotics.dashboard.config.Config
-import com.acmerobotics.dashboard.telemetry.TelemetryPacket
-import com.acmerobotics.roadrunner.AccelConstraint
-import com.acmerobotics.roadrunner.Action
-import com.acmerobotics.roadrunner.AngularVelConstraint
-import com.acmerobotics.roadrunner.DualNum
-import com.acmerobotics.roadrunner.MinVelConstraint
-import com.acmerobotics.roadrunner.MotorFeedforward
-import com.acmerobotics.roadrunner.Pose2d
-import com.acmerobotics.roadrunner.Pose2dDual
-import com.acmerobotics.roadrunner.PoseVelocity2d
-import com.acmerobotics.roadrunner.PoseVelocity2dDual
-import com.acmerobotics.roadrunner.ProfileAccelConstraint
-import com.acmerobotics.roadrunner.ProfileParams
-import com.acmerobotics.roadrunner.RamseteController
-import com.acmerobotics.roadrunner.TankKinematics
-import com.acmerobotics.roadrunner.TankKinematics.WheelIncrements
-import com.acmerobotics.roadrunner.TankKinematics.WheelVelocities
-import com.acmerobotics.roadrunner.Time
-import com.acmerobotics.roadrunner.TimeTrajectory
-import com.acmerobotics.roadrunner.TimeTurn
-import com.acmerobotics.roadrunner.TrajectoryActionBuilder
-import com.acmerobotics.roadrunner.TrajectoryBuilderParams
-import com.acmerobotics.roadrunner.TurnConstraints
-import com.acmerobotics.roadrunner.Twist2dDual
-import com.acmerobotics.roadrunner.Vector2d
-import com.acmerobotics.roadrunner.Vector2dDual
-import com.acmerobotics.roadrunner.VelConstraint
-import com.acmerobotics.roadrunner.ftc.DownsampledWriter
-import com.acmerobotics.roadrunner.ftc.Encoder
-import com.acmerobotics.roadrunner.ftc.FlightRecorder.write
-import com.acmerobotics.roadrunner.ftc.LazyImu
-import com.acmerobotics.roadrunner.ftc.OverflowEncoder
-import com.acmerobotics.roadrunner.ftc.PositionVelocityPair
-import com.acmerobotics.roadrunner.ftc.RawEncoder
-import com.acmerobotics.roadrunner.ftc.throwIfModulesAreOutdated
-import com.acmerobotics.roadrunner.now
-import com.acmerobotics.roadrunner.range
-import com.qualcomm.hardware.lynx.LynxModule
-import com.qualcomm.hardware.rev.RevHubOrientationOnRobot
-import com.qualcomm.hardware.rev.RevHubOrientationOnRobot.UsbFacingDirection
-import com.qualcomm.robotcore.hardware.DcMotor
-import com.qualcomm.robotcore.hardware.DcMotorEx
-import com.qualcomm.robotcore.hardware.HardwareMap
-import com.qualcomm.robotcore.hardware.VoltageSensor
-import org.firstinspires.ftc.teamcode.messages.DriveCommandMessage
-import org.firstinspires.ftc.teamcode.messages.PoseMessage
-import org.firstinspires.ftc.teamcode.messages.TankCommandMessage
-import org.firstinspires.ftc.teamcode.messages.TankLocalizerInputsMessage
-import java.util.Arrays
-import java.util.Collections
-import java.util.LinkedList
-import kotlin.math.ceil
-import kotlin.math.max
-
-@Config
-class TankDrive(hardwareMap: HardwareMap, var pose: Pose2d) {
-    class Params {
-        // IMU orientation
-        // TODO: fill in these values based on
-        //   see https://ftc-docs.firstinspires.org/en/latest/programming_resources/imu/imu.html?highlight=imu#physical-hub-mounting
-        var logoFacingDirection: RevHubOrientationOnRobot.LogoFacingDirection =
-            RevHubOrientationOnRobot.LogoFacingDirection.UP
-        var usbFacingDirection: UsbFacingDirection = UsbFacingDirection.FORWARD
-
-        // drive model parameters
-        var inPerTick: Double = 0.0
-        var trackWidthTicks: Double = 0.0
-
-        // feedforward parameters (in tick units)
-        var kS: Double = 0.0
-        var kV: Double = 0.0
-        var kA: Double = 0.0
-
-        // path profile parameters (in inches)
-        var maxWheelVel: Double = 50.0
-        var minProfileAccel: Double = -30.0
-        var maxProfileAccel: Double = 50.0
-
-        // turn profile parameters (in radians)
-        var maxAngVel: Double = Math.PI // shared with path
-        var maxAngAccel: Double = Math.PI
-
-        // path controller gains
-        var ramseteZeta: Double = 0.7 // in the range (0, 1)
-        var ramseteBBar: Double = 2.0 // positive
-
-        // turn controller gains
-        var turnGain: Double = 0.0
-        var turnVelGain: Double = 0.0
-    }
-
-    val kinematics: TankKinematics = TankKinematics(PARAMS.inPerTick * PARAMS.trackWidthTicks)
-
-    val defaultTurnConstraints: TurnConstraints = TurnConstraints(
-        PARAMS.maxAngVel, -PARAMS.maxAngAccel, PARAMS.maxAngAccel
-    )
-    val defaultVelConstraint: VelConstraint = MinVelConstraint(
-        Arrays.asList(
-            kinematics.WheelVelConstraint(PARAMS.maxWheelVel),
-            AngularVelConstraint(PARAMS.maxAngVel)
-        )
-    )
-    val defaultAccelConstraint: AccelConstraint =
-        ProfileAccelConstraint(PARAMS.minProfileAccel, PARAMS.maxProfileAccel)
-
-    val leftMotors: List<DcMotorEx>
-    val rightMotors: List<DcMotorEx>
-
-    val lazyImu: LazyImu
-
-    val voltageSensor: VoltageSensor
-
-    val localizer: Localizer
-
-    private val poseHistory = LinkedList<Pose2d>()
-
-    private val estimatedPoseWriter = DownsampledWriter("ESTIMATED_POSE", 50000000)
-    private val targetPoseWriter = DownsampledWriter("TARGET_POSE", 50000000)
-    private val driveCommandWriter = DownsampledWriter("DRIVE_COMMAND", 50000000)
-
-    private val tankCommandWriter = DownsampledWriter("TANK_COMMAND", 50000000)
-
-    inner class DriveLocalizer : Localizer {
-        lateinit var leftEncs: List<Encoder>
-        lateinit var rightEncs: List<Encoder>
-
-        private var lastLeftPos = 0.0
-        private var lastRightPos = 0.0
-        private var initialized = false
-
-        init {
-            run {
-                val leftEncs: MutableList<Encoder> = ArrayList()
-                for (m in leftMotors) {
-                    val e: Encoder = OverflowEncoder(RawEncoder(m))
-                    leftEncs.add(e)
-                }
-                this.leftEncs = Collections.unmodifiableList(leftEncs)
-            }
-
-            run {
-                val rightEncs: MutableList<Encoder> = ArrayList()
-                for (m in rightMotors) {
-                    val e: Encoder = OverflowEncoder(RawEncoder(m))
-                    rightEncs.add(e)
-                }
-                this.rightEncs = Collections.unmodifiableList(rightEncs)
-            }
-
-            // TODO: reverse encoder directions if needed
-            //   leftEncs.get(0).setDirection(DcMotorSimple.Direction.REVERSE);
-        }
-
-        override fun update(): Twist2dDual<Time> {
-            val leftReadings: MutableList<PositionVelocityPair> = ArrayList()
-            val rightReadings: MutableList<PositionVelocityPair> = ArrayList()
-            var meanLeftPos = 0.0
-            var meanLeftVel = 0.0
-            for (e in leftEncs) {
-                val p = e.getPositionAndVelocity()
-                meanLeftPos += p.position.toDouble()
-                meanLeftVel += p.velocity.toDouble()
-                leftReadings.add(p)
-            }
-            meanLeftPos /= leftEncs.size.toDouble()
-            meanLeftVel /= leftEncs.size.toDouble()
-
-            var meanRightPos = 0.0
-            var meanRightVel = 0.0
-            for (e in rightEncs) {
-                val p = e.getPositionAndVelocity()
-                meanRightPos += p.position.toDouble()
-                meanRightVel += p.velocity.toDouble()
-                rightReadings.add(p)
-            }
-            meanRightPos /= rightEncs.size.toDouble()
-            meanRightVel /= rightEncs.size.toDouble()
-
-            write(
-                "TANK_LOCALIZER_INPUTS",
-                TankLocalizerInputsMessage(leftReadings, rightReadings)
-            )
-
-            if (!initialized) {
-                initialized = true
-
-                lastLeftPos = meanLeftPos
-                lastRightPos = meanRightPos
-
-                return Twist2dDual(
-                    Vector2dDual.constant(Vector2d(0.0, 0.0), 2),
-                    DualNum.constant(0.0, 2)
-                )
-            }
-
-            val twist: WheelIncrements<Time> = WheelIncrements(
-                DualNum<Time>(
-                    doubleArrayOf(
-                        meanLeftPos - lastLeftPos,
-                        meanLeftVel
-                    )
-                ).times(PARAMS.inPerTick),
-                DualNum<Time>(
-                    doubleArrayOf(
-                        meanRightPos - lastRightPos,
-                        meanRightVel,
-                    )
-                ).times(PARAMS.inPerTick)
-            )
-
-            lastLeftPos = meanLeftPos
-            lastRightPos = meanRightPos
-
-            return kinematics.forward(twist)
-        }
-    }
-
-    init {
-        throwIfModulesAreOutdated(hardwareMap)
-
-        for (module in hardwareMap.getAll(LynxModule::class.java)) {
-            module.bulkCachingMode = LynxModule.BulkCachingMode.AUTO
-        }
-
-        // TODO: make sure your config has motors with these names (or change them)
-        //   add additional motors on each side if you have them
-        //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
-        leftMotors = Arrays.asList(hardwareMap.get(DcMotorEx::class.java, "left"))
-        rightMotors = Arrays.asList(hardwareMap.get(DcMotorEx::class.java, "right"))
-
-        for (m in leftMotors) {
-            m.zeroPowerBehavior = DcMotor.ZeroPowerBehavior.BRAKE
-        }
-        for (m in rightMotors) {
-            m.zeroPowerBehavior = DcMotor.ZeroPowerBehavior.BRAKE
-        }
-
-        // TODO: reverse motor directions if needed
-        //   leftMotors.get(0).setDirection(DcMotorSimple.Direction.REVERSE);
-
-        // TODO: make sure your config has an IMU with this name (can be BNO or BHI)
-        //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
-        lazyImu = LazyImu(
-            hardwareMap, "imu", RevHubOrientationOnRobot(
-                PARAMS.logoFacingDirection, PARAMS.usbFacingDirection
-            )
-        )
-
-        voltageSensor = hardwareMap.voltageSensor.iterator().next()
-
-        localizer = DriveLocalizer()
-
-        write("TANK_PARAMS", PARAMS)
-    }
-
-    fun setDrivePowers(powers: PoseVelocity2d) {
-        val wheelVels: WheelVelocities<Time> = TankKinematics(2.0).inverse(
-            PoseVelocity2dDual.constant(powers, 1)
-        )
-
-        var maxPowerMag = 1.0
-        for (power in wheelVels.all()) {
-            maxPowerMag = max(maxPowerMag, power.value())
-        }
-
-        for (m in leftMotors) {
-            m.power = wheelVels.left.get(0) / maxPowerMag
-        }
-        for (m in rightMotors) {
-            m.power = wheelVels.right.get(0) / maxPowerMag
-        }
-    }
-
-    inner class FollowTrajectoryAction(val timeTrajectory: TimeTrajectory) : Action {
-        private var beginTs = -1.0
-
-        private val xPoints: DoubleArray
-        private val yPoints: DoubleArray
-
-        init {
-            val disps = range(
-                0.0, timeTrajectory.path.length(),
-                max(
-                    2.0,
-                    (ceil(timeTrajectory.path.length() / 2).toInt()).toDouble()
-                ).toInt()
-            )
-            xPoints = DoubleArray(disps.size)
-            yPoints = DoubleArray(disps.size)
-            for (i in disps.indices) {
-                val p = timeTrajectory.path[disps[i], 1].value()
-                xPoints[i] = p.position.x
-                yPoints[i] = p.position.y
-            }
-        }
-
-        override fun run(p: TelemetryPacket): Boolean {
-            val t: Double
-            if (beginTs < 0) {
-                beginTs = now()
-                t = 0.0
-            } else {
-                t = now() - beginTs
-            }
-
-            if (t >= timeTrajectory.duration) {
-                for (m in leftMotors) {
-                    m.power = 0.0
-                }
-                for (m in rightMotors) {
-                    m.power = 0.0
-                }
-
-                return false
-            }
-
-            val x = timeTrajectory.profile[t]
-
-            val txWorldTarget = timeTrajectory.path[x.value(), 3]
-            targetPoseWriter.write(PoseMessage(txWorldTarget.value()))
-
-            updatePoseEstimate()
-
-            val command: PoseVelocity2dDual<Time> =
-                RamseteController(kinematics.trackWidth, PARAMS.ramseteZeta, PARAMS.ramseteBBar)
-                    .compute(x, txWorldTarget, pose)
-            driveCommandWriter.write(DriveCommandMessage(command))
-
-            val wheelVels: WheelVelocities<Time> = kinematics.inverse(command)
-            val voltage = voltageSensor.voltage
-            val feedforward = MotorFeedforward(
-                PARAMS.kS,
-                PARAMS.kV / PARAMS.inPerTick, PARAMS.kA / PARAMS.inPerTick
-            )
-            val leftPower = feedforward.compute(wheelVels.left) / voltage
-            val rightPower = feedforward.compute(wheelVels.right) / voltage
-            tankCommandWriter.write(TankCommandMessage(voltage, leftPower, rightPower))
-
-            for (m in leftMotors) {
-                m.power = leftPower
-            }
-            for (m in rightMotors) {
-                m.power = rightPower
-            }
-
-            p.put("x", pose.position.x)
-            p.put("y", pose.position.y)
-            p.put("heading (deg)", Math.toDegrees(pose.heading.toDouble()))
-
-            val error = txWorldTarget.value().minusExp(pose)
-            p.put("xError", error.position.x)
-            p.put("yError", error.position.y)
-            p.put("headingError (deg)", Math.toDegrees(error.heading.toDouble()))
-
-            // only draw when active; only one drive action should be active at a time
-            val c = p.fieldOverlay()
-            drawPoseHistory(c)
-
-            c.setStroke("#4CAF50")
-            Drawing.drawRobot(c, txWorldTarget.value())
-
-            c.setStroke("#3F51B5")
-            Drawing.drawRobot(c, pose)
-
-            c.setStroke("#4CAF50FF")
-            c.setStrokeWidth(1)
-            c.strokePolyline(xPoints, yPoints)
-
-            return true
-        }
-
-        override fun preview(fieldOverlay: Canvas) {
-            fieldOverlay.setStroke("#4CAF507A")
-            fieldOverlay.setStrokeWidth(1)
-            fieldOverlay.strokePolyline(xPoints, yPoints)
-        }
-    }
-
-    inner class TurnAction(private val turn: TimeTurn) : Action {
-        private var beginTs = -1.0
-
-        override fun run(p: TelemetryPacket): Boolean {
-            val t: Double
-            if (beginTs < 0) {
-                beginTs = now()
-                t = 0.0
-            } else {
-                t = now() - beginTs
-            }
-
-            if (t >= turn.duration) {
-                for (m in leftMotors) {
-                    m.power = 0.0
-                }
-                for (m in rightMotors) {
-                    m.power = 0.0
-                }
-
-                return false
-            }
-
-            val txWorldTarget: Pose2dDual<Time> = turn[t]
-            targetPoseWriter.write(PoseMessage(txWorldTarget.value()))
-
-            val robotVelRobot = updatePoseEstimate()
-
-            val command = PoseVelocity2dDual(
-                Vector2dDual.constant(Vector2d(0.0, 0.0), 3),
-                txWorldTarget.heading.velocity().plus(
-                    PARAMS.turnGain * pose.heading.minus(txWorldTarget.heading.value()) +
-                            PARAMS.turnVelGain * (robotVelRobot.angVel - txWorldTarget.heading.velocity()
-                        .value())
-                )
-            )
-            driveCommandWriter.write(DriveCommandMessage(command))
-
-            val wheelVels: WheelVelocities<Time> = kinematics.inverse(command)
-            val voltage = voltageSensor.voltage
-            val feedforward = MotorFeedforward(
-                PARAMS.kS,
-                PARAMS.kV / PARAMS.inPerTick, PARAMS.kA / PARAMS.inPerTick
-            )
-            val leftPower = feedforward.compute(wheelVels.left) / voltage
-            val rightPower = feedforward.compute(wheelVels.right) / voltage
-            tankCommandWriter.write(TankCommandMessage(voltage, leftPower, rightPower))
-
-            for (m in leftMotors) {
-                m.power = leftPower
-            }
-            for (m in rightMotors) {
-                m.power = rightPower
-            }
-
-            val c = p.fieldOverlay()
-            drawPoseHistory(c)
-
-            c.setStroke("#4CAF50")
-            Drawing.drawRobot(c, txWorldTarget.value())
-
-            c.setStroke("#3F51B5")
-            Drawing.drawRobot(c, pose)
-
-            c.setStroke("#7C4DFFFF")
-            c.fillCircle(turn.beginPose.position.x, turn.beginPose.position.y, 2.0)
-
-            return true
-        }
-
-        override fun preview(fieldOverlay: Canvas) {
-            fieldOverlay.setStroke("#7C4DFF7A")
-            fieldOverlay.fillCircle(turn.beginPose.position.x, turn.beginPose.position.y, 2.0)
-        }
-    }
-
-    fun updatePoseEstimate(): PoseVelocity2d {
-        val twist = localizer.update()
-        pose = pose.plus(twist.value())
-
-        poseHistory.add(pose)
-        while (poseHistory.size > 100) {
-            poseHistory.removeFirst()
-        }
-
-        estimatedPoseWriter.write(PoseMessage(pose))
-
-        return twist.velocity().value()
-    }
-
-    private fun drawPoseHistory(c: Canvas) {
-        val xPoints = DoubleArray(poseHistory.size)
-        val yPoints = DoubleArray(poseHistory.size)
-
-        var i = 0
-        for ((position) in poseHistory) {
-            xPoints[i] = position.x
-            yPoints[i] = position.y
-
-            i++
-        }
-
-        c.setStrokeWidth(1)
-        c.setStroke("#3F51B5")
-        c.strokePolyline(xPoints, yPoints)
-    }
-
-    fun actionBuilder(beginPose: Pose2d): TrajectoryActionBuilder {
-        return TrajectoryActionBuilder(
-            { turn: TimeTurn -> TurnAction(turn) },
-            { t: TimeTrajectory -> FollowTrajectoryAction(t) },
-            TrajectoryBuilderParams(
-                1e-6,
-                ProfileParams(
-                    0.25, 0.1, 1e-2
-                )
-            ),
-            beginPose, 0.0,
-            defaultTurnConstraints,
-            defaultVelConstraint, defaultAccelConstraint
-        )
-    }
-
-    companion object {
-        var PARAMS: Params = Params()
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoDeadWheelLocalizer.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoDeadWheelLocalizer.kt
deleted file mode 100644
index 388b063..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoDeadWheelLocalizer.kt
+++ /dev/null
@@ -1,138 +0,0 @@
-package org.firstinspires.ftc.teamcode
-
-import com.acmerobotics.dashboard.config.Config
-import com.acmerobotics.roadrunner.DualNum
-import com.acmerobotics.roadrunner.Rotation2d
-import com.acmerobotics.roadrunner.Time
-import com.acmerobotics.roadrunner.Twist2dDual
-import com.acmerobotics.roadrunner.Vector2d
-import com.acmerobotics.roadrunner.Vector2dDual
-import com.acmerobotics.roadrunner.ftc.Encoder
-import com.acmerobotics.roadrunner.ftc.FlightRecorder.write
-import com.acmerobotics.roadrunner.ftc.OverflowEncoder
-import com.acmerobotics.roadrunner.ftc.RawEncoder
-import com.qualcomm.robotcore.hardware.DcMotorEx
-import com.qualcomm.robotcore.hardware.HardwareMap
-import com.qualcomm.robotcore.hardware.IMU
-import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit
-import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity
-import org.firstinspires.ftc.robotcore.external.navigation.UnnormalizedAngleUnit
-import org.firstinspires.ftc.teamcode.messages.TwoDeadWheelInputsMessage
-import kotlin.math.abs
-import kotlin.math.sign
-
-@Config
-class TwoDeadWheelLocalizer(
-    hardwareMap: HardwareMap, val imu: IMU,
-    private val inPerTick: Double
-) :
-    Localizer {
-    class Params {
-        var parYTicks: Double = 0.0 // y position of the parallel encoder (in tick units)
-        var perpXTicks: Double = 0.0 // x position of the perpendicular encoder (in tick units)
-    }
-
-    // TODO: make sure your config has **motors** with these names (or change them)
-    //   the encoders should be plugged into the slot matching the named motor
-    //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
-    val par: Encoder =
-        OverflowEncoder(RawEncoder(hardwareMap.get(DcMotorEx::class.java, "par")))
-    val perp: Encoder =
-        OverflowEncoder(RawEncoder(hardwareMap.get(DcMotorEx::class.java, "perp")))
-
-    // TODO: reverse encoder directions if needed
-    //   par.setDirection(DcMotorSimple.Direction.REVERSE);
-
-    private var lastParPos = 0
-    private var lastPerpPos = 0
-    private var lastHeading: Rotation2d? = null
-
-    private var lastRawHeadingVel = 0.0
-    private var headingVelOffset = 0.0
-    private var initialized = false
-
-    init {
-        write("TWO_DEAD_WHEEL_PARAMS", PARAMS)
-    }
-
-    override fun update(): Twist2dDual<Time> {
-        val parPosVel = par.getPositionAndVelocity()
-        val perpPosVel = perp.getPositionAndVelocity()
-
-        val angles = imu.robotYawPitchRollAngles
-        // Use degrees here to work around https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/1070
-        val angularVelocityDegrees = imu.getRobotAngularVelocity(AngleUnit.DEGREES)
-        val angularVelocity = AngularVelocity(
-            UnnormalizedAngleUnit.RADIANS,
-            Math.toRadians(angularVelocityDegrees.xRotationRate.toDouble()).toFloat(),
-            Math.toRadians(angularVelocityDegrees.yRotationRate.toDouble()).toFloat(),
-            Math.toRadians(angularVelocityDegrees.zRotationRate.toDouble()).toFloat(),
-            angularVelocityDegrees.acquisitionTime
-        )
-
-        write(
-            "TWO_DEAD_WHEEL_INPUTS",
-            TwoDeadWheelInputsMessage(parPosVel, perpPosVel, angles, angularVelocity)
-        )
-
-        val heading = Rotation2d.exp(angles.getYaw(AngleUnit.RADIANS))
-
-        // see https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/617
-        val rawHeadingVel = angularVelocity.zRotationRate.toDouble()
-        if (abs(rawHeadingVel - lastRawHeadingVel) > Math.PI) {
-            headingVelOffset -= sign(rawHeadingVel) * 2 * Math.PI
-        }
-        lastRawHeadingVel = rawHeadingVel
-        val headingVel = headingVelOffset + rawHeadingVel
-
-        if (!initialized) {
-            initialized = true
-
-            lastParPos = parPosVel.position
-            lastPerpPos = perpPosVel.position
-            lastHeading = heading
-
-            return Twist2dDual(
-                Vector2dDual.constant(Vector2d(0.0, 0.0), 2),
-                DualNum.constant(0.0, 2)
-            )
-        }
-
-        val parPosDelta = parPosVel.position - lastParPos
-        val perpPosDelta = perpPosVel.position - lastPerpPos
-        val headingDelta = heading.minus(lastHeading!!)
-
-        val twist = Twist2dDual(
-            Vector2dDual(
-                DualNum<Time>(
-                    doubleArrayOf(
-                        parPosDelta - PARAMS.parYTicks * headingDelta,
-                        parPosVel.velocity - PARAMS.parYTicks * headingVel,
-                    )
-                ).times(inPerTick),
-                DualNum<Time>(
-                    doubleArrayOf(
-                        perpPosDelta - PARAMS.perpXTicks * headingDelta,
-                        perpPosVel.velocity - PARAMS.perpXTicks * headingVel,
-                    )
-                ).times(inPerTick)
-            ),
-            DualNum(
-                doubleArrayOf(
-                    headingDelta,
-                    headingVel,
-                )
-            )
-        )
-
-        lastParPos = parPosVel.position
-        lastPerpPos = perpPosVel.position
-        lastHeading = heading
-
-        return twist
-    }
-
-    companion object {
-        var PARAMS: Params = Params()
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.kt
deleted file mode 100644
index de31273..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.kt
+++ /dev/null
@@ -1,79 +0,0 @@
-package org.firstinspires.ftc.teamcode.tuning
-
-import com.acmerobotics.dashboard.FtcDashboard
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry
-import com.acmerobotics.dashboard.telemetry.TelemetryPacket
-import com.acmerobotics.roadrunner.Pose2d
-import com.acmerobotics.roadrunner.PoseVelocity2d
-import com.acmerobotics.roadrunner.Vector2d
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode
-import org.firstinspires.ftc.teamcode.Drawing
-import org.firstinspires.ftc.teamcode.MecanumDrive
-import org.firstinspires.ftc.teamcode.TankDrive
-
-class LocalizationTest : LinearOpMode() {
-    @Throws(InterruptedException::class)
-    override fun runOpMode() {
-        telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry)
-
-        if (TuningOpModes.DRIVE_CLASS == MecanumDrive::class.java) {
-            val drive = MecanumDrive(hardwareMap, Pose2d(0.0, 0.0, 0.0))
-
-            waitForStart()
-
-            while (opModeIsActive()) {
-                drive.setDrivePowers(
-                    PoseVelocity2d(
-                        Vector2d(
-                            -gamepad1.left_stick_y.toDouble(),
-                            -gamepad1.left_stick_x.toDouble()
-                        ),
-                        -gamepad1.right_stick_x.toDouble()
-                    )
-                )
-
-                drive.updatePoseEstimate()
-
-                telemetry.addData("x", drive.pose.position.x)
-                telemetry.addData("y", drive.pose.position.y)
-                telemetry.addData("heading (deg)", Math.toDegrees(drive.pose.heading.toDouble()))
-                telemetry.update()
-
-                val packet = TelemetryPacket()
-                packet.fieldOverlay().setStroke("#3F51B5")
-                Drawing.drawRobot(packet.fieldOverlay(), drive.pose)
-                FtcDashboard.getInstance().sendTelemetryPacket(packet)
-            }
-        } else if (TuningOpModes.DRIVE_CLASS == TankDrive::class.java) {
-            val drive = TankDrive(hardwareMap, Pose2d(0.0, 0.0, 0.0))
-
-            waitForStart()
-
-            while (opModeIsActive()) {
-                drive.setDrivePowers(
-                    PoseVelocity2d(
-                        Vector2d(
-                            -gamepad1.left_stick_y.toDouble(),
-                            0.0
-                        ),
-                        -gamepad1.right_stick_x.toDouble()
-                    )
-                )
-
-                drive.updatePoseEstimate()
-
-                telemetry.addData("x", drive.pose.position.x)
-                telemetry.addData("y", drive.pose.position.y)
-                telemetry.addData("heading (deg)", Math.toDegrees(drive.pose.heading.toDouble()))
-                telemetry.update()
-
-                val packet = TelemetryPacket()
-                packet.fieldOverlay().setStroke("#3F51B5")
-                Drawing.drawRobot(packet.fieldOverlay(), drive.pose)
-                FtcDashboard.getInstance().sendTelemetryPacket(packet)
-            }
-        } else {
-            throw RuntimeException()
-        }
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedbackTuner.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedbackTuner.kt
deleted file mode 100644
index ccfa67b..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedbackTuner.kt
+++ /dev/null
@@ -1,66 +0,0 @@
-package org.firstinspires.ftc.teamcode.tuning
-
-import com.acmerobotics.roadrunner.Pose2d
-import com.acmerobotics.roadrunner.ftc.runBlocking
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode
-import org.firstinspires.ftc.teamcode.MecanumDrive
-import org.firstinspires.ftc.teamcode.TankDrive
-import org.firstinspires.ftc.teamcode.ThreeDeadWheelLocalizer
-import org.firstinspires.ftc.teamcode.TwoDeadWheelLocalizer
-
-class ManualFeedbackTuner : LinearOpMode() {
-    @Throws(InterruptedException::class)
-    override fun runOpMode() {
-        if (TuningOpModes.DRIVE_CLASS == MecanumDrive::class.java) {
-            val drive = MecanumDrive(hardwareMap, Pose2d(0.0, 0.0, 0.0))
-
-            if (drive.localizer is TwoDeadWheelLocalizer) {
-                if (TwoDeadWheelLocalizer.Companion.PARAMS.perpXTicks == 0.0 && TwoDeadWheelLocalizer.Companion.PARAMS.parYTicks == 0.0) {
-                    throw RuntimeException("Odometry wheel locations not set! Run AngularRampLogger to tune them.")
-                }
-            } else if (drive.localizer is ThreeDeadWheelLocalizer) {
-                if (ThreeDeadWheelLocalizer.Companion.PARAMS.perpXTicks == 0.0 && ThreeDeadWheelLocalizer.Companion.PARAMS.par0YTicks == 0.0 && ThreeDeadWheelLocalizer.Companion.PARAMS.par1YTicks == 1.0) {
-                    throw RuntimeException("Odometry wheel locations not set! Run AngularRampLogger to tune them.")
-                }
-            }
-            waitForStart()
-
-            while (opModeIsActive()) {
-                runBlocking(
-                    drive.actionBuilder(Pose2d(0.0, 0.0, 0.0))
-                        .lineToX(DISTANCE)
-                        .lineToX(0.0)
-                        .build()
-                )
-            }
-        } else if (TuningOpModes.DRIVE_CLASS == TankDrive::class.java) {
-            val drive = TankDrive(hardwareMap, Pose2d(0.0, 0.0, 0.0))
-
-            if (drive.localizer is TwoDeadWheelLocalizer) {
-                if (TwoDeadWheelLocalizer.Companion.PARAMS.perpXTicks == 0.0 && TwoDeadWheelLocalizer.Companion.PARAMS.parYTicks == 0.0) {
-                    throw RuntimeException("Odometry wheel locations not set! Run AngularRampLogger to tune them.")
-                }
-            } else if (drive.localizer is ThreeDeadWheelLocalizer) {
-                if (ThreeDeadWheelLocalizer.Companion.PARAMS.perpXTicks == 0.0 && ThreeDeadWheelLocalizer.Companion.PARAMS.par0YTicks == 0.0 && ThreeDeadWheelLocalizer.Companion.PARAMS.par1YTicks == 1.0) {
-                    throw RuntimeException("Odometry wheel locations not set! Run AngularRampLogger to tune them.")
-                }
-            }
-            waitForStart()
-
-            while (opModeIsActive()) {
-                runBlocking(
-                    drive.actionBuilder(Pose2d(0.0, 0.0, 0.0))
-                        .lineToX(DISTANCE)
-                        .lineToX(0.0)
-                        .build()
-                )
-            }
-        } else {
-            throw RuntimeException()
-        }
-    }
-
-    companion object {
-        var DISTANCE: Double = 64.0
-    }
-}
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/blue/basket/left/Blue1Plus1.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/blue/basket/left/Blue1Plus1.kt
new file mode 100644
index 0000000..12a6a55
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/blue/basket/left/Blue1Plus1.kt
@@ -0,0 +1,125 @@
+package org.firstinspires.ftc.teamcode.legacy.auto.blue.basket.left
+
+//@Autonomous(name = "Blue 1 + 1 High Basket", group = "Basket", preselectTeleOp = "MainTeleOp")
+//class Blue1Plus1: OpMode() {
+//    private lateinit var armLeft: Motor
+//    private lateinit var armRight: Motor
+//    private lateinit var elevatorLeft: Motor
+//    private lateinit var elevatorRight: Motor
+//
+//    private lateinit var intake: Servo
+//    private lateinit var intakeBelt: Servo
+//
+//    private lateinit var driveSubsystem: DriveSubsystem
+//    private lateinit var intakeSubsystem: IntakeSubsystem
+//    private lateinit var intakeBeltSubsystem: IntakeBeltSubsystem
+//    private lateinit var armSubsystem: ArmSubsystem
+//    private lateinit var elevatorSubsystem: ElevatorSubsystem
+//
+//    override fun init() {
+//        telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry)
+//
+//        intake = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE.deviceName)
+//        intakeBelt = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE_BELT.deviceName)
+//
+//        armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName)
+//        armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName)
+//
+//        elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName)
+//        elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName)
+//
+//        driveSubsystem = org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem(hardwareMap)
+//        intakeSubsystem = org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem(intake)
+//        intakeBeltSubsystem = IntakeBeltSubsystem(intakeBelt)
+//        armSubsystem = ArmSubsystem(armRight, armLeft)
+//        elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle)
+//
+//        val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_LEFT.startPose)
+//            .addTemporalMarker(0.1) {
+//                intakeSubsystem.intake()
+//                intakeBeltSubsystem.intakePos()
+//            }
+//            .splineToLinearHeading(Pose2d(54.0, 55.0, Math.toRadians(225.0)), Math.toRadians(0.0))
+//            .back(4.0)
+//            .addTemporalMarker(3.0) {
+//                armSubsystem.setpoint = Math.toRadians(98.0)
+//            }
+//            .waitSeconds(1.0)
+//            .addTemporalMarker(4.0) {
+//                elevatorSubsystem.setpoint = 30.0
+//            }
+//            .waitSeconds(1.0)
+//            .addTemporalMarker(5.0) {
+//                intakeBeltSubsystem.outtakePos()
+//            }
+//            .waitSeconds(2.0)
+//            .addTemporalMarker(7.0) {
+//                intakeSubsystem.outtake()
+//            }
+//            .waitSeconds(0.5)
+//            .addTemporalMarker(7.5) {
+//                intakeBeltSubsystem.intakePos()
+//                elevatorSubsystem.setpoint = 0.0
+//            }
+//            .waitSeconds(1.5)
+//            .addTemporalMarker(9.5) {
+//                armSubsystem.setpoint = 0.0
+//                intakeBeltSubsystem.outtakePos()
+//            }
+//            .waitSeconds(1.0)
+//            .turn(Math.toRadians(38.0))
+//            .addTemporalMarker(11.0) {
+//                elevatorSubsystem.setpoint = 31.0
+//            }
+//            .waitSeconds(2.0)
+//            .addTemporalMarker(13.0) {
+//                intakeBeltSubsystem.intakePos()
+//            }
+//            .waitSeconds(1.0)
+//            .addTemporalMarker(14.0) {
+//                intakeSubsystem.intake()
+//            }
+//            .waitSeconds(1.0)
+//            .addTemporalMarker(15.0) {
+//                elevatorSubsystem.setpoint = 0.0
+//            }
+//            .waitSeconds(2.0)
+//            .turn(Math.toRadians(-40.0))
+//            .addTemporalMarker(17.0) {
+//                armSubsystem.setpoint = Math.toRadians(98.0)
+//            }
+//            .waitSeconds(1.0)
+//            .back(0.5)
+//            .addTemporalMarker(18.0) {
+//                elevatorSubsystem.setpoint = 33.0
+//            }
+//            .waitSeconds(3.0)
+//            .addTemporalMarker(21.0) {
+//                intakeBeltSubsystem.outtakePos()
+//            }
+//            .waitSeconds(1.0)
+//            .addTemporalMarker(22.0) {
+//                intakeSubsystem.outtake()
+//            }
+//            .waitSeconds(1.0)
+//            .addTemporalMarker(23.0) {
+//                elevatorSubsystem.setpoint = 0.0
+//            }
+//            .splineToSplineHeading(Pose2d(25.0, 10.0, Math.toRadians(0.0)), Math.toRadians(180.0))
+//            .build()
+//
+//        driveSubsystem.poseEstimate = AutoStartPose.BLUE_LEFT.startPose
+//        driveSubsystem.followTrajectorySequenceAsync(trajectory)
+//    }
+//
+//    override fun loop() {
+//        driveSubsystem.update()
+//
+//        elevatorSubsystem.periodic()
+//        armSubsystem.periodic()
+//
+//        telemetry.addData("Arm Position", armSubsystem.armAngle)
+//        telemetry.addData("Slides Position", elevatorSubsystem.slidePos)
+//        telemetry.update()
+//    }
+//}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/blue/basket/left/Blue1Plus2.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/blue/basket/left/Blue1Plus2.kt
new file mode 100644
index 0000000..a37874e
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/blue/basket/left/Blue1Plus2.kt
@@ -0,0 +1,197 @@
+package org.firstinspires.ftc.teamcode.legacy.auto.blue.basket.left
+
+//@Autonomous(name = "Blue 1 + 2 High Basket", group = "Basket", preselectTeleOp = "MainTeleOp")
+//class Blue1Plus2: OpMode() {
+//    private lateinit var armLeft: Motor
+//    private lateinit var armRight: Motor
+//    private lateinit var elevatorLeft: Motor
+//    private lateinit var elevatorRight: Motor
+//
+//    private lateinit var intake: Servo
+//    private lateinit var intakeBelt: Servo
+//
+//    private lateinit var driveSubsystem: DriveSubsystem
+//    private lateinit var intakeSubsystem: IntakeSubsystem
+//    private lateinit var intakeBeltSubsystem: IntakeBeltSubsystem
+//    private lateinit var armSubsystem: ArmSubsystem
+//    private lateinit var elevatorSubsystem: ElevatorSubsystem
+//
+//    override fun init() {
+//        telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry)
+//
+//        intake = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE.deviceName)
+//        intakeBelt = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE_BELT.deviceName)
+//
+//        armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName)
+//        armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName)
+//
+//        elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName)
+//        elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName)
+//
+//        driveSubsystem = org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem(hardwareMap)
+//        intakeSubsystem = org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem(intake)
+//        intakeBeltSubsystem = IntakeBeltSubsystem(intakeBelt)
+//        armSubsystem = ArmSubsystem(armRight, armLeft)
+//        elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle)
+//
+//        val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_LEFT.startPose)
+//            .addTemporalMarker(2.0) {
+//                intakeSubsystem.intake()
+//                intakeBeltSubsystem.intakePos()
+//            }
+//            .splineToLinearHeading(Pose2d(54.0, 55.0, Math.toRadians(225.0)), Math.toRadians(0.0))
+//            .back(4.0)
+//            .addTemporalMarker(1.5) {
+//                armSubsystem.setpoint = Math.toRadians(98.0)
+//            }
+//            .waitSeconds(1.0)
+//            .addTemporalMarker(4.0) {
+//                elevatorSubsystem.setpoint = 30.0
+//            }
+//            .waitSeconds(1.0)
+//            .addTemporalMarker(4.5) {
+//                intakeBeltSubsystem.outtakePos()
+//            }
+//            .waitSeconds(1.5)
+//            .addTemporalMarker(6.0) {
+//                intakeSubsystem.outtake()
+//            }
+//            .waitSeconds(0.5)
+//            .addTemporalMarker(6.5) {
+//                intakeBeltSubsystem.intakePos()
+//                elevatorSubsystem.setpoint = 0.0
+//            }
+//            .turn(Math.toRadians(32.0))
+//            .waitSeconds(1.0)
+//            .forward(3.0)
+//            .addTemporalMarker(7.5) {
+//                armSubsystem.setpoint = Math.toRadians(0.0)
+//            }
+//            .waitSeconds(1.0)
+//            .addTemporalMarker(8.5) {
+//                elevatorSubsystem.setpoint = 30.0
+//            }
+//            .waitSeconds(1.5)
+//            .addTemporalMarker(10.0) {
+//                intakeBeltSubsystem.intakePos()
+//            }
+//            .waitSeconds(1.5)
+//            .addTemporalMarker(11.5) {
+//                intakeSubsystem.intake()
+//            }
+//            .waitSeconds(0.5)
+//            .addTemporalMarker(12.0) {
+//                intakeBeltSubsystem.outtakePos()
+//                elevatorSubsystem.setpoint = 0.0
+//            }
+//            .turn(Math.toRadians(-30.0))
+//            .waitSeconds(1.0)
+//            .back(2.0)
+//            .addTemporalMarker(13.0) {
+//                armSubsystem.setpoint = Math.toRadians(98.0)
+//                intakeBeltSubsystem.intakePos()
+//            }
+//            .waitSeconds(1.0)
+//            .addTemporalMarker(15.0) {
+//                elevatorSubsystem.setpoint = 33.0
+//            }
+//            .waitSeconds(2.0)
+//            .addTemporalMarker(17.0) {
+//                intakeBeltSubsystem.outtakePos()
+//            }
+//            .turn(Math.toRadians(52.0))
+//            .waitSeconds(1.5)
+//            .addTemporalMarker(18.5) {
+//                intakeSubsystem.outtake()
+//            }
+//            .waitSeconds(0.5)
+//            .addTemporalMarker(19.0) {
+//                intakeBeltSubsystem.intakePos()
+//                elevatorSubsystem.setpoint = 0.0
+//            }
+//            .waitSeconds(1.5)
+//            .addTemporalMarker(20.5) {
+//                intakeBeltSubsystem.outtakePos()
+//                armSubsystem.setpoint = 0.0
+//            }
+//            .waitSeconds(1.0)
+//            .forward(2.0)
+//            .addTemporalMarker(21.5) {
+//                elevatorSubsystem.setpoint = 30.0
+//                intakeBeltSubsystem.intakePos()
+//            }
+//            .turn(Math.toRadians(-60.0))
+//            .waitSeconds(2.0)
+//            .addTemporalMarker(23.5) {
+//                intakeSubsystem.intake()
+//            }
+//            .waitSeconds(0.5)
+//            .addTemporalMarker(24.0) {
+//                intakeBeltSubsystem.outtakePos()
+//                elevatorSubsystem.setpoint = 0.0
+//            }
+//            .waitSeconds(0.5)
+//            .back(2.0)
+//            .addTemporalMarker(24.5) {
+//                armSubsystem.setpoint = Math.toRadians(98.0)
+//                intakeBeltSubsystem.intakePos()
+//            }
+//            .waitSeconds(1.5)
+//            .addTemporalMarker(27.0) {
+//                elevatorSubsystem.setpoint = 33.0
+//            }
+//            .waitSeconds(1.5)
+//            .addTemporalMarker(28.5) {
+//                intakeBeltSubsystem.outtakePos()
+//            }
+//            .waitSeconds(0.5)
+//            .addTemporalMarker(29.0) {
+//                intakeSubsystem.outtake()
+//            }
+//            // .strafetolinearheading
+//
+////            .waitSeconds(0.5)
+////            .addTemporalMarker(6.0) {
+////                intakeBeltSubsystem.outtakePos()
+////                armSubsystem.setpoint = 0.0
+////            }
+////            .turn(Math.toRadians(40.0))
+////            .waitSeconds(0.5)
+////            .addTemporalMarker(6.5) {
+////                elevatorSubsystem.setpoint = 30.0
+////            }
+////            .waitSeconds(1.0)
+////            .addTemporalMarker(7.5) {
+////                intakeBeltSubsystem.intakePos()
+////            }
+////            .waitSeconds(0.5)
+////            .addTemporalMarker(8.0) {
+////                intakeSubsystem.intake()
+////            }
+////            .waitSeconds(0.5)
+////            .addTemporalMarker(8.5) {
+////                elevatorSubsystem.setpoint = 0.0
+////                intakeBeltSubsystem.outtakePos()
+////            }
+////            .turn(Math.toRadians(-40.0))
+////            .waitSeconds(1.0)
+////            .addTemporalMarker(9.5) {
+////                armSubsystem.setpoint = Math.toRadians(98.0)
+////            }
+//            .build()
+//
+//        driveSubsystem.poseEstimate = AutoStartPose.BLUE_LEFT.startPose
+//        driveSubsystem.followTrajectorySequenceAsync(trajectory)
+//    }
+//
+//    override fun loop() {
+//        driveSubsystem.update()
+//
+//        elevatorSubsystem.periodic()
+//        armSubsystem.periodic()
+//
+//        telemetry.addData("Arm Position", armSubsystem.armAngle)
+//        telemetry.addData("Slides Position", elevatorSubsystem.slidePos)
+//        telemetry.update()
+//    }
+//}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/blue/basket/left/Blue1Plus2V2.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/blue/basket/left/Blue1Plus2V2.kt
new file mode 100644
index 0000000..f44d12c
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/blue/basket/left/Blue1Plus2V2.kt
@@ -0,0 +1,185 @@
+package org.firstinspires.ftc.teamcode.legacy.auto.blue.basket.left
+
+//@Autonomous(name = "V2 Blue 1 + 2", group = "Basket", preselectTeleOp = "MainTeleOp")
+//class Blue1Plus2V2: OpMode() {
+//    private lateinit var armLeft: Motor
+//    private lateinit var armRight: Motor
+//    private lateinit var elevatorLeft: Motor
+//    private lateinit var elevatorRight: Motor
+//
+//    private lateinit var intake: Servo
+//    private lateinit var intakeBelt: Servo
+//
+//    private lateinit var driveSubsystem: DriveSubsystem
+//    private lateinit var intakeSubsystem: IntakeSubsystem
+//    private lateinit var intakeBeltSubsystem: IntakeBeltSubsystem
+//    private lateinit var armSubsystem: ArmSubsystem
+//    private lateinit var elevatorSubsystem: ElevatorSubsystem
+//
+//    private lateinit var toBasket1: TrajectorySequence
+//    private lateinit var toBasket2: TrajectorySequence
+//
+//    override fun init() {
+//        telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry)
+//
+//        intake = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE.deviceName)
+//        intakeBelt = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE_BELT.deviceName)
+//
+//        armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName)
+//        armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName)
+//
+//        elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName)
+//        elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName)
+//
+//        driveSubsystem = org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem(hardwareMap)
+//        intakeSubsystem = org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem(intake)
+//        intakeBeltSubsystem = IntakeBeltSubsystem(intakeBelt)
+//        armSubsystem = ArmSubsystem(armRight, armLeft)
+//        elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle)
+//
+//        toBasket1 = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_LEFT.startPose)
+//            .splineToLinearHeading(Pose2d(54.0, 56.0, Math.toRadians(225.0)), Math.toRadians(0.0))
+//            .addTemporalMarker(0.1) {
+//                intakeSubsystem.intake()
+//                armSubsystem.setpoint = Math.toRadians(98.0)
+//                intakeBeltSubsystem.intakePos()
+//            }
+//            .waitSeconds(0.5)
+//            .addTemporalMarker(1.0) {
+//                elevatorSubsystem.setpoint = 34.0
+//                intakeBeltSubsystem.intakePos()
+//            }
+//            .waitSeconds(0.5)
+//            .addTemporalMarker(1.5) {
+//                intakeBeltSubsystem.outtakePos()
+//            }
+//            .waitSeconds(0.5)
+//            .addTemporalMarker(2.0) {
+//                intakeSubsystem.outtake()
+//            }
+//            .waitSeconds(0.1)
+//            .addTemporalMarker(2.1) {
+//                intakeBeltSubsystem.outtakePos()
+//                elevatorSubsystem.setpoint = 0.0
+//            }
+//            .turn(Math.toRadians(37.0))
+//            .waitSeconds(2.0)
+//            .addTemporalMarker(4.1) {
+//                armSubsystem.setpoint = Math.toRadians(0.0)
+//            }
+//            .waitSeconds(0.5)
+//            .addTemporalMarker(4.6) {
+//                elevatorSubsystem.setpoint = 19.0
+//                intakeBeltSubsystem.intakePos()
+//            }
+//            .waitSeconds(1.25)
+//            .addTemporalMarker(6.0) {
+//                intakeSubsystem.intake()
+//            }
+//            .waitSeconds(0.25)
+//            .turn(Math.toRadians(-37.0))
+//            .addTemporalMarker(6.25) {
+//                intakeBeltSubsystem.outtakePos()
+//                elevatorSubsystem.setpoint = 0.0
+//            }
+//            .waitSeconds(1.0)
+//            .addTemporalMarker(7.05) {
+//                armSubsystem.setpoint = Math.toRadians(98.0)
+//                intakeBeltSubsystem.intakePos()
+//            }
+//            .waitSeconds(0.75)
+//            .addTemporalMarker(7.8) {
+//                elevatorSubsystem.setpoint = 31.0
+//            }
+//            .waitSeconds(1.0)
+//            .addTemporalMarker(8.8) {
+//                intakeBeltSubsystem.outtakePos()
+//            }
+//            .waitSeconds(1.25)
+//            .turn(Math.toRadians(55.0))
+//            .addTemporalMarker(10.05) {
+//                intakeSubsystem.outtake()
+//            }
+//            .waitSeconds(0.25)
+//            .addTemporalMarker(10.3) {
+//                intakeBeltSubsystem.intakePos()
+//            }
+//            .waitSeconds(0.5)
+//            .addTemporalMarker(10.8) {
+//                elevatorSubsystem.setpoint = 0.0
+//            }
+//            .waitSeconds(1.5)
+//            .addTemporalMarker(12.3) {
+//                armSubsystem.setpoint = 0.0
+//                intakeBeltSubsystem.outtakePos()
+//            }
+//            .waitSeconds(1.0)
+//            .addTemporalMarker(13.3) {
+//                elevatorSubsystem.setpoint = 19.0
+//            }
+//            .waitSeconds(1.0)
+//            .addTemporalMarker(14.3) {
+//                intakeBeltSubsystem.intakePos()
+//            }
+//            .turn(Math.toRadians(-55.0))
+//            .waitSeconds(1.2)
+//            .back(2.0)
+//            .addTemporalMarker(15.5) {
+//                intakeSubsystem.intake()
+//            }
+//            .waitSeconds(0.5)
+//            .addTemporalMarker(16.0) {
+//                intakeBeltSubsystem.outtakePos()
+//                elevatorSubsystem.setpoint = 0.0
+//            }
+//            .waitSeconds(1.0)
+//            .addTemporalMarker(17.0) {
+//                armSubsystem.setpoint = Math.toRadians(98.0)
+//                intakeBeltSubsystem.intakePos()
+//            }
+//            .waitSeconds(1.0)
+//            .addTemporalMarker(18.0) {
+//                elevatorSubsystem.setpoint = 29.0
+//            }
+//            .waitSeconds(1.5)
+//            .addTemporalMarker(19.5) {
+//                intakeBeltSubsystem.outtakePos()
+//            }
+//            .waitSeconds(1.0)
+//            .addTemporalMarker(20.5) {
+//                intakeSubsystem.outtake()
+//            }
+//            .waitSeconds(0.5)
+//            .addTemporalMarker(21.0) {
+//                intakeBeltSubsystem.intakePos()
+//            }
+//            .waitSeconds(0.5)
+//            .addTemporalMarker(21.5) {
+//                elevatorSubsystem.setpoint = 0.0
+//            }
+//            .waitSeconds(1.0)
+//            .addTemporalMarker(22.5) {
+//                armSubsystem.setpoint = 0.0
+//            }
+//            .splineToSplineHeading(Pose2d(28.0, 10.0, Math.toRadians(0.0)), 90.0)
+//            .back(5.0)
+//            .addDisplacementMarker {
+//                driveSubsystem.followTrajectorySequenceAsync(toBasket2)
+//            }
+//            .build()
+//        driveSubsystem.poseEstimate = AutoStartPose.BLUE_LEFT.startPose
+//        driveSubsystem.followTrajectorySequenceAsync(toBasket1)
+//    }
+//
+//
+//    override fun loop() {
+//        driveSubsystem.update()
+//
+//        elevatorSubsystem.periodic()
+//        armSubsystem.periodic()
+//
+//        telemetry.addData("Arm Position", armSubsystem.armAngle)
+//        telemetry.addData("Slides Position", elevatorSubsystem.slidePos)
+//        telemetry.update()
+//    }
+//}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/Blue1Plus1.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/Blue1Plus1.kt
deleted file mode 100644
index c0c61cf..0000000
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/Blue1Plus1.kt
+++ /dev/null
@@ -1,140 +0,0 @@
-package org.firstinspires.ftc.teamcode.opModes.auto.blue.basket.left
-
-import com.acmerobotics.dashboard.FtcDashboard
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry
-import com.acmerobotics.roadrunner.geometry.Pose2d
-import com.arcrobotics.ftclib.hardware.motors.Motor
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous
-import com.qualcomm.robotcore.eventloop.opmode.OpMode
-import com.qualcomm.robotcore.hardware.Servo
-import org.firstinspires.ftc.teamcode.constants.AutoStartPose
-import org.firstinspires.ftc.teamcode.constants.ControlBoard
-import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem
-import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
-import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeBeltSubsystem
-import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem
-import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem
-
-@Autonomous(name = "Blue 1 + 1 High Basket", group = "Basket", preselectTeleOp = "MainTeleOp")
-class Blue1Plus1: OpMode() {
-    private lateinit var armLeft: Motor
-    private lateinit var armRight: Motor
-    private lateinit var elevatorLeft: Motor
-    private lateinit var elevatorRight: Motor
-
-    private lateinit var intake: Servo
-    private lateinit var intakeBelt: Servo
-
-    private lateinit var driveSubsystem: DriveSubsystem
-    private lateinit var intakeSubsystem: IntakeSubsystem
-    private lateinit var intakeBeltSubsystem: IntakeBeltSubsystem
-    private lateinit var armSubsystem: ArmSubsystem
-    private lateinit var elevatorSubsystem: ElevatorSubsystem
-
-    override fun init() {
-        telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry)
-
-        intake = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE.deviceName)
-        intakeBelt = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE_BELT.deviceName)
-
-        armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName)
-        armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName)
-
-        elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName)
-        elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName)
-
-        driveSubsystem = DriveSubsystem(hardwareMap)
-        intakeSubsystem = IntakeSubsystem(intake)
-        intakeBeltSubsystem = IntakeBeltSubsystem(intakeBelt)
-        armSubsystem = ArmSubsystem(armRight, armLeft)
-        elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle)
-
-        val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_LEFT.startPose)
-            .addTemporalMarker(0.1) {
-                intakeSubsystem.intake()
-                intakeBeltSubsystem.intakePos()
-            }
-            .splineToLinearHeading(Pose2d(54.0, 55.0, Math.toRadians(225.0)), Math.toRadians(0.0))
-            .back(4.0)
-            .addTemporalMarker(3.0) {
-                armSubsystem.setpoint = Math.toRadians(98.0)
-            }
-            .waitSeconds(1.0)
-            .addTemporalMarker(4.0) {
-                elevatorSubsystem.setpoint = 30.0
-            }
-            .waitSeconds(1.0)
-            .addTemporalMarker(5.0) {
-                intakeBeltSubsystem.outtakePos()
-            }
-            .waitSeconds(2.0)
-            .addTemporalMarker(7.0) {
-                intakeSubsystem.outtake()
-            }
-            .waitSeconds(0.5)
-            .addTemporalMarker(7.5) {
-                intakeBeltSubsystem.intakePos()
-                elevatorSubsystem.setpoint = 0.0
-            }
-            .waitSeconds(1.5)
-            .addTemporalMarker(9.5) {
-                armSubsystem.setpoint = 0.0
-                intakeBeltSubsystem.outtakePos()
-            }
-            .waitSeconds(1.0)
-            .turn(Math.toRadians(38.0))
-            .addTemporalMarker(11.0) {
-                elevatorSubsystem.setpoint = 31.0
-            }
-            .waitSeconds(2.0)
-            .addTemporalMarker(13.0) {
-                intakeBeltSubsystem.intakePos()
-            }
-            .waitSeconds(1.0)
-            .addTemporalMarker(14.0) {
-                intakeSubsystem.intake()
-            }
-            .waitSeconds(1.0)
-            .addTemporalMarker(15.0) {
-                elevatorSubsystem.setpoint = 0.0
-            }
-            .waitSeconds(2.0)
-            .turn(Math.toRadians(-40.0))
-            .addTemporalMarker(17.0) {
-                armSubsystem.setpoint = Math.toRadians(98.0)
-            }
-            .waitSeconds(1.0)
-            .back(0.5)
-            .addTemporalMarker(18.0) {
-                elevatorSubsystem.setpoint = 33.0
-            }
-            .waitSeconds(3.0)
-            .addTemporalMarker(21.0) {
-                intakeBeltSubsystem.outtakePos()
-            }
-            .waitSeconds(1.0)
-            .addTemporalMarker(22.0) {
-                intakeSubsystem.outtake()
-            }
-            .waitSeconds(1.0)
-            .addTemporalMarker(23.0) {
-                elevatorSubsystem.setpoint = 0.0
-            }
-            .splineToSplineHeading(Pose2d(25.0, 10.0, Math.toRadians(0.0)), Math.toRadians(180.0))
-            .build()
-
-        driveSubsystem.poseEstimate = AutoStartPose.BLUE_LEFT.startPose
-        driveSubsystem.followTrajectorySequenceAsync(trajectory)
-    }
-
-    override fun loop() {
-        driveSubsystem.update()
-
-        elevatorSubsystem.periodic()
-        armSubsystem.periodic()
-
-        telemetry.addData("Arm Position", armSubsystem.armAngle)
-        telemetry.addData("Slides Position", elevatorSubsystem.slidePos)
-        telemetry.update()
-    }
-}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/Blue1Plus2.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/Blue1Plus2.kt
deleted file mode 100644
index b74cf04..0000000
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/Blue1Plus2.kt
+++ /dev/null
@@ -1,212 +0,0 @@
-package org.firstinspires.ftc.teamcode.opModes.auto.blue.basket.left
-
-import com.acmerobotics.dashboard.FtcDashboard
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry
-import com.acmerobotics.roadrunner.geometry.Pose2d
-import com.arcrobotics.ftclib.hardware.motors.Motor
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous
-import com.qualcomm.robotcore.eventloop.opmode.OpMode
-import com.qualcomm.robotcore.hardware.Servo
-import org.firstinspires.ftc.teamcode.constants.AutoStartPose
-import org.firstinspires.ftc.teamcode.constants.ControlBoard
-import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem
-import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
-import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeBeltSubsystem
-import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem
-import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem
-
-@Autonomous(name = "Blue 1 + 2 High Basket", group = "Basket", preselectTeleOp = "MainTeleOp")
-class Blue1Plus2: OpMode() {
-    private lateinit var armLeft: Motor
-    private lateinit var armRight: Motor
-    private lateinit var elevatorLeft: Motor
-    private lateinit var elevatorRight: Motor
-
-    private lateinit var intake: Servo
-    private lateinit var intakeBelt: Servo
-
-    private lateinit var driveSubsystem: DriveSubsystem
-    private lateinit var intakeSubsystem: IntakeSubsystem
-    private lateinit var intakeBeltSubsystem: IntakeBeltSubsystem
-    private lateinit var armSubsystem: ArmSubsystem
-    private lateinit var elevatorSubsystem: ElevatorSubsystem
-
-    override fun init() {
-        telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry)
-
-        intake = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE.deviceName)
-        intakeBelt = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE_BELT.deviceName)
-
-        armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName)
-        armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName)
-
-        elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName)
-        elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName)
-
-        driveSubsystem = DriveSubsystem(hardwareMap)
-        intakeSubsystem = IntakeSubsystem(intake)
-        intakeBeltSubsystem = IntakeBeltSubsystem(intakeBelt)
-        armSubsystem = ArmSubsystem(armRight, armLeft)
-        elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle)
-
-        val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_LEFT.startPose)
-            .addTemporalMarker(2.0) {
-                intakeSubsystem.intake()
-                intakeBeltSubsystem.intakePos()
-            }
-            .splineToLinearHeading(Pose2d(54.0, 55.0, Math.toRadians(225.0)), Math.toRadians(0.0))
-            .back(4.0)
-            .addTemporalMarker(1.5) {
-                armSubsystem.setpoint = Math.toRadians(98.0)
-            }
-            .waitSeconds(1.0)
-            .addTemporalMarker(4.0) {
-                elevatorSubsystem.setpoint = 30.0
-            }
-            .waitSeconds(1.0)
-            .addTemporalMarker(4.5) {
-                intakeBeltSubsystem.outtakePos()
-            }
-            .waitSeconds(1.5)
-            .addTemporalMarker(6.0) {
-                intakeSubsystem.outtake()
-            }
-            .waitSeconds(0.5)
-            .addTemporalMarker(6.5) {
-                intakeBeltSubsystem.intakePos()
-                elevatorSubsystem.setpoint = 0.0
-            }
-            .turn(Math.toRadians(32.0))
-            .waitSeconds(1.0)
-            .forward(3.0)
-            .addTemporalMarker(7.5) {
-                armSubsystem.setpoint = Math.toRadians(0.0)
-            }
-            .waitSeconds(1.0)
-            .addTemporalMarker(8.5) {
-                elevatorSubsystem.setpoint = 30.0
-            }
-            .waitSeconds(1.5)
-            .addTemporalMarker(10.0) {
-                intakeBeltSubsystem.intakePos()
-            }
-            .waitSeconds(1.5)
-            .addTemporalMarker(11.5) {
-                intakeSubsystem.intake()
-            }
-            .waitSeconds(0.5)
-            .addTemporalMarker(12.0) {
-                intakeBeltSubsystem.outtakePos()
-                elevatorSubsystem.setpoint = 0.0
-            }
-            .turn(Math.toRadians(-30.0))
-            .waitSeconds(1.0)
-            .back(2.0)
-            .addTemporalMarker(13.0) {
-                armSubsystem.setpoint = Math.toRadians(98.0)
-                intakeBeltSubsystem.intakePos()
-            }
-            .waitSeconds(1.0)
-            .addTemporalMarker(15.0) {
-                elevatorSubsystem.setpoint = 33.0
-            }
-            .waitSeconds(2.0)
-            .addTemporalMarker(17.0) {
-                intakeBeltSubsystem.outtakePos()
-            }
-            .turn(Math.toRadians(52.0))
-            .waitSeconds(1.5)
-            .addTemporalMarker(18.5) {
-                intakeSubsystem.outtake()
-            }
-            .waitSeconds(0.5)
-            .addTemporalMarker(19.0) {
-                intakeBeltSubsystem.intakePos()
-                elevatorSubsystem.setpoint = 0.0
-            }
-            .waitSeconds(1.5)
-            .addTemporalMarker(20.5) {
-                intakeBeltSubsystem.outtakePos()
-                armSubsystem.setpoint = 0.0
-            }
-            .waitSeconds(1.0)
-            .forward(2.0)
-            .addTemporalMarker(21.5) {
-                elevatorSubsystem.setpoint = 30.0
-                intakeBeltSubsystem.intakePos()
-            }
-            .turn(Math.toRadians(-60.0))
-            .waitSeconds(2.0)
-            .addTemporalMarker(23.5) {
-                intakeSubsystem.intake()
-            }
-            .waitSeconds(0.5)
-            .addTemporalMarker(24.0) {
-                intakeBeltSubsystem.outtakePos()
-                elevatorSubsystem.setpoint = 0.0
-            }
-            .waitSeconds(0.5)
-            .back(2.0)
-            .addTemporalMarker(24.5) {
-                armSubsystem.setpoint = Math.toRadians(98.0)
-                intakeBeltSubsystem.intakePos()
-            }
-            .waitSeconds(1.5)
-            .addTemporalMarker(27.0) {
-                elevatorSubsystem.setpoint = 33.0
-            }
-            .waitSeconds(1.5)
-            .addTemporalMarker(28.5) {
-                intakeBeltSubsystem.outtakePos()
-            }
-            .waitSeconds(0.5)
-            .addTemporalMarker(29.0) {
-                intakeSubsystem.outtake()
-            }
-            // .strafetolinearheading
-
-//            .waitSeconds(0.5)
-//            .addTemporalMarker(6.0) {
-//                intakeBeltSubsystem.outtakePos()
-//                armSubsystem.setpoint = 0.0
-//            }
-//            .turn(Math.toRadians(40.0))
-//            .waitSeconds(0.5)
-//            .addTemporalMarker(6.5) {
-//                elevatorSubsystem.setpoint = 30.0
-//            }
-//            .waitSeconds(1.0)
-//            .addTemporalMarker(7.5) {
-//                intakeBeltSubsystem.intakePos()
-//            }
-//            .waitSeconds(0.5)
-//            .addTemporalMarker(8.0) {
-//                intakeSubsystem.intake()
-//            }
-//            .waitSeconds(0.5)
-//            .addTemporalMarker(8.5) {
-//                elevatorSubsystem.setpoint = 0.0
-//                intakeBeltSubsystem.outtakePos()
-//            }
-//            .turn(Math.toRadians(-40.0))
-//            .waitSeconds(1.0)
-//            .addTemporalMarker(9.5) {
-//                armSubsystem.setpoint = Math.toRadians(98.0)
-//            }
-            .build()
-
-        driveSubsystem.poseEstimate = AutoStartPose.BLUE_LEFT.startPose
-        driveSubsystem.followTrajectorySequenceAsync(trajectory)
-    }
-
-    override fun loop() {
-        driveSubsystem.update()
-
-        elevatorSubsystem.periodic()
-        armSubsystem.periodic()
-
-        telemetry.addData("Arm Position", armSubsystem.armAngle)
-        telemetry.addData("Slides Position", elevatorSubsystem.slidePos)
-        telemetry.update()
-    }
-}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/Blue1Plus2V2.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/Blue1Plus2V2.kt
deleted file mode 100644
index 67197c4..0000000
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/Blue1Plus2V2.kt
+++ /dev/null
@@ -1,201 +0,0 @@
-package org.firstinspires.ftc.teamcode.opModes.auto.blue.basket.left
-
-import com.acmerobotics.dashboard.FtcDashboard
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry
-import com.acmerobotics.roadrunner.geometry.Pose2d
-import com.arcrobotics.ftclib.hardware.motors.Motor
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous
-import com.qualcomm.robotcore.eventloop.opmode.OpMode
-import com.qualcomm.robotcore.hardware.Servo
-import org.firstinspires.ftc.teamcode.constants.AutoStartPose
-import org.firstinspires.ftc.teamcode.constants.ControlBoard
-import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem
-import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
-import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeBeltSubsystem
-import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem
-import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem
-import org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence.TrajectorySequence
-
-@Autonomous(name = "V2 Blue 1 + 2", group = "Basket", preselectTeleOp = "MainTeleOp")
-class Blue1Plus2V2: OpMode() {
-    private lateinit var armLeft: Motor
-    private lateinit var armRight: Motor
-    private lateinit var elevatorLeft: Motor
-    private lateinit var elevatorRight: Motor
-
-    private lateinit var intake: Servo
-    private lateinit var intakeBelt: Servo
-
-    private lateinit var driveSubsystem: DriveSubsystem
-    private lateinit var intakeSubsystem: IntakeSubsystem
-    private lateinit var intakeBeltSubsystem: IntakeBeltSubsystem
-    private lateinit var armSubsystem: ArmSubsystem
-    private lateinit var elevatorSubsystem: ElevatorSubsystem
-
-    private lateinit var toBasket1: TrajectorySequence
-    private lateinit var toBasket2: TrajectorySequence
-
-    override fun init() {
-        telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry)
-
-        intake = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE.deviceName)
-        intakeBelt = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE_BELT.deviceName)
-
-        armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName)
-        armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName)
-
-        elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName)
-        elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName)
-
-        driveSubsystem = DriveSubsystem(hardwareMap)
-        intakeSubsystem = IntakeSubsystem(intake)
-        intakeBeltSubsystem = IntakeBeltSubsystem(intakeBelt)
-        armSubsystem = ArmSubsystem(armRight, armLeft)
-        elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle)
-
-        toBasket1 = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_LEFT.startPose)
-            .splineToLinearHeading(Pose2d(54.0, 56.0, Math.toRadians(225.0)), Math.toRadians(0.0))
-            .addTemporalMarker(0.1) {
-                intakeSubsystem.intake()
-                armSubsystem.setpoint = Math.toRadians(98.0)
-                intakeBeltSubsystem.intakePos()
-            }
-            .waitSeconds(0.5)
-            .addTemporalMarker(1.0) {
-                elevatorSubsystem.setpoint = 34.0
-                intakeBeltSubsystem.intakePos()
-            }
-            .waitSeconds(0.5)
-            .addTemporalMarker(1.5) {
-                intakeBeltSubsystem.outtakePos()
-            }
-            .waitSeconds(0.5)
-            .addTemporalMarker(2.0) {
-                intakeSubsystem.outtake()
-            }
-            .waitSeconds(0.1)
-            .addTemporalMarker(2.1) {
-                intakeBeltSubsystem.outtakePos()
-                elevatorSubsystem.setpoint = 0.0
-            }
-            .turn(Math.toRadians(37.0))
-            .waitSeconds(2.0)
-            .addTemporalMarker(4.1) {
-                armSubsystem.setpoint = Math.toRadians(0.0)
-            }
-            .waitSeconds(0.5)
-            .addTemporalMarker(4.6) {
-                elevatorSubsystem.setpoint = 19.0
-                intakeBeltSubsystem.intakePos()
-            }
-            .waitSeconds(1.25)
-            .addTemporalMarker(6.0) {
-                intakeSubsystem.intake()
-            }
-            .waitSeconds(0.25)
-            .turn(Math.toRadians(-37.0))
-            .addTemporalMarker(6.25) {
-                intakeBeltSubsystem.outtakePos()
-                elevatorSubsystem.setpoint = 0.0
-            }
-            .waitSeconds(1.0)
-            .addTemporalMarker(7.05) {
-                armSubsystem.setpoint = Math.toRadians(98.0)
-                intakeBeltSubsystem.intakePos()
-            }
-            .waitSeconds(0.75)
-            .addTemporalMarker(7.8) {
-                elevatorSubsystem.setpoint = 31.0
-            }
-            .waitSeconds(1.0)
-            .addTemporalMarker(8.8) {
-                intakeBeltSubsystem.outtakePos()
-            }
-            .waitSeconds(1.25)
-            .turn(Math.toRadians(55.0))
-            .addTemporalMarker(10.05) {
-                intakeSubsystem.outtake()
-            }
-            .waitSeconds(0.25)
-            .addTemporalMarker(10.3) {
-                intakeBeltSubsystem.intakePos()
-            }
-            .waitSeconds(0.5)
-            .addTemporalMarker(10.8) {
-                elevatorSubsystem.setpoint = 0.0
-            }
-            .waitSeconds(1.5)
-            .addTemporalMarker(12.3) {
-                armSubsystem.setpoint = 0.0
-                intakeBeltSubsystem.outtakePos()
-            }
-            .waitSeconds(1.0)
-            .addTemporalMarker(13.3) {
-                elevatorSubsystem.setpoint = 19.0
-            }
-            .waitSeconds(1.0)
-            .addTemporalMarker(14.3) {
-                intakeBeltSubsystem.intakePos()
-            }
-            .turn(Math.toRadians(-55.0))
-            .waitSeconds(1.2)
-            .back(2.0)
-            .addTemporalMarker(15.5) {
-                intakeSubsystem.intake()
-            }
-            .waitSeconds(0.5)
-            .addTemporalMarker(16.0) {
-                intakeBeltSubsystem.outtakePos()
-                elevatorSubsystem.setpoint = 0.0
-            }
-            .waitSeconds(1.0)
-            .addTemporalMarker(17.0) {
-                armSubsystem.setpoint = Math.toRadians(98.0)
-                intakeBeltSubsystem.intakePos()
-            }
-            .waitSeconds(1.0)
-            .addTemporalMarker(18.0) {
-                elevatorSubsystem.setpoint = 29.0
-            }
-            .waitSeconds(1.5)
-            .addTemporalMarker(19.5) {
-                intakeBeltSubsystem.outtakePos()
-            }
-            .waitSeconds(1.0)
-            .addTemporalMarker(20.5) {
-                intakeSubsystem.outtake()
-            }
-            .waitSeconds(0.5)
-            .addTemporalMarker(21.0) {
-                intakeBeltSubsystem.intakePos()
-            }
-            .waitSeconds(0.5)
-            .addTemporalMarker(21.5) {
-                elevatorSubsystem.setpoint = 0.0
-            }
-            .waitSeconds(1.0)
-            .addTemporalMarker(22.5) {
-                armSubsystem.setpoint = 0.0
-            }
-            .splineToSplineHeading(Pose2d(28.0, 10.0, Math.toRadians(0.0)), 90.0)
-            .back(5.0)
-            .addDisplacementMarker {
-                driveSubsystem.followTrajectorySequenceAsync(toBasket2)
-            }
-            .build()
-        driveSubsystem.poseEstimate = AutoStartPose.BLUE_LEFT.startPose
-        driveSubsystem.followTrajectorySequenceAsync(toBasket1)
-    }
-
-
-    override fun loop() {
-        driveSubsystem.update()
-
-        elevatorSubsystem.periodic()
-        armSubsystem.periodic()
-
-        telemetry.addData("Arm Position", armSubsystem.armAngle)
-        telemetry.addData("Slides Position", elevatorSubsystem.slidePos)
-        telemetry.update()
-    }
-}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/teleOp/MainTeleOp.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/teleOp/MainTeleOp.kt
index 4e68b02..a843761 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/teleOp/MainTeleOp.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/teleOp/MainTeleOp.kt
@@ -13,7 +13,7 @@ import org.firstinspires.ftc.teamcode.commands.intake.IntakeCommand
 import org.firstinspires.ftc.teamcode.subsystems.arm.OpenArmSubsystem
 import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
 import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem
-import org.firstinspires.ftc.teamcode.subsystems.slides.OpenSlidesSubsystem
+import org.firstinspires.ftc.teamcode.subsystems.slides.OpenElevatorSubsystem
 
 @TeleOp
 class MainTeleOp : CommandOpMode() {
@@ -32,13 +32,13 @@ class MainTeleOp : CommandOpMode() {
         driver = GamepadEx(gamepad1)
         operator = GamepadEx(gamepad2)
 
-        OpenSlidesSubsystem.initialize(hardwareMap)
+        OpenElevatorSubsystem.initialize(hardwareMap)
         OpenArmSubsystem.initialize(hardwareMap)
         DriveSubsystem.initialize(hardwareMap)
         IntakeSubsystem.initialize(hardwareMap)
 
-        spinUpCommand = SpinUpCommand(OpenSlidesSubsystem)
-        spinDownCommand = SpinDownCommand(OpenSlidesSubsystem)
+        spinUpCommand = SpinUpCommand(OpenElevatorSubsystem)
+        spinDownCommand = SpinDownCommand(OpenElevatorSubsystem)
         armUpCommand = OpenArmCommand(OpenArmSubsystem, true)
         armDownCommand = OpenArmCommand(OpenArmSubsystem, false)
         //intakeCommand = IntakeCommand(true, intakeSubsystem)
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/IntakeTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/IntakeTuner.kt
index 016ee4a..16f0c6c 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/IntakeTuner.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/IntakeTuner.kt
@@ -27,7 +27,7 @@ class IntakeTuner : CommandOpMode() {
         intake = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE.deviceName)
         intakeBelt = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE_BELT.deviceName)
 
-        intakeSubsystem = IntakeSubsystem(intake)
+        IntakeSubsystem.initialize(hardwareMap)
         intakeBeltSubsystem = IntakeBeltSubsystem(intakeBelt)
 
         RunCommand({
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesPIDTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesPIDTuner.kt
index 9c96cd0..010af0a 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesPIDTuner.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesPIDTuner.kt
@@ -42,11 +42,11 @@ class SlidesPIDTuner : CommandOpMode() {
         slidesLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName, Motor.GoBILDA.RPM_1150)
         slidesRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName, Motor.GoBILDA.RPM_1150)
 
-        armSubsystem = OpenArmSubsystem(armRight, armLeft) { 0.0 }
+        OpenArmSubsystem.initialize(hardwareMap)
         slidesSubsystem = ElevatorSubsystem(slidesRight, slidesLeft, armSubsystem::armAngle)
 
-        armUpCommand = OpenArmCommand(armSubsystem, true)
-        armDownCommand = OpenArmCommand(armSubsystem, false)
+        armUpCommand = OpenArmCommand(OpenArmSubsystem, true)
+        armDownCommand = OpenArmCommand(OpenArmSubsystem, false)
 
         operator.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER).whileHeld(armUpCommand)
         operator.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER).whileHeld(armDownCommand)
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Drawing.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/Drawing.kt
similarity index 90%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Drawing.kt
rename to TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/Drawing.kt
index 45466e1..9d8aabf 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Drawing.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/Drawing.kt
@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode
+package org.firstinspires.ftc.teamcode.roadrunner
 
 import com.acmerobotics.dashboard.canvas.Canvas
 import com.acmerobotics.roadrunner.Pose2d
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Localizer.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/Localizer.kt
similarity index 74%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Localizer.kt
rename to TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/Localizer.kt
index fa5428e..af85a51 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Localizer.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/Localizer.kt
@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode
+package org.firstinspires.ftc.teamcode.roadrunner
 
 import com.acmerobotics.roadrunner.Time
 import com.acmerobotics.roadrunner.Twist2dDual
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/ThreeDeadWheelLocalizer.kt
similarity index 89%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.kt
rename to TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/ThreeDeadWheelLocalizer.kt
index 975f549..2cc13d1 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/ThreeDeadWheelLocalizer.kt
@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode
+package org.firstinspires.ftc.teamcode.roadrunner
 
 import com.acmerobotics.dashboard.config.Config
 import com.acmerobotics.roadrunner.DualNum
@@ -6,13 +6,13 @@ import com.acmerobotics.roadrunner.Time
 import com.acmerobotics.roadrunner.Twist2dDual
 import com.acmerobotics.roadrunner.Vector2d
 import com.acmerobotics.roadrunner.Vector2dDual
-import com.acmerobotics.roadrunner.ftc.Encoder
 import com.acmerobotics.roadrunner.ftc.FlightRecorder.write
 import com.acmerobotics.roadrunner.ftc.OverflowEncoder
 import com.acmerobotics.roadrunner.ftc.RawEncoder
 import com.qualcomm.robotcore.hardware.DcMotorEx
 import com.qualcomm.robotcore.hardware.HardwareMap
-import org.firstinspires.ftc.teamcode.messages.ThreeDeadWheelInputsMessage
+import org.firstinspires.ftc.teamcode.constants.ControlBoard
+import org.firstinspires.ftc.teamcode.roadrunner.messages.ThreeDeadWheelInputsMessage
 
 @Config
 class ThreeDeadWheelLocalizer(hardwareMap: HardwareMap, val inPerTick: Double) : Localizer {
@@ -25,12 +25,12 @@ class ThreeDeadWheelLocalizer(hardwareMap: HardwareMap, val inPerTick: Double) :
     // TODO: make sure your config has **motors** with these names (or change them)
     //   the encoders should be plugged into the slot matching the named motor
     //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
-    val par0: Encoder =
-        OverflowEncoder(RawEncoder(hardwareMap.get(DcMotorEx::class.java, "par0")))
-    val par1: Encoder =
-        OverflowEncoder(RawEncoder(hardwareMap.get(DcMotorEx::class.java, "par1")))
-    val perp: Encoder =
-        OverflowEncoder(RawEncoder(hardwareMap.get(DcMotorEx::class.java, "perp")))
+    val par0 =
+        OverflowEncoder(RawEncoder(hardwareMap.get(DcMotorEx::class.java, ControlBoard.ODO_LEFT_ENCODER.deviceName)))
+    val par1 =
+        OverflowEncoder(RawEncoder(hardwareMap.get(DcMotorEx::class.java, ControlBoard.ODO_RIGHT_ENCODER.deviceName)))
+    val perp =
+        OverflowEncoder(RawEncoder(hardwareMap.get(DcMotorEx::class.java, ControlBoard.ODO_STRAFE_ENCODER.deviceName)))
 
     // TODO: reverse encoder directions if needed
     //   par0.setDirection(DcMotorSimple.Direction.REVERSE);
@@ -78,13 +78,13 @@ class ThreeDeadWheelLocalizer(hardwareMap: HardwareMap, val inPerTick: Double) :
                         (PARAMS.par0YTicks * par1PosDelta - PARAMS.par1YTicks * par0PosDelta) / (PARAMS.par0YTicks - PARAMS.par1YTicks),
                         (PARAMS.par0YTicks * par1PosVel.velocity - PARAMS.par1YTicks * par0PosVel.velocity) / (PARAMS.par0YTicks - PARAMS.par1YTicks),
                     )
-                ).times(inPerTick),
+                ) * inPerTick,
                 DualNum<Time>(
                     doubleArrayOf(
                         (PARAMS.perpXTicks / (PARAMS.par0YTicks - PARAMS.par1YTicks) * (par1PosDelta - par0PosDelta) + perpPosDelta),
                         (PARAMS.perpXTicks / (PARAMS.par0YTicks - PARAMS.par1YTicks) * (par1PosVel.velocity - par0PosVel.velocity) + perpPosVel.velocity),
                     )
-                ).times(inPerTick)
+                ) * inPerTick
             ),
             DualNum(
                 doubleArrayOf(
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/DriveCommandMessage.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/DriveCommandMessage.kt
similarity index 90%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/DriveCommandMessage.kt
rename to TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/DriveCommandMessage.kt
index 9a2b8bf..fabde81 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/DriveCommandMessage.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/DriveCommandMessage.kt
@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.messages
+package org.firstinspires.ftc.teamcode.roadrunner.messages
 
 import com.acmerobotics.roadrunner.PoseVelocity2dDual
 import com.acmerobotics.roadrunner.Time
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/MecanumCommandMessage.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/MecanumCommandMessage.kt
similarity index 79%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/MecanumCommandMessage.kt
rename to TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/MecanumCommandMessage.kt
index cd33c77..edd0e42 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/MecanumCommandMessage.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/MecanumCommandMessage.kt
@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.messages
+package org.firstinspires.ftc.teamcode.roadrunner.messages
 
 class MecanumCommandMessage(
     var voltage: Double,
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/MecanumLocalizerInputsMessage.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/MecanumLocalizerInputsMessage.kt
similarity index 93%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/MecanumLocalizerInputsMessage.kt
rename to TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/MecanumLocalizerInputsMessage.kt
index 1e0061e..f93e312 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/MecanumLocalizerInputsMessage.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/MecanumLocalizerInputsMessage.kt
@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.messages
+package org.firstinspires.ftc.teamcode.roadrunner.messages
 
 import com.acmerobotics.roadrunner.ftc.PositionVelocityPair
 import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/PoseMessage.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/PoseMessage.kt
similarity index 80%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/PoseMessage.kt
rename to TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/PoseMessage.kt
index 96ad295..81eb5c0 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/PoseMessage.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/PoseMessage.kt
@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.messages
+package org.firstinspires.ftc.teamcode.roadrunner.messages
 
 import com.acmerobotics.roadrunner.Pose2d
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/TankCommandMessage.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/TankCommandMessage.kt
similarity index 70%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/TankCommandMessage.kt
rename to TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/TankCommandMessage.kt
index e0811e9..de1571e 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/TankCommandMessage.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/TankCommandMessage.kt
@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.messages
+package org.firstinspires.ftc.teamcode.roadrunner.messages
 
 class TankCommandMessage(var voltage: Double, var leftPower: Double, var rightPower: Double) {
     var timestamp: Long = System.nanoTime()
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/TankLocalizerInputsMessage.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/TankLocalizerInputsMessage.kt
similarity index 87%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/TankLocalizerInputsMessage.kt
rename to TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/TankLocalizerInputsMessage.kt
index 0e636db..5ed861a 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/TankLocalizerInputsMessage.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/TankLocalizerInputsMessage.kt
@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.messages
+package org.firstinspires.ftc.teamcode.roadrunner.messages
 
 import com.acmerobotics.roadrunner.ftc.PositionVelocityPair
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/ThreeDeadWheelInputsMessage.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/ThreeDeadWheelInputsMessage.kt
similarity index 81%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/ThreeDeadWheelInputsMessage.kt
rename to TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/ThreeDeadWheelInputsMessage.kt
index 3ac4cd3..58262f0 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/ThreeDeadWheelInputsMessage.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/ThreeDeadWheelInputsMessage.kt
@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.messages
+package org.firstinspires.ftc.teamcode.roadrunner.messages
 
 import com.acmerobotics.roadrunner.ftc.PositionVelocityPair
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/TwoDeadWheelInputsMessage.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/TwoDeadWheelInputsMessage.kt
similarity index 95%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/TwoDeadWheelInputsMessage.kt
rename to TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/TwoDeadWheelInputsMessage.kt
index 680a49b..b565732 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/TwoDeadWheelInputsMessage.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/TwoDeadWheelInputsMessage.kt
@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.messages
+package org.firstinspires.ftc.teamcode.roadrunner.messages
 
 import com.acmerobotics.roadrunner.ftc.PositionVelocityPair
 import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/tuning/LocalizationTest.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/tuning/LocalizationTest.kt
new file mode 100644
index 0000000..b0e855f
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/tuning/LocalizationTest.kt
@@ -0,0 +1,55 @@
+package org.firstinspires.ftc.teamcode.roadrunner.tuning
+
+import com.acmerobotics.dashboard.FtcDashboard
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry
+import com.acmerobotics.dashboard.telemetry.TelemetryPacket
+import com.acmerobotics.roadrunner.PoseVelocity2d
+import com.acmerobotics.roadrunner.Vector2d
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode
+import org.firstinspires.ftc.teamcode.roadrunner.Drawing
+import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
+
+class LocalizationTest : LinearOpMode() {
+    @Throws(InterruptedException::class)
+    override fun runOpMode() {
+        telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry)
+
+        when (TuningOpModes.DRIVE_CLASS) {
+            DriveSubsystem::class.java -> {
+                DriveSubsystem.initialize(hardwareMap)
+
+                val drive = DriveSubsystem
+
+                waitForStart()
+
+                while (opModeIsActive()) {
+                    drive.setDrivePowers(
+                        PoseVelocity2d(
+                            Vector2d(
+                                -gamepad1.left_stick_y.toDouble(),
+                                -gamepad1.left_stick_x.toDouble()
+                            ),
+                            -gamepad1.right_stick_x.toDouble()
+                        )
+                    )
+
+                    drive.updatePoseEstimate()
+
+                    telemetry.addData("x", drive.pose.position.x)
+                    telemetry.addData("y", drive.pose.position.y)
+                    telemetry.addData("heading (deg)", Math.toDegrees(drive.pose.heading.toDouble()))
+                    telemetry.update()
+
+                    val packet = TelemetryPacket()
+                    packet.fieldOverlay().setStroke("#3F51B5")
+                    Drawing.drawRobot(packet.fieldOverlay(), drive.pose)
+                    FtcDashboard.getInstance().sendTelemetryPacket(packet)
+                }
+            }
+
+            else -> {
+                throw RuntimeException()
+            }
+        }
+    }
+}
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/tuning/ManualFeedbackTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/tuning/ManualFeedbackTuner.kt
new file mode 100644
index 0000000..18cbeb9
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/tuning/ManualFeedbackTuner.kt
@@ -0,0 +1,44 @@
+package org.firstinspires.ftc.teamcode.roadrunner.tuning
+
+import com.acmerobotics.roadrunner.Pose2d
+import com.acmerobotics.roadrunner.ftc.runBlocking
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode
+import org.firstinspires.ftc.teamcode.roadrunner.ThreeDeadWheelLocalizer
+import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
+
+class ManualFeedbackTuner : LinearOpMode() {
+    @Throws(InterruptedException::class)
+    override fun runOpMode() {
+        when (TuningOpModes.DRIVE_CLASS) {
+            DriveSubsystem::class.java -> {
+                DriveSubsystem.initialize(hardwareMap)
+
+                val drive = DriveSubsystem
+
+                if (drive.localizer is ThreeDeadWheelLocalizer) {
+                    if (ThreeDeadWheelLocalizer.PARAMS.perpXTicks == 0.0 && ThreeDeadWheelLocalizer.PARAMS.par0YTicks == 0.0 && ThreeDeadWheelLocalizer.Companion.PARAMS.par1YTicks == 1.0) {
+                        throw RuntimeException("Odometry wheel locations not set! Run AngularRampLogger to tune them.")
+                    }
+                }
+                waitForStart()
+
+                while (opModeIsActive()) {
+                    runBlocking(
+                        drive.actionBuilder(Pose2d(0.0, 0.0, 0.0))
+                            .lineToX(DISTANCE)
+                            .lineToX(0.0)
+                            .build()
+                    )
+                }
+            }
+
+            else -> {
+                throw RuntimeException()
+            }
+        }
+    }
+
+    companion object {
+        var DISTANCE: Double = 64.0
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/SplineTest.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/tuning/SplineTest.kt
similarity index 54%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/SplineTest.kt
rename to TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/tuning/SplineTest.kt
index f9015ae..f407efc 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/SplineTest.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/tuning/SplineTest.kt
@@ -1,31 +1,20 @@
-package org.firstinspires.ftc.teamcode.tuning
+package org.firstinspires.ftc.teamcode.roadrunner.tuning
 
 import com.acmerobotics.roadrunner.Pose2d
 import com.acmerobotics.roadrunner.Vector2d
 import com.acmerobotics.roadrunner.ftc.runBlocking
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode
-import org.firstinspires.ftc.teamcode.MecanumDrive
-import org.firstinspires.ftc.teamcode.TankDrive
+import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
 
 class SplineTest : LinearOpMode() {
     @Throws(InterruptedException::class)
     override fun runOpMode() {
         val beginPose = Pose2d(0.0, 0.0, 0.0)
         when (TuningOpModes.DRIVE_CLASS) {
-            MecanumDrive::class.java -> {
-                val drive = MecanumDrive(hardwareMap, beginPose)
+            DriveSubsystem::class.java -> {
+                DriveSubsystem.initialize(hardwareMap)
 
-                waitForStart()
-
-                runBlocking(
-                    drive.actionBuilder(beginPose)
-                        .splineTo(Vector2d(30.0, 30.0), Math.PI / 2)
-                        .splineTo(Vector2d(0.0, 60.0), Math.PI)
-                        .build()
-                )
-            }
-            TankDrive::class.java -> {
-                val drive = TankDrive(hardwareMap, beginPose)
+                val drive = DriveSubsystem
 
                 waitForStart()
 
@@ -36,6 +25,7 @@ class SplineTest : LinearOpMode() {
                         .build()
                 )
             }
+
             else -> {
                 throw RuntimeException()
             }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/tuning/TuningOpModes.kt
similarity index 56%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.kt
rename to TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/tuning/TuningOpModes.kt
index 0ea4239..35f305a 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/tuning/TuningOpModes.kt
@@ -1,10 +1,9 @@
-package org.firstinspires.ftc.teamcode.tuning
+package org.firstinspires.ftc.teamcode.roadrunner.tuning
 
 import com.acmerobotics.dashboard.FtcDashboard
 import com.acmerobotics.dashboard.config.reflection.ReflectionConfig
 import com.acmerobotics.dashboard.config.variable.CustomVariable
 import com.acmerobotics.roadrunner.MotorFeedforward
-import com.acmerobotics.roadrunner.Pose2d
 import com.acmerobotics.roadrunner.ftc.AngularRampLogger
 import com.acmerobotics.roadrunner.ftc.DeadWheelDirectionDebugger
 import com.acmerobotics.roadrunner.ftc.DriveType
@@ -21,19 +20,15 @@ import com.qualcomm.hardware.lynx.LynxModule
 import com.qualcomm.robotcore.eventloop.opmode.OpMode
 import com.qualcomm.robotcore.eventloop.opmode.OpModeManager
 import com.qualcomm.robotcore.eventloop.opmode.OpModeRegistrar
-import com.qualcomm.robotcore.hardware.DcMotorEx
 import com.qualcomm.robotcore.hardware.HardwareMap
 import org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.hardwareMap
 import org.firstinspires.ftc.robotcore.internal.opmode.OpModeMeta
-import org.firstinspires.ftc.teamcode.MecanumDrive
-import org.firstinspires.ftc.teamcode.TankDrive
-import org.firstinspires.ftc.teamcode.ThreeDeadWheelLocalizer
-import org.firstinspires.ftc.teamcode.TwoDeadWheelLocalizer
-import java.util.Arrays
+import org.firstinspires.ftc.teamcode.roadrunner.ThreeDeadWheelLocalizer
+import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
 
 object TuningOpModes {
     // TODO: change this to TankDrive.class if you're using tank
-    val DRIVE_CLASS: Class<*> = MecanumDrive::class.java
+    val DRIVE_CLASS: Class<*> = DriveSubsystem::class.java
 
     const val GROUP: String = "quickstart"
     const val DISABLED: Boolean = false
@@ -52,38 +47,45 @@ object TuningOpModes {
 
         val dvf: DriveViewFactory
         when (DRIVE_CLASS) {
-            MecanumDrive::class.java -> {
+            DriveSubsystem::class.java -> {
                 dvf = object : DriveViewFactory {
                     override fun make(h: HardwareMap): DriveView {
-                        val md = MecanumDrive(hardwareMap, Pose2d(0.0, 0.0, 0.0))
+                        DriveSubsystem.initialize(hardwareMap)
+
+                        val md = DriveSubsystem
+
                         val leftEncs: MutableList<Encoder> = ArrayList()
                         val rightEncs: MutableList<Encoder> = ArrayList()
                         val parEncs: MutableList<Encoder> = ArrayList()
                         val perpEncs: MutableList<Encoder> = ArrayList()
-                        if (md.localizer is MecanumDrive.DriveLocalizer) {
-                            val dl = md.localizer
-                            leftEncs.add(dl.leftFront)
-                            leftEncs.add(dl.leftBack)
-                            rightEncs.add(dl.rightFront)
-                            rightEncs.add(dl.rightBack)
-                        } else if (md.localizer is ThreeDeadWheelLocalizer) {
-                            val dl = md.localizer
-                            parEncs.add(dl.par0)
-                            parEncs.add(dl.par1)
-                            perpEncs.add(dl.perp)
-                        } else if (md.localizer is TwoDeadWheelLocalizer) {
-                            val dl = md.localizer
-                            parEncs.add(dl.par)
-                            perpEncs.add(dl.perp)
-                        } else {
-                            throw RuntimeException("unknown localizer: " + md.localizer.javaClass.name)
+
+                        when (md.localizer) {
+                            is DriveSubsystem.DriveLocalizer -> {
+                                val dl = md.localizer
+                                leftEncs.add(dl.leftFront)
+                                leftEncs.add(dl.leftBack)
+                                rightEncs.add(dl.rightFront)
+                                rightEncs.add(dl.rightBack)
+                            }
+
+                            is ThreeDeadWheelLocalizer -> {
+                                val dl = md.localizer
+                                parEncs.add(dl.par0)
+                                parEncs.add(dl.par1)
+                                perpEncs.add(dl.perp)
+                            }
+
+                            else -> {
+                                throw RuntimeException("unknown localizer: " + md.localizer.javaClass.name)
+                            }
                         }
+
                         return DriveView(
                             DriveType.MECANUM,
-                            MecanumDrive.PARAMS.inPerTick,
-                            MecanumDrive.PARAMS.maxWheelVel,
-                            MecanumDrive.PARAMS.minProfileAccel,
-                            MecanumDrive.PARAMS.maxProfileAccel,
+                            DriveSubsystem.PARAMS.inPerTick,
+                            DriveSubsystem.PARAMS.maxWheelVel,
+                            DriveSubsystem.PARAMS.minProfileAccel,
+                            DriveSubsystem.PARAMS.maxProfileAccel,
                             hardwareMap.getAll(LynxModule::class.java),
                             listOf(
                                 md.leftFront,
@@ -101,71 +103,15 @@ object TuningOpModes {
                             md.voltageSensor
                         ) {
                             MotorFeedforward(
-                                MecanumDrive.PARAMS.kS,
-                                MecanumDrive.PARAMS.kV / MecanumDrive.PARAMS.inPerTick,
-                                MecanumDrive.PARAMS.kA / MecanumDrive.PARAMS.inPerTick
+                                DriveSubsystem.PARAMS.kS,
+                                DriveSubsystem.PARAMS.kV / DriveSubsystem.PARAMS.inPerTick,
+                                DriveSubsystem.PARAMS.kA / DriveSubsystem.PARAMS.inPerTick
                             )
                         }
                     }
                 }
             }
-            TankDrive::class.java -> {
-                dvf = object : DriveViewFactory {
-                    override fun make(h: HardwareMap): DriveView {
-                        val td = TankDrive(hardwareMap, Pose2d(0.0, 0.0, 0.0))
-                        val leftEncs: MutableList<Encoder> = ArrayList()
-                        val rightEncs: MutableList<Encoder> = ArrayList()
-                        val parEncs: MutableList<Encoder> = ArrayList()
-                        val perpEncs: MutableList<Encoder> = ArrayList()
-                        when (td.localizer) {
-                            is TankDrive.DriveLocalizer -> {
-                                val dl = td.localizer
-                                leftEncs.addAll(dl.leftEncs)
-                                rightEncs.addAll(dl.rightEncs)
-                            }
-
-                            is ThreeDeadWheelLocalizer -> {
-                                val dl = td.localizer
-                                parEncs.add(dl.par0)
-                                parEncs.add(dl.par1)
-                                perpEncs.add(dl.perp)
-                            }
-
-                            is TwoDeadWheelLocalizer -> {
-                                val dl = td.localizer
-                                parEncs.add(dl.par)
-                                perpEncs.add(dl.perp)
-                            }
 
-                            else -> {
-                                throw RuntimeException("unknown localizer: " + td.localizer.javaClass.name)
-                            }
-                        }
-                        return DriveView(
-                            DriveType.TANK,
-                            TankDrive.PARAMS.inPerTick,
-                            TankDrive.PARAMS.maxWheelVel,
-                            TankDrive.PARAMS.minProfileAccel,
-                            TankDrive.PARAMS.maxProfileAccel,
-                            hardwareMap.getAll(LynxModule::class.java),
-                            td.leftMotors,
-                            td.rightMotors,
-                            leftEncs,
-                            rightEncs,
-                            parEncs,
-                            perpEncs,
-                            td.lazyImu,
-                            td.voltageSensor
-                        ) {
-                            MotorFeedforward(
-                                TankDrive.PARAMS.kS,
-                                TankDrive.PARAMS.kV / TankDrive.PARAMS.inPerTick,
-                                TankDrive.PARAMS.kA / TankDrive.PARAMS.inPerTick
-                            )
-                        }
-                    }
-                }
-            }
             else -> {
                 throw RuntimeException()
             }
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/OpenArmSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/OpenArmSubsystem.kt
index 2102f85..d185d1c 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/OpenArmSubsystem.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/OpenArmSubsystem.kt
@@ -8,7 +8,6 @@ import com.arcrobotics.ftclib.hardware.motors.MotorGroup
 import com.qualcomm.robotcore.hardware.HardwareMap
 import org.firstinspires.ftc.teamcode.constants.ArmConstants
 import org.firstinspires.ftc.teamcode.constants.ControlBoard
-import org.firstinspires.ftc.teamcode.opModes.teleOp.MainTeleOp.Companion.kT
 import kotlin.math.PI
 import kotlin.math.cos
 
@@ -42,6 +41,6 @@ object OpenArmSubsystem : SubsystemBase() {
     }
 
     fun stop() {
-        turnMotors.set(ArmConstants.kCos.value * cos(armAngle) + kT * 0.0)
+        turnMotors.set(ArmConstants.kCos.value * cos(armAngle))
     }
 }
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/drive/DriveSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/drive/DriveSubsystem.kt
index c3deca6..dd42eab 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/drive/DriveSubsystem.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/drive/DriveSubsystem.kt
@@ -1,14 +1,534 @@
 package org.firstinspires.ftc.teamcode.subsystems.drive
 
+import com.acmerobotics.dashboard.canvas.Canvas
+import com.acmerobotics.dashboard.telemetry.TelemetryPacket
+import com.acmerobotics.roadrunner.AccelConstraint
+import com.acmerobotics.roadrunner.Action
+import com.acmerobotics.roadrunner.AngularVelConstraint
+import com.acmerobotics.roadrunner.DualNum
+import com.acmerobotics.roadrunner.HolonomicController
+import com.acmerobotics.roadrunner.MecanumKinematics
+import com.acmerobotics.roadrunner.MecanumKinematics.WheelIncrements
+import com.acmerobotics.roadrunner.MinVelConstraint
+import com.acmerobotics.roadrunner.MotorFeedforward
+import com.acmerobotics.roadrunner.Pose2d
+import com.acmerobotics.roadrunner.PoseVelocity2d
+import com.acmerobotics.roadrunner.PoseVelocity2dDual
+import com.acmerobotics.roadrunner.ProfileAccelConstraint
+import com.acmerobotics.roadrunner.ProfileParams
+import com.acmerobotics.roadrunner.Rotation2d
+import com.acmerobotics.roadrunner.Time
+import com.acmerobotics.roadrunner.TimeTrajectory
+import com.acmerobotics.roadrunner.TimeTurn
+import com.acmerobotics.roadrunner.TrajectoryActionBuilder
+import com.acmerobotics.roadrunner.TrajectoryBuilderParams
+import com.acmerobotics.roadrunner.TurnConstraints
+import com.acmerobotics.roadrunner.Twist2dDual
+import com.acmerobotics.roadrunner.Vector2d
+import com.acmerobotics.roadrunner.Vector2dDual
+import com.acmerobotics.roadrunner.VelConstraint
+import com.acmerobotics.roadrunner.ftc.DownsampledWriter
+import com.acmerobotics.roadrunner.ftc.Encoder
+import com.acmerobotics.roadrunner.ftc.FlightRecorder.write
+import com.acmerobotics.roadrunner.ftc.LazyImu
+import com.acmerobotics.roadrunner.ftc.OverflowEncoder
+import com.acmerobotics.roadrunner.ftc.RawEncoder
+import com.acmerobotics.roadrunner.ftc.throwIfModulesAreOutdated
+import com.acmerobotics.roadrunner.now
+import com.acmerobotics.roadrunner.range
 import com.arcrobotics.ftclib.command.SubsystemBase
+import com.qualcomm.hardware.lynx.LynxModule
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot.UsbFacingDirection
+import com.qualcomm.robotcore.hardware.DcMotor
+import com.qualcomm.robotcore.hardware.DcMotorEx
 import com.qualcomm.robotcore.hardware.HardwareMap
+import com.qualcomm.robotcore.hardware.IMU
+import com.qualcomm.robotcore.hardware.VoltageSensor
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit
+import org.firstinspires.ftc.teamcode.constants.ControlBoard
+import org.firstinspires.ftc.teamcode.roadrunner.Drawing
+import org.firstinspires.ftc.teamcode.roadrunner.Localizer
+import org.firstinspires.ftc.teamcode.roadrunner.messages.DriveCommandMessage
+import org.firstinspires.ftc.teamcode.roadrunner.messages.MecanumCommandMessage
+import org.firstinspires.ftc.teamcode.roadrunner.messages.MecanumLocalizerInputsMessage
+import org.firstinspires.ftc.teamcode.roadrunner.messages.PoseMessage
+import java.util.LinkedList
+import kotlin.math.ceil
+import kotlin.math.max
 
 object DriveSubsystem : SubsystemBase() {
     private lateinit var hardwareMap: HardwareMap
+    var pose: Pose2d = Pose2d(0.0, 0.0, 0.0)
 
-    fun initialize(hardwareMap: HardwareMap) {
+    var PARAMS: Params = Params()
+
+    fun initialize(hardwareMap: HardwareMap,
+                   pose2d: Pose2d = Pose2d(0.0, 0.0, 0.0)
+    ) {
         this.hardwareMap = hardwareMap
+        this.pose = pose2d
     }
 
-    fun drive(leftY: Double, leftX: Double, rightX: Double) = 0.0
+    class Params {
+        // IMU orientation
+        // TODO: fill in these values based on
+        //   see https://ftc-docs.firstinspires.org/en/latest/programming_resources/imu/imu.html?highlight=imu#physical-hub-mounting
+        var logoFacingDirection: RevHubOrientationOnRobot.LogoFacingDirection =
+            RevHubOrientationOnRobot.LogoFacingDirection.UP
+        var usbFacingDirection: UsbFacingDirection = UsbFacingDirection.FORWARD
+
+        // drive model parameters
+        var inPerTick: Double = 1.0
+        var lateralInPerTick: Double = inPerTick
+        var trackWidthTicks: Double = 0.0
+
+        // feedforward parameters (in tick units)
+        var kS: Double = 0.0
+        var kV: Double = 0.0
+        var kA: Double = 0.0
+
+        // path profile parameters (in inches)
+        var maxWheelVel: Double = 50.0
+        var minProfileAccel: Double = -30.0
+        var maxProfileAccel: Double = 50.0
+
+        // turn profile parameters (in radians)
+        var maxAngVel: Double = Math.PI // shared with path
+        var maxAngAccel: Double = Math.PI
+
+        // path controller gains
+        var axialGain: Double = 0.0
+        var lateralGain: Double = 0.0
+        var headingGain: Double = 0.0 // shared with turn
+
+        var axialVelGain: Double = 0.0
+        var lateralVelGain: Double = 0.0
+        var headingVelGain: Double = 0.0 // shared with turn
+    }
+
+    val kinematics: MecanumKinematics = MecanumKinematics(
+        PARAMS.inPerTick * PARAMS.trackWidthTicks, PARAMS.inPerTick / PARAMS.lateralInPerTick
+    )
+
+    val defaultTurnConstraints: TurnConstraints = TurnConstraints(
+        PARAMS.maxAngVel, -PARAMS.maxAngAccel, PARAMS.maxAngAccel
+    )
+    val defaultVelConstraint: VelConstraint = MinVelConstraint(
+        listOf(
+            kinematics.WheelVelConstraint(PARAMS.maxWheelVel),
+            AngularVelConstraint(PARAMS.maxAngVel)
+        )
+    )
+    val defaultAccelConstraint: AccelConstraint =
+        ProfileAccelConstraint(PARAMS.minProfileAccel, PARAMS.maxProfileAccel)
+
+    val leftFront: DcMotorEx
+    val leftBack: DcMotorEx
+    val rightBack: DcMotorEx
+    val rightFront: DcMotorEx
+
+    val voltageSensor: VoltageSensor
+
+    val lazyImu: LazyImu
+
+    val localizer: Localizer
+
+    private val poseHistory = LinkedList<Pose2d>()
+
+    private val estimatedPoseWriter = DownsampledWriter("ESTIMATED_POSE", 50000000)
+    private val targetPoseWriter = DownsampledWriter("TARGET_POSE", 50000000)
+    private val driveCommandWriter = DownsampledWriter("DRIVE_COMMAND", 50000000)
+    private val mecanumCommandWriter = DownsampledWriter("MECANUM_COMMAND", 50000000)
+
+    class DriveLocalizer : Localizer {
+        val leftFront: Encoder =
+            OverflowEncoder(RawEncoder(DriveSubsystem.leftFront))
+        val leftBack: Encoder =
+            OverflowEncoder(RawEncoder(DriveSubsystem.leftBack))
+        val rightBack: Encoder =
+            OverflowEncoder(RawEncoder(DriveSubsystem.rightBack))
+        val rightFront: Encoder =
+            OverflowEncoder(RawEncoder(DriveSubsystem.rightFront))
+        val imu: IMU = lazyImu.get()
+
+        private var lastLeftFrontPos = 0
+        private var lastLeftBackPos = 0
+        private var lastRightBackPos = 0
+        private var lastRightFrontPos = 0
+        private var lastHeading: Rotation2d = Rotation2d.exp(imu.robotYawPitchRollAngles.getYaw(AngleUnit.RADIANS))
+        private var initialized = false
+
+//        init {
+            // TODO: reverse encoders if needed
+            //   leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
+//        }
+
+        override fun update(): Twist2dDual<Time> {
+            val leftFrontPosVel = leftFront.getPositionAndVelocity()
+            val leftBackPosVel = leftBack.getPositionAndVelocity()
+            val rightBackPosVel = rightBack.getPositionAndVelocity()
+            val rightFrontPosVel = rightFront.getPositionAndVelocity()
+
+            val angles = imu.robotYawPitchRollAngles
+
+            write(
+                "MECANUM_LOCALIZER_INPUTS", MecanumLocalizerInputsMessage(
+                    leftFrontPosVel, leftBackPosVel, rightBackPosVel, rightFrontPosVel, angles
+                )
+            )
+
+            val heading = Rotation2d.exp(angles.getYaw(AngleUnit.RADIANS))
+
+            if (!initialized) {
+                initialized = true
+
+                lastLeftFrontPos = leftFrontPosVel.position
+                lastLeftBackPos = leftBackPosVel.position
+                lastRightBackPos = rightBackPosVel.position
+                lastRightFrontPos = rightFrontPosVel.position
+
+                lastHeading = heading
+
+                return Twist2dDual(
+                    Vector2dDual.constant(Vector2d(0.0, 0.0), 2),
+                    DualNum.constant(0.0, 2)
+                )
+            }
+
+            val headingDelta = heading - lastHeading
+            val twist = kinematics.forward(
+                WheelIncrements(
+                    DualNum<Time>(
+                        doubleArrayOf(
+                            (leftFrontPosVel.position - lastLeftFrontPos).toDouble(),
+                            leftFrontPosVel.velocity.toDouble(),
+                        )
+                    ).times(PARAMS.inPerTick),
+                    DualNum<Time>(
+                        doubleArrayOf(
+                            (leftBackPosVel.position - lastLeftBackPos).toDouble(),
+                            leftBackPosVel.velocity.toDouble(),
+                        )
+                    ).times(PARAMS.inPerTick),
+                    DualNum<Time>(
+                        doubleArrayOf(
+                            (rightBackPosVel.position - lastRightBackPos).toDouble(),
+                            rightBackPosVel.velocity.toDouble(),
+                        )
+                    ).times(PARAMS.inPerTick),
+                    DualNum<Time>(
+                        doubleArrayOf(
+                            (rightFrontPosVel.position - lastRightFrontPos).toDouble(),
+                            rightFrontPosVel.velocity.toDouble(),
+                        )
+                    ).times(PARAMS.inPerTick)
+                )
+            )
+
+            lastLeftFrontPos = leftFrontPosVel.position
+            lastLeftBackPos = leftBackPosVel.position
+            lastRightBackPos = rightBackPosVel.position
+            lastRightFrontPos = rightFrontPosVel.position
+
+            lastHeading = heading
+
+            return Twist2dDual(
+                twist.line,
+                DualNum.cons(headingDelta, twist.angle.drop(1))
+            )
+        }
+    }
+
+    init {
+        throwIfModulesAreOutdated(hardwareMap)
+
+        for (module in hardwareMap.getAll(LynxModule::class.java)) {
+            module.bulkCachingMode = LynxModule.BulkCachingMode.AUTO
+        }
+
+        // TODO: make sure your config has motors with these names (or change them)
+        //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
+        leftFront = hardwareMap.get(DcMotorEx::class.java, ControlBoard.DRIVE_LEFT_FRONT.deviceName)
+        leftBack = hardwareMap.get(DcMotorEx::class.java, ControlBoard.DRIVE_LEFT_REAR.deviceName)
+        rightBack = hardwareMap.get(DcMotorEx::class.java, ControlBoard.DRIVE_RIGHT_REAR.deviceName)
+        rightFront = hardwareMap.get(DcMotorEx::class.java, ControlBoard.DRIVE_RIGHT_FRONT.deviceName)
+
+        leftFront.zeroPowerBehavior = DcMotor.ZeroPowerBehavior.BRAKE
+        leftBack.zeroPowerBehavior = DcMotor.ZeroPowerBehavior.BRAKE
+        rightBack.zeroPowerBehavior = DcMotor.ZeroPowerBehavior.BRAKE
+        rightFront.zeroPowerBehavior = DcMotor.ZeroPowerBehavior.BRAKE
+
+        // TODO: reverse motor directions if needed
+        //   leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
+
+        // TODO: make sure your config has an IMU with this name (can be BNO or BHI)
+        //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
+        lazyImu = LazyImu(
+            hardwareMap, "imu", RevHubOrientationOnRobot(
+                PARAMS.logoFacingDirection, PARAMS.usbFacingDirection
+            )
+        )
+
+        voltageSensor = hardwareMap.voltageSensor.iterator().next()
+
+        localizer = DriveLocalizer()
+
+        write("MECANUM_PARAMS", PARAMS)
+    }
+
+    fun setDrivePowers(powers: PoseVelocity2d) {
+        val wheelVels: MecanumKinematics.WheelVelocities<Time> = MecanumKinematics(1.0).inverse(
+            PoseVelocity2dDual.constant(powers, 1)
+        )
+
+        var maxPowerMag = 1.0
+        for (power in wheelVels.all()) {
+            maxPowerMag = max(maxPowerMag, power.value())
+        }
+
+        leftFront.power = wheelVels.leftFront[0] / maxPowerMag
+        leftBack.power = wheelVels.leftBack[0] / maxPowerMag
+        rightBack.power = wheelVels.rightBack[0] / maxPowerMag
+        rightFront.power = wheelVels.rightFront[0] / maxPowerMag
+    }
+
+    class FollowTrajectoryAction(val timeTrajectory: TimeTrajectory) : Action {
+        private var beginTs = -1.0
+
+        private val xPoints: DoubleArray
+        private val yPoints: DoubleArray
+
+        init {
+            val disps = range(
+                0.0, timeTrajectory.path.length(),
+                max(
+                    2.0,
+                    (ceil(timeTrajectory.path.length() / 2).toInt()).toDouble()
+                ).toInt()
+            )
+            xPoints = DoubleArray(disps.size)
+            yPoints = DoubleArray(disps.size)
+            for (i in disps.indices) {
+                val p = timeTrajectory.path[disps[i], 1].value()
+                xPoints[i] = p.position.x
+                yPoints[i] = p.position.y
+            }
+        }
+
+        override fun run(p: TelemetryPacket): Boolean {
+            val t: Double
+            if (beginTs < 0) {
+                beginTs = now()
+                t = 0.0
+            } else {
+                t = now() - beginTs
+            }
+
+            if (t >= timeTrajectory.duration) {
+                leftFront.power = 0.0
+                leftBack.power = 0.0
+                rightBack.power = 0.0
+                rightFront.power = 0.0
+
+                return false
+            }
+
+            val txWorldTarget = timeTrajectory[t]
+            targetPoseWriter.write(PoseMessage(txWorldTarget.value()))
+
+            val robotVelRobot = updatePoseEstimate()
+
+            val command: PoseVelocity2dDual<Time> = HolonomicController(
+                PARAMS.axialGain, PARAMS.lateralGain, PARAMS.headingGain,
+                PARAMS.axialVelGain, PARAMS.lateralVelGain, PARAMS.headingVelGain
+            )
+                .compute(txWorldTarget, pose, robotVelRobot)
+            driveCommandWriter.write(DriveCommandMessage(command))
+
+            val wheelVels: MecanumKinematics.WheelVelocities<Time> = kinematics.inverse(command)
+            val voltage = voltageSensor.voltage
+
+            val feedforward = MotorFeedforward(
+                PARAMS.kS,
+                PARAMS.kV / PARAMS.inPerTick, PARAMS.kA / PARAMS.inPerTick
+            )
+            val leftFrontPower = feedforward.compute(wheelVels.leftFront) / voltage
+            val leftBackPower = feedforward.compute(wheelVels.leftBack) / voltage
+            val rightBackPower = feedforward.compute(wheelVels.rightBack) / voltage
+            val rightFrontPower = feedforward.compute(wheelVels.rightFront) / voltage
+            mecanumCommandWriter.write(
+                MecanumCommandMessage(
+                    voltage, leftFrontPower, leftBackPower, rightBackPower, rightFrontPower
+                )
+            )
+
+            leftFront.power = leftFrontPower
+            leftBack.power = leftBackPower
+            rightBack.power = rightBackPower
+            rightFront.power = rightFrontPower
+
+            p.put("x", pose.position.x)
+            p.put("y", pose.position.y)
+            p.put("heading (deg)", Math.toDegrees(pose.heading.toDouble()))
+
+            val error = txWorldTarget.value().minusExp(pose)
+            p.put("xError", error.position.x)
+            p.put("yError", error.position.y)
+            p.put("headingError (deg)", Math.toDegrees(error.heading.toDouble()))
+
+            // only draw when active; only one drive action should be active at a time
+            val c = p.fieldOverlay()
+            drawPoseHistory(c)
+
+            c.setStroke("#4CAF50")
+            Drawing.drawRobot(c, txWorldTarget.value())
+
+            c.setStroke("#3F51B5")
+            Drawing.drawRobot(c, pose)
+
+            c.setStroke("#4CAF50FF")
+            c.setStrokeWidth(1)
+            c.strokePolyline(xPoints, yPoints)
+
+            return true
+        }
+
+        override fun preview(fieldOverlay: Canvas) {
+            fieldOverlay.setStroke("#4CAF507A")
+            fieldOverlay.setStrokeWidth(1)
+            fieldOverlay.strokePolyline(xPoints, yPoints)
+        }
+    }
+
+    class TurnAction(private val turn: TimeTurn) : Action {
+        private var beginTs = -1.0
+
+        override fun run(p: TelemetryPacket): Boolean {
+            val t: Double
+            if (beginTs < 0) {
+                beginTs = now()
+                t = 0.0
+            } else {
+                t = now() - beginTs
+            }
+
+            if (t >= turn.duration) {
+                leftFront.power = 0.0
+                leftBack.power = 0.0
+                rightBack.power = 0.0
+                rightFront.power = 0.0
+
+                return false
+            }
+
+            val txWorldTarget = turn[t]
+            targetPoseWriter.write(PoseMessage(txWorldTarget.value()))
+
+            val robotVelRobot = updatePoseEstimate()
+
+            val command: PoseVelocity2dDual<Time> = HolonomicController(
+                PARAMS.axialGain, PARAMS.lateralGain, PARAMS.headingGain,
+                PARAMS.axialVelGain, PARAMS.lateralVelGain, PARAMS.headingVelGain
+            )
+                .compute(txWorldTarget, pose, robotVelRobot)
+            driveCommandWriter.write(DriveCommandMessage(command))
+
+            val wheelVels: MecanumKinematics.WheelVelocities<Time> = kinematics.inverse(command)
+            val voltage = voltageSensor.voltage
+            val feedforward = MotorFeedforward(
+                PARAMS.kS,
+                PARAMS.kV / PARAMS.inPerTick, PARAMS.kA / PARAMS.inPerTick
+            )
+            val leftFrontPower = feedforward.compute(wheelVels.leftFront) / voltage
+            val leftBackPower = feedforward.compute(wheelVels.leftBack) / voltage
+            val rightBackPower = feedforward.compute(wheelVels.rightBack) / voltage
+            val rightFrontPower = feedforward.compute(wheelVels.rightFront) / voltage
+            mecanumCommandWriter.write(
+                MecanumCommandMessage(
+                    voltage, leftFrontPower, leftBackPower, rightBackPower, rightFrontPower
+                )
+            )
+
+            leftFront.power = feedforward.compute(wheelVels.leftFront) / voltage
+            leftBack.power = feedforward.compute(wheelVels.leftBack) / voltage
+            rightBack.power = feedforward.compute(wheelVels.rightBack) / voltage
+            rightFront.power = feedforward.compute(wheelVels.rightFront) / voltage
+
+            val c = p.fieldOverlay()
+            drawPoseHistory(c)
+
+            c.setStroke("#4CAF50")
+            Drawing.drawRobot(c, txWorldTarget.value())
+
+            c.setStroke("#3F51B5")
+            Drawing.drawRobot(c, pose)
+
+            c.setStroke("#7C4DFFFF")
+            c.fillCircle(turn.beginPose.position.x, turn.beginPose.position.y, 2.0)
+
+            return true
+        }
+
+        override fun preview(fieldOverlay: Canvas) {
+            fieldOverlay.setStroke("#7C4DFF7A")
+            fieldOverlay.fillCircle(turn.beginPose.position.x, turn.beginPose.position.y, 2.0)
+        }
+    }
+
+    fun updatePoseEstimate(): PoseVelocity2d {
+        val twist = localizer.update()
+        pose = pose.plus(twist.value())
+
+        poseHistory.add(pose)
+        while (poseHistory.size > 100) {
+            poseHistory.removeFirst()
+        }
+
+        estimatedPoseWriter.write(PoseMessage(pose))
+
+        return twist.velocity().value()
+    }
+
+    private fun drawPoseHistory(c: Canvas) {
+        val xPoints = DoubleArray(poseHistory.size)
+        val yPoints = DoubleArray(poseHistory.size)
+
+        var i = 0
+        for ((position) in poseHistory) {
+            xPoints[i] = position.x
+            yPoints[i] = position.y
+
+            i++
+        }
+
+        c.setStrokeWidth(1)
+        c.setStroke("#3F51B5")
+        c.strokePolyline(xPoints, yPoints)
+    }
+
+    fun actionBuilder(beginPose: Pose2d): TrajectoryActionBuilder {
+        return TrajectoryActionBuilder(
+            { turn: TimeTurn -> TurnAction(turn) },
+            { t: TimeTrajectory -> FollowTrajectoryAction(t) },
+            TrajectoryBuilderParams(
+                1e-6,
+                ProfileParams(
+                    0.25, 0.1, 1e-2
+                )
+            ),
+            beginPose, 0.0,
+            defaultTurnConstraints,
+            defaultVelConstraint, defaultAccelConstraint
+        )
+    }
+
+    fun drive(leftY: Double, leftX: Double, rightX: Double) {
+        setDrivePowers(
+            PoseVelocity2d(
+                linearVel = Vector2d(leftX, leftY),
+                angVel = rightX
+            )
+        )
+    }
 }
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/OpenElevatorSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/OpenElevatorSubsystem.kt
index fd8441e..b6660a6 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/OpenElevatorSubsystem.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/OpenElevatorSubsystem.kt
@@ -3,27 +3,30 @@ package org.firstinspires.ftc.teamcode.subsystems.slides
 import com.arcrobotics.ftclib.command.SubsystemBase
 import com.arcrobotics.ftclib.hardware.motors.Motor
 import com.arcrobotics.ftclib.hardware.motors.MotorGroup
+import com.qualcomm.robotcore.hardware.HardwareMap
+import org.firstinspires.ftc.teamcode.constants.ControlBoard
 import org.firstinspires.ftc.teamcode.constants.SlidesConstants
+import org.firstinspires.ftc.teamcode.subsystems.arm.OpenArmSubsystem
 import kotlin.math.sin
 
-class OpenElevatorSubsystem(
-    //sets them as a private variable thats a motor (same name as in driver hub)
-    elevatorRight : Motor,
-    elevatorLeft : Motor,
-    private val slideAngle: () -> Double
-) : SubsystemBase() {
-    //makes a motor group bc you have to move them at the same time
-    private val elevatorMotors = MotorGroup(elevatorRight, elevatorLeft)
+object OpenElevatorSubsystem: SubsystemBase() {
+    private lateinit var elevatorMotors: MotorGroup
+
     val slidePos: Double
         get() = elevatorMotors.positions[0] * SlidesConstants.MAX_HEIGHT_INCH.value / SlidesConstants.MAX_HEIGHT_TICKS.value
 
-    init {
-        elevatorRight.inverted = true
-        elevatorRight.encoder.setDirection(Motor.Direction.REVERSE)
-        elevatorMotors.resetEncoder()
+    val slideVelocity: Double
+        get() = -elevatorMotors.velocities[0] * SlidesConstants.MAX_HEIGHT_INCH.value / SlidesConstants.MAX_HEIGHT_TICKS.value
+
+    fun initialize(hardwareMap: HardwareMap) {
+        val elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName)
+        val elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName)
+
+        elevatorLeft.inverted = true
+
+        elevatorMotors = MotorGroup(elevatorLeft, elevatorRight)
     }
 
-    // Functions for moving slides up and down, number being speed, 1 being 100%
     fun up() {
         elevatorMotors.set(1.0)
     }
@@ -33,7 +36,7 @@ class OpenElevatorSubsystem(
     }
 
     fun stop() {
-        elevatorMotors.set(SlidesConstants.kG.value * sin(slideAngle.invoke()))
+        elevatorMotors.set(SlidesConstants.kG.value * sin(OpenArmSubsystem.armAngle))
     }
 
 }
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/OpenSlidesSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/OpenSlidesSubsystem.kt
deleted file mode 100644
index 893cd24..0000000
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/OpenSlidesSubsystem.kt
+++ /dev/null
@@ -1,42 +0,0 @@
-package org.firstinspires.ftc.teamcode.subsystems.slides
-
-import com.arcrobotics.ftclib.command.SubsystemBase
-import com.arcrobotics.ftclib.hardware.motors.Motor
-import com.arcrobotics.ftclib.hardware.motors.MotorGroup
-import com.qualcomm.robotcore.hardware.HardwareMap
-import org.firstinspires.ftc.teamcode.constants.ControlBoard
-import org.firstinspires.ftc.teamcode.constants.SlidesConstants
-import org.firstinspires.ftc.teamcode.subsystems.arm.OpenArmSubsystem
-import kotlin.math.sin
-
-object OpenSlidesSubsystem: SubsystemBase() {
-    private lateinit var elevatorMotors: MotorGroup
-
-    val slidePos: Double
-        get() = elevatorMotors.positions[0] * SlidesConstants.MAX_HEIGHT_INCH.value / SlidesConstants.MAX_HEIGHT_TICKS.value
-
-    val slideVelocity: Double
-        get() = -elevatorMotors.velocities[0] * SlidesConstants.MAX_HEIGHT_INCH.value / SlidesConstants.MAX_HEIGHT_TICKS.value
-
-    fun initialize(hardwareMap: HardwareMap) {
-        val elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName)
-        val elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName)
-
-        elevatorLeft.inverted = true
-
-        elevatorMotors = MotorGroup(elevatorLeft, elevatorRight)
-    }
-
-    fun up() {
-        elevatorMotors.set(1.0)
-    }
-
-    fun down() {
-        elevatorMotors.set(-1.0)
-    }
-
-    fun stop() {
-        elevatorMotors.set(SlidesConstants.kG.value * sin(OpenArmSubsystem.armAngle))
-    }
-
-}
\ No newline at end of file