diff --git a/.idea/misc.xml b/.idea/misc.xml
index 3c82750..2205f44 100644
--- a/.idea/misc.xml
+++ b/.idea/misc.xml
@@ -1,9 +1,10 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <project version="4">
   <component name="EntryPointsManager">
-    <list size="2">
-      <item index="0" class="java.lang.String" itemvalue="com.qualcomm.robotcore.eventloop.opmode.TeleOp" />
+    <list size="3">
+      <item index="0" class="java.lang.String" itemvalue="com.qualcomm.robotcore.eventloop.opmode.Autonomous" />
       <item index="1" class="java.lang.String" itemvalue="com.qualcomm.robotcore.eventloop.opmode.TeleOp" />
+      <item index="2" class="java.lang.String" itemvalue="com.qualcomm.robotcore.eventloop.opmode.TeleOp" />
     </list>
   </component>
   <component name="ExternalStorageConfigurationManager" enabled="true" />
diff --git a/PathPlanning/src/main/kotlin/com/example/pathplanning/legacy/blue/basket/BlueHighBasket1.kt b/PathPlanning/src/main/kotlin/com/example/pathplanning/legacy/blue/basket/BlueHighBasket1.kt
index cede8a9..7fdcf8b 100644
--- a/PathPlanning/src/main/kotlin/com/example/pathplanning/legacy/blue/basket/BlueHighBasket1.kt
+++ b/PathPlanning/src/main/kotlin/com/example/pathplanning/legacy/blue/basket/BlueHighBasket1.kt
@@ -1,32 +1,35 @@
 package com.example.pathplanning.legacy.blue.basket
 
-//object BlueHighBasket1 {
-//    @JvmStatic
-//    fun main(args: Array<String>) {
-//        val meepMeep = MeepMeep(800)
-//
-//        val myBot = DefaultBotBuilder(meepMeep) // Set bot constraints: maxVel, maxAccel, maxAngVel, maxAngAccel, track width
-//            .setConstraints(30.0, 30.0, Math.toRadians(170.0), Math.toRadians(170.0), 13.94)
-//            .followTrajectorySequence { drive ->
-//                drive.trajectorySequenceBuilder(Pose2d(-12.0, 62.0, (270.0).toRadians()))
-//                    .turn((90.0).toRadians())
-//                    .forward(60.0)
-//                    .waitSeconds(2.0)
-//                    .build()
-//            }
-//        var img: Image? = null
-//        try {
-//            img = ImageIO.read(File("C://Users//arava//Downloads//field.png/"))
-//        } catch (_ : IOException)  {
-//
-//        }
-//
-//        if (img != null) {
-//            meepMeep.setBackground(img)
-//                .setDarkMode(true)
-//                .setBackgroundAlpha(0.95f)
-//                .addEntity(myBot)
-//                .start()
-//        }
-//    }
-//}
\ No newline at end of file
+import com.acmerobotics.roadrunner.Pose2d
+import com.noahbres.meepmeep.MeepMeep
+import com.noahbres.meepmeep.core.toRadians
+import com.noahbres.meepmeep.roadrunner.DefaultBotBuilder
+
+object BlueHighBasket1 {
+    @JvmStatic
+    fun main(args: Array<String>) {
+        val meepMeep = MeepMeep(800)
+
+        val myBot = DefaultBotBuilder(meepMeep) // Set bot constraints: maxVel, maxAccel, maxAngVel, maxAngAccel, track width
+            .setStartPose(Pose2d(12.0, 62.0, (270.0).toRadians()))
+            .setConstraints(30.0, 30.0, Math.toRadians(170.0), Math.toRadians(170.0), 13.94)
+            .build()
+
+        myBot.runAction(
+            myBot.drive.actionBuilder(
+                Pose2d(12.0, 62.0, (270.0).toRadians())
+            )
+                .turn((90.0).toRadians())
+                .lineToX(35.0)
+                .waitSeconds(2.0)
+                .build()
+        )
+
+
+            meepMeep.setBackground(MeepMeep.Background.FIELD_INTO_THE_DEEP_JUICE_DARK)
+                .setDarkMode(true)
+                .setBackgroundAlpha(0.95f)
+                .addEntity(myBot)
+                .start()
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/ActionCommand.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/ActionCommand.kt
new file mode 100644
index 0000000..70c7f77
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/ActionCommand.kt
@@ -0,0 +1,33 @@
+package org.firstinspires.ftc.teamcode.commands
+
+import com.acmerobotics.dashboard.FtcDashboard
+import com.acmerobotics.dashboard.telemetry.TelemetryPacket
+import com.acmerobotics.roadrunner.Action
+import com.arcrobotics.ftclib.command.CommandBase
+import com.arcrobotics.ftclib.command.Subsystem
+
+/**
+ * Generic command that wraps Roadrunner 1.0 Actions
+ */
+class ActionCommand(
+    private val action: Action,
+    private vararg val subsystem: Subsystem,
+    private val endBehavior: () -> Unit = { },
+) : CommandBase() {
+    var finished = false
+
+    override fun initialize() = addRequirements(*subsystem)
+
+    override fun execute() {
+        val packet = TelemetryPacket()
+
+        action.preview(packet.fieldOverlay())
+        finished = !action.run(packet)
+
+        FtcDashboard.getInstance().sendTelemetryPacket(packet)
+    }
+
+    override fun isFinished() = finished
+
+    override fun end(interrupted: Boolean) = endBehavior.invoke()
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/arm/ArmCommand.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/arm/ArmCommand.kt
index 4748835..0663240 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/arm/ArmCommand.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/arm/ArmCommand.kt
@@ -3,12 +3,17 @@ package org.firstinspires.ftc.teamcode.commands.arm
 import com.arcrobotics.ftclib.command.CommandBase
 import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem
 
+/**
+ * Arm Command to set PID position for the arm
+ * @see ArmSubsystem
+ */
 class ArmCommand(
     private val setpoint: Double,
     private val subsystem: ArmSubsystem
 ) : CommandBase() {
     override fun initialize() {
         subsystem.setpoint = setpoint
+        addRequirements(subsystem)
     }
 
     override fun execute() = subsystem.operateArm()
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/arm/OpenArmCommand.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/arm/OpenArmCommand.kt
index c5ff6c2..98cf0ee 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/arm/OpenArmCommand.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/arm/OpenArmCommand.kt
@@ -7,6 +7,9 @@ class OpenArmCommand(
     private val subsystem: ArmSubsystem,
     private val turn: Boolean,
 ) : CommandBase() {
+    override fun initialize() {
+            addRequirements(subsystem)
+    }
 
     override fun execute() {
         if (turn) {
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/drive/DriveCommand.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/drive/DriveCommand.kt
index e3f53ef..3b5915c 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/drive/DriveCommand.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/drive/DriveCommand.kt
@@ -17,7 +17,7 @@ class DriveCommand(
 
     override fun execute() {
         subsystem.drive(
-            leftY = zonedDrive(-leftY.invoke() * 0.9, zoneVal).pow(3),
+            leftY = zonedDrive(leftY.invoke() * 0.9, zoneVal).pow(3),
             leftX = zonedDrive(leftX.invoke() * 0.9, zoneVal).pow(3),
             rightX = zonedDrive(rightX.invoke() * 0.9, zoneVal).pow(3),
         )
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/ElevatorCommand.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/ElevatorCommand.kt
index 70645ba..99660bb 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/ElevatorCommand.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/ElevatorCommand.kt
@@ -4,11 +4,12 @@ import com.arcrobotics.ftclib.command.CommandBase
 import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem
 
 class ElevatorCommand(
+    private val setpoint: Double,
     private val subsystem: ElevatorSubsystem,
-    private val setpoint: Double
 ) : CommandBase() {
     override fun initialize() {
         subsystem.setpoint = setpoint
+        addRequirements(ElevatorSubsystem)
     }
 
     override fun execute() = subsystem.operateElevator()
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/PoseStorage.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/PoseStorage.kt
deleted file mode 100644
index 7bd3e0c..0000000
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/PoseStorage.kt
+++ /dev/null
@@ -1,7 +0,0 @@
-package org.firstinspires.ftc.teamcode.constants
-
-import com.acmerobotics.roadrunner.Pose2d
-
-object PoseStorage {
-    var poseEstimate = Pose2d(0.0, 0.0, 0.0)
-}
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/GenericAuto.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/GenericAuto.kt
new file mode 100644
index 0000000..4c275f5
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/GenericAuto.kt
@@ -0,0 +1,71 @@
+package org.firstinspires.ftc.teamcode.opModes.auto
+
+import com.acmerobotics.roadrunner.Pose2d
+import com.arcrobotics.ftclib.command.CommandOpMode
+import com.arcrobotics.ftclib.command.SequentialCommandGroup
+import com.arcrobotics.ftclib.command.WaitCommand
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous
+import org.firstinspires.ftc.teamcode.commands.ActionCommand
+import org.firstinspires.ftc.teamcode.commands.arm.ArmCommand
+import org.firstinspires.ftc.teamcode.commands.elevator.ElevatorCommand
+import org.firstinspires.ftc.teamcode.commands.intake.IntakeCommand
+import org.firstinspires.ftc.teamcode.constants.AutoStartPose
+import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem
+import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
+import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem
+import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem
+
+@Autonomous(name = "Generic Auto")
+class GenericAuto : CommandOpMode() {
+    override fun initialize() {
+        DriveSubsystem.initialize(hardwareMap)
+        ArmSubsystem.initialize(hardwareMap)
+        ElevatorSubsystem.initialize(hardwareMap)
+        IntakeSubsystem.initialize(hardwareMap)
+
+        val scorePreloaded =
+            SequentialCommandGroup(
+                ActionCommand(
+                    DriveSubsystem.actionBuilder(AutoStartPose.BLUE_LEFT::startPose)
+                        .splineToLinearHeading(
+                            Pose2d(54.0, 56.0, Math.toRadians(225.0)),
+                            Math.toRadians(0.0)
+                        )
+                        .build(),
+                    DriveSubsystem
+                ),
+                ArmCommand(90.0, ArmSubsystem),
+                ElevatorCommand(30.0, ElevatorSubsystem),
+                IntakeCommand(true, IntakeSubsystem),
+                WaitCommand(500),
+                IntakeCommand(false, IntakeSubsystem),
+                ElevatorCommand(0.0, ElevatorSubsystem),
+                ArmCommand(0.0, ArmSubsystem),
+            )
+
+        val scoreFirst =
+            SequentialCommandGroup(
+                ActionCommand(
+                    DriveSubsystem.actionBuilder(DriveSubsystem::pose)
+                        .turn(Math.toRadians(37.0))
+                        .build(),
+                    DriveSubsystem
+                ),
+                ElevatorCommand(20.0, ElevatorSubsystem),
+                IntakeCommand(true, IntakeSubsystem),
+                WaitCommand(500),
+                IntakeCommand(false, IntakeSubsystem),
+                ElevatorCommand(0.0, ElevatorSubsystem),
+                ArmCommand(90.0, ArmSubsystem),
+                ElevatorCommand(30.0, ElevatorSubsystem)
+            )
+
+        schedule(
+            scorePreloaded
+            .andThen(
+                scoreFirst
+            )
+        )
+    }
+
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/ThreeDeadWheelLocalizer.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/ThreeDeadWheelLocalizer.kt
index 2cc13d1..5455667 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/ThreeDeadWheelLocalizer.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/ThreeDeadWheelLocalizer.kt
@@ -16,28 +16,28 @@ import org.firstinspires.ftc.teamcode.roadrunner.messages.ThreeDeadWheelInputsMe
 
 @Config
 class ThreeDeadWheelLocalizer(hardwareMap: HardwareMap, val inPerTick: Double) : Localizer {
-    class Params {
-        var par0YTicks: Double = 0.0 // y position of the first parallel encoder (in tick units)
-        var par1YTicks: Double = 1.0 // y position of the second parallel encoder (in tick units)
-        var perpXTicks: Double = 0.0 // x position of the perpendicular encoder (in tick units)
-    }
+    data class Params(
+        @JvmField var leftYTicks: Double = -2547.028032399116, // y position of the first parallel encoder (in tick units)
+        @JvmField var rightYTicks: Double = 2556.586233603863, // y position of the second parallel encoder (in tick units)
+        @JvmField var strafeXTicks: Double = -490.4056033307692 // 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 par0 =
+    val left =
         OverflowEncoder(RawEncoder(hardwareMap.get(DcMotorEx::class.java, ControlBoard.ODO_LEFT_ENCODER.deviceName)))
-    val par1 =
+    val right =
         OverflowEncoder(RawEncoder(hardwareMap.get(DcMotorEx::class.java, ControlBoard.ODO_RIGHT_ENCODER.deviceName)))
-    val perp =
+    val strafe =
         OverflowEncoder(RawEncoder(hardwareMap.get(DcMotorEx::class.java, ControlBoard.ODO_STRAFE_ENCODER.deviceName)))
 
     // TODO: reverse encoder directions if needed
     //   par0.setDirection(DcMotorSimple.Direction.REVERSE);
 
-    private var lastPar0Pos = 0
-    private var lastPar1Pos = 0
-    private var lastPerpPos = 0
+    private var lastLeftPos = 0
+    private var lastRightPos = 0
+    private var lastStrafePos = 0
     private var initialized = false
 
     init {
@@ -45,21 +45,21 @@ class ThreeDeadWheelLocalizer(hardwareMap: HardwareMap, val inPerTick: Double) :
     }
 
     override fun update(): Twist2dDual<Time> {
-        val par0PosVel = par0.getPositionAndVelocity()
-        val par1PosVel = par1.getPositionAndVelocity()
-        val perpPosVel = perp.getPositionAndVelocity()
+        val leftPosVel = left.getPositionAndVelocity()
+        val rightPosVel = right.getPositionAndVelocity()
+        val strafePosVel = strafe.getPositionAndVelocity()
 
         write(
             "THREE_DEAD_WHEEL_INPUTS",
-            ThreeDeadWheelInputsMessage(par0PosVel, par1PosVel, perpPosVel)
+            ThreeDeadWheelInputsMessage(leftPosVel, rightPosVel, strafePosVel)
         )
 
         if (!initialized) {
             initialized = true
 
-            lastPar0Pos = par0PosVel.position
-            lastPar1Pos = par1PosVel.position
-            lastPerpPos = perpPosVel.position
+            lastLeftPos = leftPosVel.position
+            lastRightPos = rightPosVel.position
+            lastStrafePos = strafePosVel.position
 
             return Twist2dDual(
                 Vector2dDual.constant(Vector2d(0.0, 0.0), 2),
@@ -67,41 +67,41 @@ class ThreeDeadWheelLocalizer(hardwareMap: HardwareMap, val inPerTick: Double) :
             )
         }
 
-        val par0PosDelta = par0PosVel.position - lastPar0Pos
-        val par1PosDelta = par1PosVel.position - lastPar1Pos
-        val perpPosDelta = perpPosVel.position - lastPerpPos
+        val leftPosDelta = leftPosVel.position - lastLeftPos
+        val rightPosDelta = rightPosVel.position - lastRightPos
+        val strafePosDelta = strafePosVel.position - lastStrafePos
 
         val twist = Twist2dDual(
             Vector2dDual(
                 DualNum<Time>(
                     doubleArrayOf(
-                        (PARAMS.par0YTicks * par1PosDelta - PARAMS.par1YTicks * par0PosDelta) / (PARAMS.par0YTicks - PARAMS.par1YTicks),
-                        (PARAMS.par0YTicks * par1PosVel.velocity - PARAMS.par1YTicks * par0PosVel.velocity) / (PARAMS.par0YTicks - PARAMS.par1YTicks),
+                        (PARAMS.leftYTicks * rightPosDelta - PARAMS.rightYTicks * leftPosDelta) / (PARAMS.leftYTicks - PARAMS.rightYTicks),
+                        (PARAMS.leftYTicks * rightPosVel.velocity - PARAMS.rightYTicks * leftPosVel.velocity) / (PARAMS.leftYTicks - PARAMS.rightYTicks),
                     )
                 ) * 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),
+                        (PARAMS.strafeXTicks / (PARAMS.leftYTicks - PARAMS.rightYTicks) * (rightPosDelta - leftPosDelta) + strafePosDelta),
+                        (PARAMS.strafeXTicks / (PARAMS.leftYTicks - PARAMS.rightYTicks) * (rightPosVel.velocity - leftPosVel.velocity) + strafePosVel.velocity),
                     )
                 ) * inPerTick
             ),
             DualNum(
                 doubleArrayOf(
-                    (par0PosDelta - par1PosDelta) / (PARAMS.par0YTicks - PARAMS.par1YTicks),
-                    (par0PosVel.velocity - par1PosVel.velocity) / (PARAMS.par0YTicks - PARAMS.par1YTicks),
+                    (leftPosDelta - rightPosDelta) / (PARAMS.leftYTicks - PARAMS.rightYTicks),
+                    (leftPosVel.velocity - rightPosVel.velocity) / (PARAMS.leftYTicks - PARAMS.rightYTicks),
                 )
             )
         )
 
-        lastPar0Pos = par0PosVel.position
-        lastPar1Pos = par1PosVel.position
-        lastPerpPos = perpPosVel.position
+        lastLeftPos = leftPosVel.position
+        lastRightPos = rightPosVel.position
+        lastStrafePos = strafePosVel.position
 
         return twist
     }
 
     companion object {
-        var PARAMS: Params = Params()
+        @JvmField var PARAMS = Params()
     }
 }
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/DriveCommandMessage.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/DriveCommandMessage.kt
index fabde81..73596b0 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/DriveCommandMessage.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/DriveCommandMessage.kt
@@ -4,11 +4,11 @@ import com.acmerobotics.roadrunner.PoseVelocity2dDual
 import com.acmerobotics.roadrunner.Time
 
 class DriveCommandMessage(poseVelocity: PoseVelocity2dDual<Time>) {
-    var timestamp: Long = System.nanoTime()
-    var forwardVelocity: Double = poseVelocity.linearVel.x[0]
-    var forwardAcceleration: Double = poseVelocity.linearVel.x[1]
-    var lateralVelocity: Double = poseVelocity.linearVel.y[0]
-    var lateralAcceleration: Double = poseVelocity.linearVel.y[1]
-    var angularVelocity: Double = poseVelocity.angVel[0]
-    var angularAcceleration: Double = poseVelocity.angVel[1]
+    @JvmField var timestamp: Long = System.nanoTime()
+    @JvmField var forwardVelocity: Double = poseVelocity.linearVel.x[0]
+    @JvmField var forwardAcceleration: Double = poseVelocity.linearVel.x[1]
+    @JvmField var lateralVelocity: Double = poseVelocity.linearVel.y[0]
+    @JvmField var lateralAcceleration: Double = poseVelocity.linearVel.y[1]
+    @JvmField var angularVelocity: Double = poseVelocity.angVel[0]
+    @JvmField var angularAcceleration: Double = poseVelocity.angVel[1]
 }
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/MecanumCommandMessage.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/MecanumCommandMessage.kt
index edd0e42..1c1edda 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/MecanumCommandMessage.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/MecanumCommandMessage.kt
@@ -1,11 +1,11 @@
 package org.firstinspires.ftc.teamcode.roadrunner.messages
 
 class MecanumCommandMessage(
-    var voltage: Double,
-    var leftFrontPower: Double,
-    var leftBackPower: Double,
-    var rightBackPower: Double,
-    var rightFrontPower: Double
+    @JvmField var voltage: Double,
+    @JvmField var leftFrontPower: Double,
+    @JvmField var leftBackPower: Double,
+    @JvmField var rightBackPower: Double,
+    @JvmField var rightFrontPower: Double
 ) {
-    var timestamp: Long = System.nanoTime()
+    @JvmField var timestamp: Long = System.nanoTime()
 }
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/MecanumLocalizerInputsMessage.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/MecanumLocalizerInputsMessage.kt
index f93e312..0ae858d 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/MecanumLocalizerInputsMessage.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/MecanumLocalizerInputsMessage.kt
@@ -5,22 +5,14 @@ import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit
 import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles
 
 class MecanumLocalizerInputsMessage(
-    var leftFront: PositionVelocityPair,
-    var leftBack: PositionVelocityPair,
-    var rightBack: PositionVelocityPair,
-    var rightFront: PositionVelocityPair,
+    @JvmField var leftFront: PositionVelocityPair,
+    @JvmField var leftBack: PositionVelocityPair,
+    @JvmField var rightBack: PositionVelocityPair,
+    @JvmField var rightFront: PositionVelocityPair,
     angles: YawPitchRollAngles
 ) {
-    var timestamp: Long = System.nanoTime()
-    var yaw: Double = 0.0
-    var pitch: Double = 0.0
-    var roll: Double = 0.0
-
-    init {
-        run {
-            this.yaw = angles.getYaw(AngleUnit.RADIANS)
-            this.pitch = angles.getPitch(AngleUnit.RADIANS)
-            this.roll = angles.getRoll(AngleUnit.RADIANS)
-        }
-    }
+    @JvmField var timestamp: Long = System.nanoTime()
+    @JvmField var yaw: Double = angles.getYaw(AngleUnit.RADIANS)
+    @JvmField var pitch: Double = angles.getPitch(AngleUnit.RADIANS)
+    @JvmField var roll: Double = angles.getRoll(AngleUnit.RADIANS)
 }
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/PoseMessage.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/PoseMessage.kt
index 81eb5c0..224149c 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/PoseMessage.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/PoseMessage.kt
@@ -3,9 +3,9 @@ package org.firstinspires.ftc.teamcode.roadrunner.messages
 import com.acmerobotics.roadrunner.Pose2d
 
 class PoseMessage(pose: Pose2d) {
-    var timestamp: Long = System.nanoTime()
-    var x: Double = pose.position.x
-    var y: Double = pose.position.y
-    var heading: Double = pose.heading.toDouble()
+    @JvmField var timestamp: Long = System.nanoTime()
+    @JvmField var x: Double = pose.position.x
+    @JvmField var y: Double = pose.position.y
+    @JvmField var heading: Double = pose.heading.toDouble()
 }
 
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/TankCommandMessage.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/TankCommandMessage.kt
index de1571e..be8a6e4 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/TankCommandMessage.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/TankCommandMessage.kt
@@ -1,5 +1,9 @@
 package org.firstinspires.ftc.teamcode.roadrunner.messages
 
-class TankCommandMessage(var voltage: Double, var leftPower: Double, var rightPower: Double) {
-    var timestamp: Long = System.nanoTime()
+class TankCommandMessage(
+    @JvmField var voltage: Double,
+    @JvmField var leftPower: Double,
+    @JvmField var rightPower: Double
+) {
+    @JvmField var timestamp: Long = System.nanoTime()
 }
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/TankLocalizerInputsMessage.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/TankLocalizerInputsMessage.kt
index 5ed861a..761711f 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/TankLocalizerInputsMessage.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/TankLocalizerInputsMessage.kt
@@ -6,7 +6,7 @@ class TankLocalizerInputsMessage(
     left: List<PositionVelocityPair>,
     right: List<PositionVelocityPair>
 ) {
-    var timestamp: Long = System.nanoTime()
-    var left: Array<PositionVelocityPair> = left.toTypedArray<PositionVelocityPair>()
-    var right: Array<PositionVelocityPair> = right.toTypedArray<PositionVelocityPair>()
+    @JvmField var timestamp: Long = System.nanoTime()
+    @JvmField var left = left.toTypedArray()
+    @JvmField var right = right.toTypedArray()
 }
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/ThreeDeadWheelInputsMessage.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/ThreeDeadWheelInputsMessage.kt
index 58262f0..6773343 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/ThreeDeadWheelInputsMessage.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/ThreeDeadWheelInputsMessage.kt
@@ -2,10 +2,10 @@ package org.firstinspires.ftc.teamcode.roadrunner.messages
 
 import com.acmerobotics.roadrunner.ftc.PositionVelocityPair
 
-class ThreeDeadWheelInputsMessage(
-    var par0: PositionVelocityPair,
-    var par1: PositionVelocityPair,
-    var perp: PositionVelocityPair
+data class ThreeDeadWheelInputsMessage(
+    @JvmField var par0: PositionVelocityPair,
+    @JvmField var par1: PositionVelocityPair,
+    @JvmField var perp: PositionVelocityPair
 ) {
-    var timestamp: Long = System.nanoTime()
+    @JvmField var timestamp: Long = System.nanoTime()
 }
\ No newline at end of file
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
index 18cbeb9..119a09c 100644
--- 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
@@ -16,7 +16,7 @@ class ManualFeedbackTuner : LinearOpMode() {
                 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) {
+                    if (ThreeDeadWheelLocalizer.PARAMS.strafeXTicks == 0.0 && ThreeDeadWheelLocalizer.PARAMS.leftYTicks == 0.0 && ThreeDeadWheelLocalizer.Companion.PARAMS.rightYTicks == 1.0) {
                         throw RuntimeException("Odometry wheel locations not set! Run AngularRampLogger to tune them.")
                     }
                 }
@@ -24,7 +24,7 @@ class ManualFeedbackTuner : LinearOpMode() {
 
                 while (opModeIsActive()) {
                     runBlocking(
-                        drive.actionBuilder(Pose2d(0.0, 0.0, 0.0))
+                        drive.actionBuilder { Pose2d(0.0, 0.0, 0.0) }
                             .lineToX(DISTANCE)
                             .lineToX(0.0)
                             .build()
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/tuning/SplineTest.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/tuning/SplineTest.kt
index f407efc..42af313 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/tuning/SplineTest.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/tuning/SplineTest.kt
@@ -19,7 +19,7 @@ class SplineTest : LinearOpMode() {
                 waitForStart()
 
                 runBlocking(
-                    drive.actionBuilder(beginPose)
+                    drive.actionBuilder { beginPose }
                         .splineTo(Vector2d(30.0, 30.0), Math.PI / 2)
                         .splineTo(Vector2d(0.0, 60.0), Math.PI)
                         .build()
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/tuning/TuningOpModes.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/tuning/TuningOpModes.kt
index 752b8ca..89c53fc 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/tuning/TuningOpModes.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/tuning/TuningOpModes.kt
@@ -21,7 +21,6 @@ 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.HardwareMap
-import org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.hardwareMap
 import org.firstinspires.ftc.robotcore.internal.opmode.OpModeMeta
 import org.firstinspires.ftc.teamcode.roadrunner.ThreeDeadWheelLocalizer
 import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
@@ -39,6 +38,7 @@ object TuningOpModes {
             .setName(cls.simpleName)
             .setGroup(GROUP)
             .setFlavor(OpModeMeta.Flavor.TELEOP)
+            .setSource(OpModeMeta.Source.EXTERNAL_LIBRARY)
             .build()
     }
 
@@ -52,7 +52,7 @@ object TuningOpModes {
             DriveSubsystem::class.java -> {
                 dvf = object : DriveViewFactory {
                     override fun make(h: HardwareMap): DriveView {
-                        DriveSubsystem.initialize(hardwareMap)
+                        DriveSubsystem.initialize(h)
 
                         val md = DriveSubsystem
 
@@ -65,16 +65,16 @@ object TuningOpModes {
                             is DriveSubsystem.DriveLocalizer -> {
                                 val dl = md.localizer as DriveSubsystem.DriveLocalizer
                                 leftEncs.add(dl.leftFront)
-                                leftEncs.add(dl.leftBack)
+                                leftEncs.add(dl.leftRear)
                                 rightEncs.add(dl.rightFront)
-                                rightEncs.add(dl.rightBack)
+                                rightEncs.add(dl.rightRear)
                             }
 
                             is ThreeDeadWheelLocalizer -> {
                                 val dl = md.localizer as ThreeDeadWheelLocalizer
-                                parEncs.add(dl.par0)
-                                parEncs.add(dl.par1)
-                                perpEncs.add(dl.perp)
+                                parEncs.add(dl.left)
+                                parEncs.add(dl.right)
+                                perpEncs.add(dl.strafe)
                             }
 
                             else -> {
@@ -88,14 +88,14 @@ object TuningOpModes {
                             DriveSubsystem.PARAMS.maxWheelVel,
                             DriveSubsystem.PARAMS.minProfileAccel,
                             DriveSubsystem.PARAMS.maxProfileAccel,
-                            hardwareMap.getAll(LynxModule::class.java),
+                            h.getAll(LynxModule::class.java),
                             listOf(
                                 md.leftFront,
-                                md.leftBack
+                                md.leftRear
                             ),
                             listOf(
                                 md.rightFront,
-                                md.rightBack
+                                md.rightRear
                             ),
                             leftEncs,
                             rightEncs,
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 8931d2d..18c316c 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
@@ -43,6 +43,7 @@ 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.DcMotorSimple
 import com.qualcomm.robotcore.hardware.HardwareMap
 import com.qualcomm.robotcore.hardware.IMU
 import com.qualcomm.robotcore.hardware.VoltageSensor
@@ -50,6 +51,7 @@ 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.ThreeDeadWheelLocalizer
 import org.firstinspires.ftc.teamcode.roadrunner.messages.DriveCommandMessage
 import org.firstinspires.ftc.teamcode.roadrunner.messages.MecanumCommandMessage
 import org.firstinspires.ftc.teamcode.roadrunner.messages.MecanumLocalizerInputsMessage
@@ -61,7 +63,7 @@ import kotlin.math.max
 @Config
 object DriveSubsystem : SubsystemBase() {
     private lateinit var hardwareMap: HardwareMap
-    var pose: Pose2d = Pose2d(0.0, 0.0, 0.0)
+    var pose = Pose2d(0.0, 0.0, 0.0)
 
     @JvmField
     var PARAMS = Params()
@@ -70,65 +72,37 @@ object DriveSubsystem : SubsystemBase() {
         // 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.DOWN
-        var usbFacingDirection: UsbFacingDirection = UsbFacingDirection.RIGHT
+        var logoFacingDirection = RevHubOrientationOnRobot.LogoFacingDirection.DOWN
+        var usbFacingDirection = UsbFacingDirection.LEFT
 
         // drive model parameters
-        @JvmField
-        var inPerTick: Double = 1.0
-
-        @JvmField
-        var lateralInPerTick: Double = inPerTick
-
-        @JvmField
-        var trackWidthTicks: Double = 0.0
+        @JvmField var inPerTick = 0.002953423066
+        @JvmField var lateralInPerTick = inPerTick
+        @JvmField var trackWidthTicks = 4669.698132939092
 
         // feedforward parameters (in tick units)
-        @JvmField
-        var kS: Double = 0.0
-
-        @JvmField
-        var kV: Double = 0.0
-
-        @JvmField
-        var kA: Double = 0.0
+        @JvmField var kS = 1.308709766262468
+        @JvmField var kV = 3.701322333374629E-4
+        @JvmField var kA = 0.0001
 
         // path profile parameters (in inches)
-        @JvmField
-        var maxWheelVel: Double = 50.0
-
-        @JvmField
-        var minProfileAccel: Double = -30.0
-
-        @JvmField
-        var maxProfileAccel: Double = 50.0
+        @JvmField var maxWheelVel = 50.0
+        @JvmField var minProfileAccel = -30.0
+        @JvmField var maxProfileAccel = 50.0
 
         // turn profile parameters (in radians)
-        @JvmField
-        var maxAngVel: Double = Math.PI // shared with path
-
-        @JvmField
-        var maxAngAccel: Double = Math.PI
+        @JvmField var maxAngVel = Math.PI // shared with path
+        @JvmField var maxAngAccel = Math.PI
 
         // path controller gains
-        @JvmField
-        var axialGain: Double = 0.0
-
-        @JvmField
-        var lateralGain: Double = 0.0
-
-        @JvmField
-        var headingGain: Double = 0.0 // shared with turn
+        @JvmField var axialGain = 0.0
+        @JvmField var lateralGain = 0.0
+        @JvmField var headingGain = 0.0 // shared with turn
 
-        @JvmField
-        var axialVelGain: Double = 0.0
+        @JvmField var axialVelGain = 0.0
 
-        @JvmField
-        var lateralVelGain: Double = 0.0
-
-        @JvmField
-        var headingVelGain: Double = 0.0 // shared with turn
+        @JvmField var lateralVelGain = 0.0
+        @JvmField var headingVelGain = 0.0 // shared with turn
     }
 
     val kinematics: MecanumKinematics = MecanumKinematics(
@@ -148,8 +122,8 @@ object DriveSubsystem : SubsystemBase() {
         ProfileAccelConstraint(PARAMS.minProfileAccel, PARAMS.maxProfileAccel)
 
     lateinit var leftFront: DcMotorEx
-    lateinit var leftBack: DcMotorEx
-    lateinit var rightBack: DcMotorEx
+    lateinit var leftRear: DcMotorEx
+    lateinit var rightRear: DcMotorEx
     lateinit var rightFront: DcMotorEx
 
     lateinit var voltageSensor: VoltageSensor
@@ -168,10 +142,10 @@ object DriveSubsystem : SubsystemBase() {
     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 leftRear: Encoder =
+            OverflowEncoder(RawEncoder(DriveSubsystem.leftRear))
+        val rightRear: Encoder =
+            OverflowEncoder(RawEncoder(DriveSubsystem.rightRear))
         val rightFront: Encoder =
             OverflowEncoder(RawEncoder(DriveSubsystem.rightFront))
         val imu: IMU = lazyImu.get()
@@ -190,8 +164,8 @@ object DriveSubsystem : SubsystemBase() {
 
         override fun update(): Twist2dDual<Time> {
             val leftFrontPosVel = leftFront.getPositionAndVelocity()
-            val leftBackPosVel = leftBack.getPositionAndVelocity()
-            val rightBackPosVel = rightBack.getPositionAndVelocity()
+            val leftBackPosVel = leftRear.getPositionAndVelocity()
+            val rightBackPosVel = rightRear.getPositionAndVelocity()
             val rightFrontPosVel = rightFront.getPositionAndVelocity()
 
             val angles = imu.robotYawPitchRollAngles
@@ -264,11 +238,11 @@ object DriveSubsystem : SubsystemBase() {
         }
     }
 
-    fun initialize(hardwareMap: HardwareMap,
-                   pose2d: Pose2d = Pose2d(0.0, 0.0, 0.0)
-    ) : DriveSubsystem {
+    fun initialize(hardwareMap: HardwareMap, reset: Boolean = true) {
+        if (reset)
+            pose = Pose2d(0.0, 0.0, 0.0)
+
         this.hardwareMap = hardwareMap
-        this.pose = pose2d
 
         throwIfModulesAreOutdated(hardwareMap)
 
@@ -279,17 +253,17 @@ object DriveSubsystem : SubsystemBase() {
         // 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)
+        leftRear = hardwareMap.get(DcMotorEx::class.java, ControlBoard.DRIVE_LEFT_REAR.deviceName)
+        rightRear = 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
+        leftRear.zeroPowerBehavior = DcMotor.ZeroPowerBehavior.BRAKE
+        rightRear.zeroPowerBehavior = DcMotor.ZeroPowerBehavior.BRAKE
         rightFront.zeroPowerBehavior = DcMotor.ZeroPowerBehavior.BRAKE
 
         // TODO: reverse motor directions if needed
-        //   leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
+        leftRear.direction = 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
@@ -301,11 +275,9 @@ object DriveSubsystem : SubsystemBase() {
 
         voltageSensor = hardwareMap.voltageSensor.iterator().next()
 
-        localizer = DriveLocalizer()
+        localizer = ThreeDeadWheelLocalizer(hardwareMap, PARAMS.inPerTick)
 
         write("MECANUM_PARAMS", PARAMS)
-
-        return this
     }
 
     fun setDrivePowers(powers: PoseVelocity2d) {
@@ -319,8 +291,8 @@ object DriveSubsystem : SubsystemBase() {
         }
 
         leftFront.power = wheelVels.leftFront[0] / maxPowerMag
-        leftBack.power = wheelVels.leftBack[0] / maxPowerMag
-        rightBack.power = wheelVels.rightBack[0] / maxPowerMag
+        leftRear.power = wheelVels.leftBack[0] / maxPowerMag
+        rightRear.power = wheelVels.rightBack[0] / maxPowerMag
         rightFront.power = wheelVels.rightFront[0] / maxPowerMag
     }
 
@@ -358,8 +330,8 @@ object DriveSubsystem : SubsystemBase() {
 
             if (t >= timeTrajectory.duration) {
                 leftFront.power = 0.0
-                leftBack.power = 0.0
-                rightBack.power = 0.0
+                leftRear.power = 0.0
+                rightRear.power = 0.0
                 rightFront.power = 0.0
 
                 return false
@@ -395,8 +367,8 @@ object DriveSubsystem : SubsystemBase() {
             )
 
             leftFront.power = leftFrontPower
-            leftBack.power = leftBackPower
-            rightBack.power = rightBackPower
+            leftRear.power = leftBackPower
+            rightRear.power = rightBackPower
             rightFront.power = rightFrontPower
 
             p.put("x", pose.position.x)
@@ -446,8 +418,8 @@ object DriveSubsystem : SubsystemBase() {
 
             if (t >= turn.duration) {
                 leftFront.power = 0.0
-                leftBack.power = 0.0
-                rightBack.power = 0.0
+                leftRear.power = 0.0
+                rightRear.power = 0.0
                 rightFront.power = 0.0
 
                 return false
@@ -482,8 +454,8 @@ object DriveSubsystem : SubsystemBase() {
             )
 
             leftFront.power = feedforward.compute(wheelVels.leftFront) / voltage
-            leftBack.power = feedforward.compute(wheelVels.leftBack) / voltage
-            rightBack.power = feedforward.compute(wheelVels.rightBack) / voltage
+            leftRear.power = feedforward.compute(wheelVels.leftBack) / voltage
+            rightRear.power = feedforward.compute(wheelVels.rightBack) / voltage
             rightFront.power = feedforward.compute(wheelVels.rightFront) / voltage
 
             val c = p.fieldOverlay()
@@ -509,7 +481,7 @@ object DriveSubsystem : SubsystemBase() {
 
     fun updatePoseEstimate(): PoseVelocity2d {
         val twist = localizer.update()
-        pose = pose.plus(twist.value())
+        pose += twist.value()
 
         poseHistory.add(pose)
         while (poseHistory.size > 100) {
@@ -538,17 +510,17 @@ object DriveSubsystem : SubsystemBase() {
         c.strokePolyline(xPoints, yPoints)
     }
 
-    fun actionBuilder(beginPose: Pose2d): TrajectoryActionBuilder {
+    fun actionBuilder(beginPose: () -> Pose2d): TrajectoryActionBuilder {
         return TrajectoryActionBuilder(
-            { turn: TimeTurn -> TurnAction(turn) },
-            { t: TimeTrajectory -> FollowTrajectoryAction(t) },
+            { TurnAction(it) },
+            { FollowTrajectoryAction(it) },
             TrajectoryBuilderParams(
                 1e-6,
                 ProfileParams(
                     0.25, 0.1, 1e-2
                 )
             ),
-            beginPose, 0.0,
+            beginPose.invoke(), 0.0,
             defaultTurnConstraints,
             defaultVelConstraint, defaultAccelConstraint
         )
@@ -557,8 +529,8 @@ object DriveSubsystem : SubsystemBase() {
     fun drive(leftY: Double, leftX: Double, rightX: Double) {
         setDrivePowers(
             PoseVelocity2d(
-                linearVel = Vector2d(leftX, leftY),
-                angVel = rightX
+                linearVel = Vector2d(leftY, -leftX),
+                angVel = -rightX
             )
         )
     }