From 8cdd3dd447fa2bd3eabe347a73ca6a3d2bbc02ef Mon Sep 17 00:00:00 2001
From: missing <piecenotfound@gmail.com>
Date: Mon, 23 Jan 2023 23:52:26 -0600
Subject: [PATCH] literally everything ive done over the past >2 months in one
 commit lol

most important stuff: kinematics, autons, config, dashboard, easyopencv + april tags, and probably more im forgetting

https://xkcd.com/1296/
---
 .idea/jarRepositories.xml                     |   5 +
 TeamCode/build.gradle                         |   5 +-
 .../ftc/teamcode/AutonMovement.java           | 177 ++++++++++
 .../ftc/teamcode/AutonMovement2.java          | 119 +++++++
 .../ftc/teamcode/AutonMovement3.java          |  68 ++++
 .../teamcode/DashboardTelemetryWrapper.java   | 228 +++++++++++++
 .../firstinspires/ftc/teamcode/Gamepad.java   |  46 ---
 .../firstinspires/ftc/teamcode/Hardware.java  | 102 ++++--
 .../ftc/teamcode/MotorPowers.java             |  43 +++
 .../ftc/teamcode/MovementAPI.java             |  53 +--
 .../firstinspires/ftc/teamcode/Strings.java   |   9 +
 .../ftc/teamcode/auto/AprilTagAutonBase.java  | 210 ++++++++++++
 .../auto/AprilTagDetectionPipeline.java       | 312 ++++++++++++++++++
 .../ftc/teamcode/{ => auto}/AutonLeft.java    |  14 +-
 .../ftc/teamcode/auto/AutonMovement.java      | 165 +++++++++
 .../ftc/teamcode/{ => auto}/AutonRight.java   |  14 +-
 .../ftc/teamcode/auto/MultiConeAutonLeft.java |  89 +++++
 .../ftc/teamcode/auto/OneConeAutonLeft.java   |  59 ++++
 .../ftc/teamcode/auto/OneConeAutonRight.java  |  59 ++++
 .../ftc/teamcode/auto/ResetEncoders.java      |  21 ++
 .../firstinspires/ftc/teamcode/auto/Test.java |  36 ++
 .../ftc/teamcode/auto/ZeroConeAutonLeft.java  |  41 +++
 .../ftc/teamcode/auto/ZeroConeAutonRight.java |  40 +++
 .../ftc/teamcode/config/ConfigSleeve123.java  |  12 +
 .../teamcode/config/ConfigSleeveVariantA.java |  12 +
 .../teamcode/config/ConfigSleeveVariantB.java |  12 +
 .../teamcode/config/ConfigSleeveVariantC.java |  12 +
 .../ftc/teamcode/config/RuntimeConfig.java    |  24 ++
 .../ftc/teamcode/kinematics/Distance.java     |  89 +++++
 .../kinematics/EncoderIntegrator.java         | 117 +++++++
 .../ftc/teamcode/kinematics/FieldVector.java  |  79 +++++
 .../ftc/teamcode/kinematics/LiftPosition.java |  57 ++++
 .../ftc/teamcode/kinematics/NullablePose.java |  96 ++++++
 .../ftc/teamcode/kinematics/Pose.java         |  19 ++
 .../ftc/teamcode/kinematics/Poser.java        | 118 +++++++
 .../ftc/teamcode/teleop/Gamepad.java          | 115 +++++++
 .../ftc/teamcode/teleop/MonoGamepad.java      | 114 +++++++
 build.common.gradle                           |   4 +-
 build.dependencies.gradle                     |   2 +
 build.gradle                                  |   2 +-
 40 files changed, 2677 insertions(+), 122 deletions(-)
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonMovement.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonMovement2.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonMovement3.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DashboardTelemetryWrapper.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gamepad.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MotorPowers.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Strings.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AprilTagAutonBase.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AprilTagDetectionPipeline.java
 rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => auto}/AutonLeft.java (63%)
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutonMovement.java
 rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => auto}/AutonRight.java (62%)
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/MultiConeAutonLeft.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/OneConeAutonLeft.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/OneConeAutonRight.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/ResetEncoders.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/Test.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/ZeroConeAutonLeft.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/ZeroConeAutonRight.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/config/ConfigSleeve123.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/config/ConfigSleeveVariantA.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/config/ConfigSleeveVariantB.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/config/ConfigSleeveVariantC.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/config/RuntimeConfig.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/Distance.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/EncoderIntegrator.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/FieldVector.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/LiftPosition.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/NullablePose.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/Pose.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/Poser.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Gamepad.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/MonoGamepad.java

diff --git a/.idea/jarRepositories.xml b/.idea/jarRepositories.xml
index d2ce72d..c403d3c 100644
--- a/.idea/jarRepositories.xml
+++ b/.idea/jarRepositories.xml
@@ -21,5 +21,10 @@
       <option name="name" value="Google" />
       <option name="url" value="https://dl.google.com/dl/android/maven2/" />
     </remote-repository>
+    <remote-repository>
+      <option name="id" value="maven" />
+      <option name="name" value="maven" />
+      <option name="url" value="https://maven.brott.dev/" />
+    </remote-repository>
   </component>
 </project>
\ No newline at end of file
diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle
index 436ce67..42c1cca 100644
--- a/TeamCode/build.gradle
+++ b/TeamCode/build.gradle
@@ -26,4 +26,7 @@ android {
 dependencies {
     implementation project(':FtcRobotController')
     annotationProcessor files('lib/OpModeAnnotationProcessor.jar')
-}
+
+    implementation 'org.openftc:easyopencv:1.5.2'
+    implementation 'org.openftc:apriltag:1.1.0'
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonMovement.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonMovement.java
new file mode 100644
index 0000000..7cd7000
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonMovement.java
@@ -0,0 +1,177 @@
+//package org.firstinspires.ftc.teamcode;
+//
+//import androidx.annotation.NonNull;
+//import androidx.annotation.Nullable;
+//
+//import com.qualcomm.hardware.bosch.BNO055IMU;
+//
+//import org.firstinspires.ftc.robotcore.external.navigation.Acceleration;
+//import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+//import org.firstinspires.ftc.robotcore.external.navigation.NavUtil;
+//import org.firstinspires.ftc.robotcore.external.navigation.Position;
+//import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
+//
+//public class AutonMovement {
+//    final Hardware hardware;
+//    final MovementAPI movementAPI;
+//    final CustomAccelerationIntegrator integrator;
+////    ExecutorService integratorThread;
+////    final Object integratorLock = new Object();
+//
+//    public AutonMovement(Hardware hardware) {
+//        this.hardware = hardware;
+//        this.movementAPI = new MovementAPI(hardware);
+//        this.integrator = new CustomAccelerationIntegrator(FieldVector.ZERO);
+//        this.hardware.initIMU();
+//        this.hardware.initPosIMU(this.integrator);
+//
+////        this.integratorThread = ThreadPool.newSingleThreadExecutor("imu acceleration integration");
+////        this.integratorThread.execute(() -> {
+////            while (!Thread.currentThread().isInterrupted()) {
+////                synchronized (this.integratorLock) {
+////                    this.integrator.update();
+////                }
+////
+////                Thread.yield();
+////            }
+////        });
+//    }
+//
+//    public void move(FieldVector relativePos) {
+//        this.moveTo(this.hardware.getIMUPosition().add(relativePos));
+//    }
+//
+//    public void moveTo(FieldVector targetPos) {
+//        final double KP = 1;
+//        final double KI = 0;
+//        final double KD = 0;
+//
+//        FieldVector currentPos2 = this.hardware.getIMUPosition();
+//
+//        double scaleFactor = currentPos2.sub(targetPos).magnitude();
+//
+//        FieldVector integral = FieldVector.ZERO;
+//        FieldVector prevPos = currentPos2;
+//
+//        while (true) {
+//            FieldVector currentPos = this.hardware.getIMUPosition();
+//            FieldVector posDiff = targetPos.sub(currentPos);
+//
+//            if (posDiff.sqMagnitude() < 100 * 100) return;
+//
+//            integral = integral.add(posDiff);
+//
+//            FieldVector unscaled = posDiff.mul(KP)
+//                    .add(currentPos.sub(prevPos).mul(KD))
+//                    .add(integral.mul(KI));
+//
+//            FieldVector scaled = unscaled.div(scaleFactor * 2);
+//
+//            double yaw = this.hardware.getImuYaw();
+//
+//            FieldVector rotated = scaled.rot(-yaw);
+//
+//            this.movementAPI.move(rotated.x, rotated.y, 0, 0.7);
+//
+//            prevPos = currentPos;
+//        }
+//    }
+//
+//    public void rotTo(double targetAngle) {
+//        final double KP = 1;
+//        final double KI = 0;
+//        final double KD = 0;
+//
+//        double integral = 0;
+//        double prevYaw = this.hardware.getImuYaw();
+//
+//        while (true) {
+//            double currentYaw = this.hardware.getImuYaw();
+//            double yawDiff = targetAngle - currentYaw;
+//
+//            if (yawDiff < 2) return;
+//
+//            integral += yawDiff;
+//
+//            double turnSpeed = yawDiff * KP
+//                    + (currentYaw - prevYaw) * KD
+//                    + integral * KI;
+//
+//            // note: should be `-turnSpeed` but `MovementAPI.move` has the reverse mapping of +/- to cw/ccw
+//            this.movementAPI.move(0, 0, turnSpeed, 0.7);
+//
+//            prevYaw = currentYaw;
+//        }
+//    }
+//
+//    public static class CustomAccelerationIntegrator implements BNO055IMU.AccelerationIntegrator {
+//        BNO055IMU.Parameters parameters = null;
+//        Position position = new Position();
+//        Velocity velocity = new Velocity();
+//        Acceleration acceleration = null;
+//
+//        public Position getPosition() {
+//            return this.position;
+//        }
+//
+//        public Velocity getVelocity() {
+//            return this.velocity;
+//        }
+//
+//        public Acceleration getAcceleration() {
+//            return this.acceleration;
+//        }
+//
+//        public CustomAccelerationIntegrator(FieldVector initialPosition) {
+//            this.position = new Position(DistanceUnit.MM, initialPosition.x, initialPosition.y, 0, 0);
+//        }
+//
+//        public void initialize(@NonNull BNO055IMU.Parameters parameters, @Nullable Position initialPosition, @Nullable Velocity initialVelocity) {
+//            this.parameters = parameters;
+//            this.position = initialPosition != null ? initialPosition : this.position;
+//            this.velocity = initialVelocity != null ? initialVelocity : this.velocity;
+//            this.acceleration = null;
+//        }
+//
+//        public void update(Acceleration linearAcceleration) {
+//            if (linearAcceleration.acquisitionTime != 0L) {
+//                if (Math.sqrt(
+//                        linearAcceleration.xAccel * linearAcceleration.xAccel
+//                        + linearAcceleration.yAccel * linearAcceleration.yAccel
+//                        + linearAcceleration.zAccel * linearAcceleration.zAccel
+//                ) < 0.05) {
+//                    linearAcceleration.xAccel = 0;
+//                    linearAcceleration.yAccel = 0;
+//                    linearAcceleration.zAccel = 0;
+//                }
+//
+//                if (this.acceleration != null) {
+//                    Acceleration accelPrev = this.acceleration;
+//                    Velocity velocityPrev = this.velocity;
+//                    this.acceleration = linearAcceleration;
+//                    if (accelPrev.acquisitionTime != 0L) {
+//                        Velocity deltaVelocity = NavUtil.meanIntegrate(this.acceleration, accelPrev);
+//                        this.velocity = NavUtil.plus(this.velocity, deltaVelocity);
+//                    }
+//
+//                    if (Math.sqrt(
+//                            this.velocity.xVeloc * this.velocity.xVeloc
+//                            + this.velocity.yVeloc * this.velocity.yVeloc
+//                            + this.velocity.zVeloc * this.velocity.zVeloc
+//                    ) < 0.05) {
+//                        this.velocity.xVeloc = 0;
+//                        this.velocity.yVeloc = 0;
+//                        this.velocity.zVeloc = 0;
+//                    }
+//
+//                    if (velocityPrev.acquisitionTime != 0L) {
+//                        Position deltaPosition = NavUtil.meanIntegrate(this.velocity, velocityPrev);
+//                        this.position = NavUtil.plus(this.position, deltaPosition);
+//                    }
+//                } else {
+//                    this.acceleration = linearAcceleration;
+//                }
+//            }
+//        }
+//    }
+//}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonMovement2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonMovement2.java
new file mode 100644
index 0000000..200ff5e
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonMovement2.java
@@ -0,0 +1,119 @@
+//package org.firstinspires.ftc.teamcode;
+//
+//import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+//
+//public class AutonMovement2 {
+//    final Hardware hardware;
+//    final MovementAPI movementAPI;
+//    final OpMode opMode;
+//    FieldVector currentPosition;
+//
+//    public AutonMovement2(OpMode opMode, FieldVector initialPosition) {
+//        this.opMode = opMode;
+//        this.hardware = new Hardware(opMode);
+//        this.movementAPI = new MovementAPI(hardware);
+//        this.hardware.initIMU();
+//        this.currentPosition = initialPosition;
+//    }
+//
+//    public void move(FieldVector relativePos) {
+//        this.moveTo(currentPosition.add(relativePos));
+//    }
+//
+//    public void moveTo(FieldVector targetPos) {
+//        final double KP = 1;
+//        final double KI = 0.3;
+//        final double KD = 0.5;
+//
+//        final double encoderTicksPerRev = 537.7;
+//        final double mmPerRev = 96 /* diameter of gobilda mecanums */ * Math.PI;
+//        final double encoderTicksPerMm = encoderTicksPerRev / mmPerRev;
+//
+//        FieldVector posDiffNormalized = targetPos.sub(currentPosition).normalized();
+//        MotorPowers powers = MotorPowers.calculateUnscaled(posDiffNormalized.x, posDiffNormalized.y, 0, 1);
+//
+//        final double port0EncoderScale = powers.fl;
+//        final double port3EncoderScale = powers.fr;
+//
+//        double port0EncoderStart = hardware.fl.getCurrentPosition();
+//        double port3EncoderStart = hardware.fr.getCurrentPosition();
+//
+//        FieldVector integral = FieldVector.ZERO;
+//        FieldVector prevPos = this.currentPosition;
+//
+//        while (true) {
+//            double port0Encoder = hardware.fl.getCurrentPosition();
+//            double port3Encoder = hardware.fr.getCurrentPosition();
+//
+//            double port0EncoderScaled = (port0Encoder - port0EncoderStart) / port0EncoderScale;
+//            double port3EncoderScaled = (port3Encoder - port3EncoderStart) / port3EncoderScale;
+//
+//            double encoderTicksMoved = (port0EncoderScaled + port3EncoderScaled) / 2;
+//            double mmMoved = encoderTicksMoved / encoderTicksPerMm;
+//
+//            opMode.telemetry.addLine("the port 0 encoder is " + port0Encoder);
+//            opMode.telemetry.addLine("and it started as " + port0EncoderStart);
+//            opMode.telemetry.addLine("oh and the scale value is " + port0EncoderScale);
+//            opMode.telemetry.addLine("the port 3 encoder is " + port3Encoder);
+//            opMode.telemetry.addLine("and it started as " + port3EncoderStart);
+//            opMode.telemetry.addLine("oh and the scale value is " + port3EncoderScale);
+//            opMode.telemetry.addLine("i think we have moved " + encoderTicksMoved + " encoder ticks");
+//            opMode.telemetry.addLine("i think we have moved " + mmMoved + "mm");
+//
+//            FieldVector currentPos = posDiffNormalized.mul(mmMoved).add(this.currentPosition /* actually more like initialPosition */);
+//            FieldVector posDiff = targetPos.sub(currentPos);
+//
+//            opMode.telemetry.addLine("i think we are at (" + currentPos.x + ", " + currentPos.y + ")");
+//            opMode.telemetry.update();
+//
+//            if (posDiff.sqMagnitude() < 5 * 5) {
+//                movementAPI.stop();
+//                this.currentPosition = currentPos; // finally update `this.currentPosition`
+//                return;
+//            }
+//
+//            integral = integral.add(posDiff);
+//
+//            FieldVector unscaled = posDiff.mul(KP)
+//                    .add(currentPos.sub(prevPos).mul(KD))
+//                    .add(integral.mul(KI));
+//
+//            FieldVector scaled = unscaled.div(Distance.MM_PER_FOOT);
+//
+//            double yaw = this.hardware.getImuYaw();
+//
+//            FieldVector rotated = scaled.rot(yaw);
+//
+//            this.movementAPI.move(rotated.x, rotated.y, 0, 0.4);
+//
+//            prevPos = currentPos;
+//        }
+//    }
+//
+//    public void rotTo(double targetAngle) {
+//        final double KP = 1;
+//        final double KI = 0;
+//        final double KD = 0;
+//
+//        double integral = 0;
+//        double prevYaw = this.hardware.getImuYaw();
+//
+//        while (true) {
+//            double currentYaw = this.hardware.getImuYaw();
+//            double yawDiff = targetAngle - currentYaw;
+//
+//            if (yawDiff < 2) return;
+//
+//            integral += yawDiff;
+//
+//            double turnSpeed = yawDiff * KP
+//                    + (currentYaw - prevYaw) * KD
+//                    + integral * KI;
+//
+//            // note: should be `-turnSpeed` but `MovementAPI.move` has the reverse mapping of +/- to cw/ccw
+//            this.movementAPI.move(0, 0, turnSpeed / 15, 0.4);
+//
+//            prevYaw = currentYaw;
+//        }
+//    }
+//}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonMovement3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonMovement3.java
new file mode 100644
index 0000000..7cb9894
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonMovement3.java
@@ -0,0 +1,68 @@
+//package org.firstinspires.ftc.teamcode;
+//
+//public class AutonMovement3 {
+//    Hardware hardware;
+//    MovementAPI movement;
+//
+//    double speed;
+//
+//    public AutonMovement3(Hardware hardware, double speed) {
+//        this.hardware = hardware;
+//        this.movement = new MovementAPI(hardware);
+//        this.speed = speed;
+//    }
+//
+//    private void sleep(double seconds) {
+//        double startTime = System.nanoTime();
+//        double targetTime = startTime + seconds * 1000 * 1000 * 1000;
+//
+//        while (System.nanoTime() < targetTime);
+//    }
+//
+//    public void straight(double power, double tiles) {
+//        this.moveFor(0, power, tiles * (23/18.));
+//    }
+//
+//    public void strafe(double power, double tiles) {
+//        this.moveFor(power, 0, tiles * (23/17.));
+//    }
+//
+//    public void moveFor(double powerX, double powerY, double seconds) {
+//        movement.move(powerX, powerY, 0, this.speed);
+//
+//        this.sleep(seconds);
+//        movement.stop();
+//        this.sleep(1);
+//    }
+//
+//    private double modAngle(double angle) {
+//        double angle2 = angle % 360;
+//        if (angle2 >= 180) {
+//            return angle2 - 360;
+//        } else {
+//            return angle2;
+//        }
+//    }
+//
+//    public void turn(double degrees) {
+//        double startYaw = hardware.getImuYaw();
+//        double targetYaw = startYaw + (-degrees);
+//
+//        while (true) {
+//            double yaw = hardware.getImuYaw();
+//            double yawDiff = this.modAngle(targetYaw - yaw);
+//
+//            if (Math.abs(yawDiff) < 1) {
+//                break;
+//            }
+//
+//            if (Math.abs(yawDiff) > 20) {
+//                movement.move(0, 0, Math.signum(yawDiff), this.speed);
+//            } else {
+//                movement.move(0, 0, Math.signum(yawDiff) * (yawDiff / 40 + 0.5), this.speed);
+//            }
+//        }
+//        movement.stop();
+//        this.sleep(0.3);
+//    }
+//}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DashboardTelemetryWrapper.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DashboardTelemetryWrapper.java
new file mode 100644
index 0000000..1bb7226
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DashboardTelemetryWrapper.java
@@ -0,0 +1,228 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.canvas.Canvas;
+import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
+
+import org.firstinspires.ftc.robotcore.external.Func;
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.kinematics.Distance;
+import org.firstinspires.ftc.teamcode.kinematics.FieldVector;
+import org.firstinspires.ftc.teamcode.kinematics.Pose;
+
+public class DashboardTelemetryWrapper implements Telemetry {
+    private final FtcDashboard dashboard;
+    private TelemetryPacket packet;
+    private final TelemetryLog log;
+
+    public DashboardTelemetryWrapper(FtcDashboard dashboard) {
+        this.dashboard = dashboard;
+        this.packet = new TelemetryPacket();
+        this.log = this.new TelemetryLog();
+    }
+
+    private void rawDrawRobot(FieldVector pos, double yaw) {
+        FieldVector topLeft = new FieldVector(Distance.ROBOT_WIDTH.neg(), Distance.ROBOT_LENGTH).div(2).rot(yaw).add(pos);
+        FieldVector topRight = new FieldVector(Distance.ROBOT_WIDTH, Distance.ROBOT_LENGTH).div(2).rot(yaw).add(pos);
+        FieldVector bottomLeft = new FieldVector(Distance.ROBOT_WIDTH.neg(), Distance.ROBOT_LENGTH.neg()).div(2).rot(yaw).add(pos);
+        FieldVector bottomRight = new FieldVector(Distance.ROBOT_WIDTH, Distance.ROBOT_LENGTH.neg()).div(2).rot(yaw).add(pos);
+
+        Canvas canvas = this.packet.fieldOverlay();
+        canvas.fillPolygon(
+                new double[]{topLeft.x.valInInches(), topRight.x.valInInches(), bottomRight.x.valInInches(), bottomLeft.x.valInInches()},
+                new double[]{topLeft.y.valInInches(), topRight.y.valInInches(), bottomRight.y.valInInches(), bottomLeft.y.valInInches()}
+        );
+    }
+
+    public void drawRobot(FieldVector pos, double yaw) {
+        Canvas canvas = this.packet.fieldOverlay();
+        canvas.setStrokeWidth(1);
+        canvas.setFill("gray");
+        canvas.setStroke("black");
+
+        this.rawDrawRobot(pos, yaw);
+    }
+
+    public void drawTargetOnDashboard(Pose target, Pose current) {
+        Canvas canvas = this.packet.fieldOverlay();
+        canvas.setStrokeWidth(1);
+        canvas.setFill("transparent");
+        canvas.setStroke("green");
+
+        canvas.fillCircle(target.pos.x.valInInches(), target.pos.y.valInInches(), 1);
+        canvas.strokeLine(
+                current.pos.x.valInInches(),
+                current.pos.y.valInInches(),
+                target.pos.x.valInInches(),
+                target.pos.y.valInInches()
+        );
+        this.rawDrawRobot(target.pos, target.yaw);
+    }
+
+    @Override
+    public Item addData(String caption, String format, Object... args) {
+        return this.addData(caption, String.format(format, args));
+    }
+
+    @Override
+    public Item addData(String caption, Object value) {
+        this.packet.put(caption, value);
+        return null;
+    }
+
+    @Override
+    public <T> Item addData(String caption, Func<T> valueProducer) {
+        return this.addData(caption, valueProducer.value());
+    }
+
+    @Override
+    public <T> Item addData(String caption, String format, Func<T> valueProducer) {
+        return this.addData(caption, format, valueProducer.value());
+    }
+
+    @Override
+    public boolean removeItem(Item item) {
+        throw new UnsupportedOperationException();
+    }
+
+    @Override
+    public void clear() {
+        this.dashboard.clearTelemetry();
+        this.packet = new TelemetryPacket();
+    }
+
+    @Override
+    public void clearAll() {
+        this.clear();
+    }
+
+    @Override
+    public Object addAction(Runnable action) {
+        throw new UnsupportedOperationException();
+    }
+
+    @Override
+    public boolean removeAction(Object token) {
+        throw new UnsupportedOperationException();
+    }
+
+    @Override
+    public void speak(String text) {
+        throw new UnsupportedOperationException();
+    }
+
+    @Override
+    public void speak(String text, String languageCode, String countryCode) {
+        throw new UnsupportedOperationException();
+    }
+
+    @Override
+    public boolean update() {
+        this.dashboard.sendTelemetryPacket(packet);
+        this.packet = new TelemetryPacket();
+        return true;
+    }
+
+    @Override
+    public Line addLine() {
+        return null;
+    }
+
+    @Override
+    public Line addLine(String lineCaption) {
+        this.packet.addLine(lineCaption);
+        return null;
+    }
+
+    @Override
+    public boolean removeLine(Line line) {
+        throw new UnsupportedOperationException();
+    }
+
+    @Override
+    public boolean isAutoClear() {
+        return false;
+    }
+
+    @Override
+    public void setAutoClear(boolean autoClear) {
+        throw new UnsupportedOperationException();
+    }
+
+    @Override
+    public int getMsTransmissionInterval() {
+        return this.dashboard.getTelemetryTransmissionInterval();
+    }
+
+    @Override
+    public void setMsTransmissionInterval(int msTransmissionInterval) {
+        this.dashboard.setTelemetryTransmissionInterval(msTransmissionInterval);
+    }
+
+    @Override
+    public String getItemSeparator() {
+        return null;
+    }
+
+    @Override
+    public void setItemSeparator(String itemSeparator) {
+
+    }
+
+    @Override
+    public String getCaptionValueSeparator() {
+        return null;
+    }
+
+    @Override
+    public void setCaptionValueSeparator(String captionValueSeparator) {
+
+    }
+
+    @Override
+    public void setDisplayFormat(DisplayFormat displayFormat) {
+
+    }
+
+    @Override
+    public Log log() {
+        return log;
+    }
+
+    private class TelemetryLog implements Log {
+        @Override
+        public int getCapacity() {
+            return 0;
+        }
+
+        @Override
+        public void setCapacity(int capacity) {
+
+        }
+
+        @Override
+        public DisplayOrder getDisplayOrder() {
+            return DisplayOrder.OLDEST_FIRST;
+        }
+
+        @Override
+        public void setDisplayOrder(DisplayOrder displayOrder) {
+
+        }
+
+        @Override
+        public void add(String entry) {
+            packet.addLine(entry);
+        }
+
+        @Override
+        public void add(String format, Object... args) {
+            this.add(String.format(format, args));
+        }
+
+        @Override
+        public void clear() {
+            packet.clearLines();
+        }
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gamepad.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gamepad.java
deleted file mode 100644
index 8fd4caa..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gamepad.java
+++ /dev/null
@@ -1,46 +0,0 @@
-package org.firstinspires.ftc.teamcode;
-
-import com.qualcomm.robotcore.eventloop.opmode.OpMode;
-import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-
-@TeleOp(name = "BBBBBBBBB")
-public class Gamepad extends OpMode {
-    Hardware hardware;
-    MovementAPI movement;
-    double speed = 0.6;
-
-    @Override
-    public void init() {
-        hardware = new Hardware(this);
-        movement = new MovementAPI(hardware);
-
-        hardware.claw.setPosition(1);
-        hardware.wrist.setPosition(0.5);
-    }
-
-    @Override
-    public void loop() {
-        if (gamepad1.left_bumper) {
-            speed -= 0.005;
-        }
-
-        if (gamepad1.right_bumper) {
-            speed += 0.005;
-        }
-
-        speed = Math.max(0, Math.min(speed, 1));
-
-        hardware.spool.setPower(gamepad2.left_stick_y);
-        hardware.claw.setPosition(1 - gamepad2.left_trigger);
-        hardware.wrist.setPosition((-gamepad2.right_stick_y + 1) / 2);
-
-        movement.move(
-                gamepad1.left_stick_x,
-                -gamepad1.left_stick_y,
-                gamepad1.right_stick_x,
-                speed
-        );
-
-        telemetry.addLine(speed + "");
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hardware.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hardware.java
index 3b78cc7..b2adf1c 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hardware.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hardware.java
@@ -1,34 +1,44 @@
 package org.firstinspires.ftc.teamcode;
 
-import com.qualcomm.hardware.bosch.BNO055IMU;
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.hardware.lynx.LynxModule;
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
 import com.qualcomm.robotcore.eventloop.opmode.OpMode;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorSimple;
 import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.IMU;
 import com.qualcomm.robotcore.hardware.Servo;
 
-import org.firstinspires.ftc.robotcore.external.navigation.Position;
-import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+
+import java.util.List;
 
 public class Hardware {
+    public final OpMode opMode;
+    public final DashboardTelemetryWrapper dashboardTelemetryWrapper;
     public final DcMotor fl;
     public final DcMotor fr;
     public final DcMotor bl;
     public final DcMotor br;
     public final DcMotor spool;
-    public final Servo claw;
+    public final Servo clawLeft;
+    public final Servo clawRight;
     public final Servo wrist;
-    public final BNO055IMU imu;
+    public final IMU imu;
+    public final List<LynxModule> hubs;
 
     public Hardware(OpMode opMode) {
-        this(opMode.hardwareMap);
-    }
+        HardwareMap hm = opMode.hardwareMap;
+        this.opMode = opMode;
+        this.dashboardTelemetryWrapper = new DashboardTelemetryWrapper(FtcDashboard.getInstance());
+        opMode.telemetry = new MultipleTelemetry(opMode.telemetry, this.dashboardTelemetryWrapper);
 
-    public Hardware(HardwareMap hardwareMap) {
-        this.fl = hardwareMap.get(DcMotor.class, "fl");
-        this.fr = hardwareMap.get(DcMotor.class, "fr");
-        this.bl = hardwareMap.get(DcMotor.class, "bl");
-        this.br = hardwareMap.get(DcMotor.class, "br");
+        this.fl = hm.get(DcMotor.class, "fl");
+        this.fr = hm.get(DcMotor.class, "fr");
+        this.bl = hm.get(DcMotor.class, "bl");
+        this.br = hm.get(DcMotor.class, "br");
 
         this.fl.setDirection(DcMotorSimple.Direction.REVERSE);
         this.bl.setDirection(DcMotorSimple.Direction.REVERSE);
@@ -38,22 +48,72 @@ public class Hardware {
         this.fr.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
         this.bl.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
         this.br.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        this.fl.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        this.fr.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        this.bl.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        this.br.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
 
-        this.spool = hardwareMap.get(DcMotor.class, "spool");
-        this.claw = hardwareMap.get(Servo.class, "claw");
-        this.wrist = hardwareMap.get(Servo.class, "wrist");
-        this.imu = hardwareMap.get(BNO055IMU.class, "imu");
+        this.spool = hm.get(DcMotor.class, "spool");
+        this.clawLeft = hm.get(Servo.class, "clawLeft");
+        this.clawRight = hm.get(Servo.class, "clawRight");
+        this.wrist = hm.get(Servo.class, "wrist");
+        this.imu = hm.get(IMU.class, "imu");
+//        this.posImu = hm.get(BNO055IMU.class, "imu");
 
+        this.spool.setDirection(DcMotorSimple.Direction.FORWARD);
+        this.spool.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+
+        this.clawLeft.scaleRange(0, 1/3.);
+        this.clawRight.scaleRange(2/3., 1);
         this.wrist.scaleRange(0.25, 1);
+
+        this.hubs = hm.getAll(LynxModule.class);
+        for (LynxModule hub : this.hubs) {
+            hub.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
+        }
+    }
+
+    public void setClawPosition(double pos) {
+        this.clawLeft.setPosition(1 - pos);
+        this.clawRight.setPosition(pos);
+    }
+
+    public void setWristPosition(double pos) {
+        this.wrist.setPosition((pos + 1) / 2);
     }
 
     public void initIMU() {
-        BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
-        parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
-        parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
-        parameters.calibrationDataFile = "BNO055IMUCalibration.json";
+        IMU.Parameters parameters = new IMU.Parameters(
+                new RevHubOrientationOnRobot(
+                        RevHubOrientationOnRobot.LogoFacingDirection.FORWARD,
+                        RevHubOrientationOnRobot.UsbFacingDirection.LEFT
+                )
+        );
 
         this.imu.initialize(parameters);
-        this.imu.startAccelerationIntegration(new Position(), new Velocity(), 1);
     }
+
+//    public void initPosIMU(BNO055IMU.AccelerationIntegrator integrator) {
+//        BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
+//        parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
+//        parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
+//        parameters.calibrationDataFile = "AdafruitIMUCalibration.json";
+//        parameters.accelerationIntegrationAlgorithm = integrator;
+//
+//        this.posImu.initialize(parameters);
+//        this.posImu.stopAccelerationIntegration();
+//    }
+
+    public double getImuYaw() {
+        return -this.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
+    }
+
+    public int getLiftEncoder() {
+        return -spool.getCurrentPosition();
+    }
+
+//    public FieldVector getIMUPosition() {
+//        Position tmp = this.posImu.getPosition().toUnit(DistanceUnit.MM);
+//        return FieldVector.inMM(tmp.x, tmp.y);
+//    }
 }
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MotorPowers.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MotorPowers.java
new file mode 100644
index 0000000..5c8b83d
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MotorPowers.java
@@ -0,0 +1,43 @@
+package org.firstinspires.ftc.teamcode;
+
+public class MotorPowers {
+    public final double fl;
+    public final double fr;
+    public final double bl;
+    public final double br;
+
+    public MotorPowers(double fl, double fr, double bl, double br) {
+        this.fl = fl;
+        this.fr = fr;
+        this.bl = bl;
+        this.br = br;
+    }
+
+    public static MotorPowers calculateUnscaled(double powerX, double powerY, double turn, double speed) {
+        double fl = (powerY + powerX + turn) * speed;
+        double fr = (powerY - powerX - turn) * speed;
+        double bl = (powerY - powerX + turn) * speed;
+        double br = (powerY + powerX - turn) * speed;
+
+        return new MotorPowers(fl, fr, bl, br);
+    }
+
+    public MotorPowers scaled() {
+        double scale = 1;
+        scale = Math.max(scale, Math.abs(this.fl));
+        scale = Math.max(scale, Math.abs(this.fr));
+        scale = Math.max(scale, Math.abs(this.bl));
+        scale = Math.max(scale, Math.abs(this.br));
+
+        double fl = this.fl / scale;
+        double fr = this.fr / scale;
+        double bl = this.bl / scale;
+        double br = this.br / scale;
+
+        return new MotorPowers(fl, fr, bl, br);
+    }
+
+    public static MotorPowers calculate(double powerX, double powerY, double turn, double speed) {
+        return MotorPowers.calculateUnscaled(powerX, powerY, turn, speed).scaled();
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MovementAPI.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MovementAPI.java
index aa942fe..c0b4e8f 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MovementAPI.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MovementAPI.java
@@ -1,58 +1,29 @@
 package org.firstinspires.ftc.teamcode;
 
 import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.DcMotorSimple.Direction;
-import com.qualcomm.robotcore.hardware.HardwareMap;
 
 public class MovementAPI {
-    private final DcMotor fl;
-    private final DcMotor fr;
-    private final DcMotor bl;
-    private final DcMotor br;
-
-    public DcMotor getFL() { return this.fl; }
-    public DcMotor getFR() { return this.fr; }
-    public DcMotor getBL() { return this.bl; }
-    public DcMotor getBR() { return this.br; }
+    private final Hardware hardware;
 
     public MovementAPI(Hardware hardware) {
-        this(
-                hardware.fl,
-                hardware.fr,
-                hardware.bl,
-                hardware.br
-        );
+        this.hardware = hardware;
     }
 
-    public MovementAPI(DcMotor fl, DcMotor fr, DcMotor bl, DcMotor br) {
-        this.fl = fl;
-        this.fr = fr;
-        this.bl = bl;
-        this.br = br;
+    public void move(MotorPowers powers) {
+        hardware.fl.setPower(powers.fl);
+        hardware.fr.setPower(powers.fr);
+        hardware.bl.setPower(powers.bl);
+        hardware.br.setPower(powers.br);
     }
 
     public void move(double powerX, double powerY, double turn, double speed) {
-        double flPower = (powerY + powerX + turn) * speed;
-        double frPower = (powerY - powerX - turn) * speed;
-        double blPower = (powerY - powerX + turn) * speed;
-        double brPower = (powerY + powerX - turn) * speed;
-
-        double scale = Math.max(1, (Math.abs(powerY) + Math.abs(turn) + Math.abs(powerX)) * Math.abs(speed)); // shortcut for max(abs([fl,fr,bl,br]))
-        flPower /= scale;
-        frPower /= scale;
-        blPower /= scale;
-        brPower /= scale;
-
-        fl.setPower(flPower);
-        fr.setPower(frPower);
-        bl.setPower(blPower);
-        br.setPower(brPower);
+        this.move(MotorPowers.calculate(powerX, powerY, turn, speed));
     }
 
     public void stop() {
-        fl.setPower(0);
-        fr.setPower(0);
-        bl.setPower(0);
-        br.setPower(0);
+        hardware.fl.setPower(0);
+        hardware.fr.setPower(0);
+        hardware.bl.setPower(0);
+        hardware.br.setPower(0);
     }
 }
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Strings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Strings.java
new file mode 100644
index 0000000..350df21
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Strings.java
@@ -0,0 +1,9 @@
+package org.firstinspires.ftc.teamcode;
+
+public class Strings {
+    public static final String dualGamepadOpModeName = "BBBBBBBBB";
+    public static final String monoGamepadOpModeName = "bbbbbbbbb";
+    public static final String mainOpModeName = Strings.dualGamepadOpModeName;
+    public static final String leftSideGroup = "Storming the castle (left side)";
+    public static final String rightSideGroup = "Face the forest (right side)";
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AprilTagAutonBase.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AprilTagAutonBase.java
new file mode 100644
index 0000000..c9fe7bf
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AprilTagAutonBase.java
@@ -0,0 +1,210 @@
+package org.firstinspires.ftc.teamcode.auto;
+
+import android.annotation.SuppressLint;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
+import org.firstinspires.ftc.teamcode.config.RuntimeConfig;
+import org.openftc.apriltag.AprilTagDetection;
+import org.openftc.easyopencv.OpenCvCamera;
+import org.openftc.easyopencv.OpenCvCameraFactory;
+import org.openftc.easyopencv.OpenCvCameraRotation;
+
+import java.util.ArrayList;
+
+public abstract class AprilTagAutonBase extends LinearOpMode {
+    OpenCvCamera camera;
+    AprilTagDetectionPipeline aprilTagDetectionPipeline;
+
+    // Lens intrinsics
+    // UNITS ARE PIXELS
+    // NOTE: this calibration is for the C920 webcam at 800x448.
+    // You will need to do your own calibration for other configurations!
+    double fx = 578.272;
+    double fy = 578.272;
+    double cx = 402.145;
+    double cy = 221.506;
+
+    // UNITS ARE METERS
+    double tagSize = 0.0381; // 1.5 in
+
+    // All from the 36h11 family
+    int[][] ID_TAGS_OF_INTEREST = {
+            {
+                    584,
+                    449,
+                    275,
+            },
+            {
+                    125,
+                    345,
+                    392,
+            },
+            {
+                    87,
+                    93,
+                    554
+            },
+            {
+                    1,
+                    2,
+                    3
+            }
+    };
+
+    int VARIANT_COUNT = ID_TAGS_OF_INTEREST.length;
+
+    AprilTagDetection[][] tagsOfInterest = new AprilTagDetection[VARIANT_COUNT][3];
+
+    abstract void onInit();
+    abstract void onPlay(int signalZone);
+
+    @Override
+    public void runOpMode() {
+        if (RuntimeConfig.SIGNAL_SLEEVE_VARIANT == null) {
+            double endTime = getRuntime() + 5;
+            while (!isStarted() && getRuntime() < endTime) {
+                telemetry.addLine("Signal sleeve variant not set!");
+                telemetry.addLine("Press play or wait 5 seconds to end the opmode.");
+                telemetry.addLine("Here is a flashing block to get your attention:");
+                telemetry.addLine("####################################");
+                telemetry.addLine("####################################");
+                telemetry.addLine("####################################");
+                telemetry.update();
+
+                telemetry.addLine("Signal sleeve variant not set!");
+                telemetry.addLine("Press play or wait 5 seconds to end the opmode.");
+                telemetry.addLine("Here is a flashing block to get your attention:");
+                telemetry.addLine("");
+                telemetry.addLine("");
+                telemetry.addLine("");
+                telemetry.update();
+            }
+
+            return;
+        }
+
+        onInit();
+
+        int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
+        camera = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, "Webcam 1"), cameraMonitorViewId);
+        aprilTagDetectionPipeline = new AprilTagDetectionPipeline(tagSize, fx, fy, cx, cy);
+
+        camera.setPipeline(aprilTagDetectionPipeline);
+        camera.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() {
+            @Override
+            public void onOpened() {
+                camera.startStreaming(800,448, OpenCvCameraRotation.SIDEWAYS_RIGHT);
+                FtcDashboard dashboard = FtcDashboard.getInstance();
+                dashboard.startCameraStream(camera, 0);
+            }
+
+            @Override
+            public void onError(int errorCode) {}
+        });
+
+        telemetry.setMsTransmissionInterval(50);
+
+        Integer[] mostRecentlySeenPerVariant = new Integer[VARIANT_COUNT];
+
+        /*
+         * The INIT-loop:
+         * This REPLACES waitForStart!
+         */
+        while (!isStarted() && !isStopRequested()) {
+            telemetry.addLine("Chosen variant: " + RuntimeConfig.signalSleeveVariantName());
+
+            ArrayList<AprilTagDetection> currentDetections = aprilTagDetectionPipeline.getLatestDetections();
+
+            ArrayList<Integer>[] tagsFound = new ArrayList[VARIANT_COUNT];
+            // fill the array, cause i dont know how to do this better
+            for (int i = 0; i < tagsFound.length; i++) tagsFound[i] = new ArrayList<>();
+
+            for (AprilTagDetection tag : currentDetections) {
+                for (int variant_index = 0; variant_index < VARIANT_COUNT; variant_index++) {
+                    for (int id_index = 0; id_index < 3; id_index++) {
+                        if (tag.id == ID_TAGS_OF_INTEREST[variant_index][id_index]) {
+                            tagsFound[variant_index].add(id_index);
+
+                            tagsOfInterest[variant_index][id_index] = tag;
+                        }
+                    }
+                }
+            }
+
+            for (int variant_index = 0; variant_index < VARIANT_COUNT; variant_index++) {
+                if (tagsFound[variant_index].size() == 1) {
+                    mostRecentlySeenPerVariant[variant_index] = tagsFound[variant_index].get(0);
+                }
+            }
+
+            if (tagsFound[RuntimeConfig.SIGNAL_SLEEVE_VARIANT].size() == 1) {
+                int id = mostRecentlySeenPerVariant[RuntimeConfig.SIGNAL_SLEEVE_VARIANT];
+
+                telemetry.addLine("Tag " + RuntimeConfig.signalSleeveVariantName() + "-" + (id + 1) + " is in sight!\n\nLocation data:");
+                tagToTelemetry(tagsOfInterest[RuntimeConfig.SIGNAL_SLEEVE_VARIANT][id]);
+            } else {
+                telemetry.addLine("Don't currently see any tags from variant " + RuntimeConfig.signalSleeveVariantName() + " :(");
+
+                if (mostRecentlySeenPerVariant[RuntimeConfig.SIGNAL_SLEEVE_VARIANT] == null) {
+                    telemetry.addLine("(No tags have ever been seen)");
+                } else {
+                    int id = mostRecentlySeenPerVariant[RuntimeConfig.SIGNAL_SLEEVE_VARIANT];
+
+                    telemetry.addLine("\nBut we HAVE seen tags before; tag " + RuntimeConfig.signalSleeveVariantName() + "-" + (id + 1) + " last seen at:");
+                    tagToTelemetry(tagsOfInterest[RuntimeConfig.SIGNAL_SLEEVE_VARIANT][id]);
+                }
+            }
+
+            telemetry.update();
+            sleep(20);
+        }
+
+        /*
+         * The START command just came in: now work off the latest snapshot acquired
+         * during the init loop.
+         */
+
+        int signalZone;
+
+        if (mostRecentlySeenPerVariant[RuntimeConfig.SIGNAL_SLEEVE_VARIANT] == null) {
+            telemetry.addLine("Uh oh, we never saw variant " + RuntimeConfig.signalSleeveVariantName() + "! Checking other variants...");
+
+            Integer id = null;
+            Integer variant = null;
+
+            for (int variant_index = 0; variant_index < VARIANT_COUNT; variant_index++) {
+                if (mostRecentlySeenPerVariant[variant_index] != null) {
+                    id = mostRecentlySeenPerVariant[variant_index];
+                    variant = variant_index;
+                }
+            }
+
+            if (id != null) {
+                telemetry.addLine("We saw tag " + RuntimeConfig.signalSleeveVariantName(variant) + "-" + (id + 1) + " at some point, using that.");
+                signalZone = id;
+            } else {
+                telemetry.addLine("Nope, nothing, nada, defaulting to signal zone 1.");
+                signalZone = 1;
+            }
+            telemetry.update();
+        } else {
+            signalZone = mostRecentlySeenPerVariant[RuntimeConfig.SIGNAL_SLEEVE_VARIANT];
+        }
+
+        this.onPlay(signalZone + 1);
+    }
+
+    @SuppressLint("DefaultLocale")
+    void tagToTelemetry(AprilTagDetection detection) {
+        telemetry.addLine(String.format("\nDetected tag ID=%d", detection.id));
+        telemetry.addLine(String.format("Translation X: %.2f metres", detection.pose.x));
+        telemetry.addLine(String.format("Translation Y: %.2f metres", detection.pose.y));
+        telemetry.addLine(String.format("Translation Z: %.2f metres", detection.pose.z));
+        telemetry.addLine(String.format("Rotation Yaw: %.2f degrees", Math.toDegrees(detection.pose.yaw)));
+        telemetry.addLine(String.format("Rotation Pitch: %.2f degrees", Math.toDegrees(detection.pose.pitch)));
+        telemetry.addLine(String.format("Rotation Roll: %.2f degrees", Math.toDegrees(detection.pose.roll)));
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AprilTagDetectionPipeline.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AprilTagDetectionPipeline.java
new file mode 100644
index 0000000..1894adb
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AprilTagDetectionPipeline.java
@@ -0,0 +1,312 @@
+/*
+ * Copyright (c) 2021 OpenFTC Team
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package org.firstinspires.ftc.teamcode.auto;
+
+import org.opencv.calib3d.Calib3d;
+import org.opencv.core.CvType;
+import org.opencv.core.Mat;
+import org.opencv.core.MatOfDouble;
+import org.opencv.core.MatOfPoint2f;
+import org.opencv.core.MatOfPoint3f;
+import org.opencv.core.Point;
+import org.opencv.core.Point3;
+import org.opencv.core.Scalar;
+import org.opencv.imgproc.Imgproc;
+import org.openftc.apriltag.AprilTagDetection;
+import org.openftc.apriltag.AprilTagDetectorJNI;
+import org.openftc.easyopencv.OpenCvPipeline;
+
+import java.util.ArrayList;
+
+class AprilTagDetectionPipeline extends OpenCvPipeline
+{
+    private long nativeApriltagPtr;
+    private Mat grey = new Mat();
+    private ArrayList<AprilTagDetection> detections = new ArrayList<>();
+
+    private ArrayList<AprilTagDetection> detectionsUpdate = new ArrayList<>();
+    private final Object detectionsUpdateSync = new Object();
+
+    Mat cameraMatrix;
+
+    Scalar blue = new Scalar(7,197,235,255);
+    Scalar red = new Scalar(255,0,0,255);
+    Scalar green = new Scalar(0,255,0,255);
+    Scalar white = new Scalar(255,255,255,255);
+
+    double fx;
+    double fy;
+    double cx;
+    double cy;
+
+    // UNITS ARE METERS
+    double tagsize;
+    double tagsizeX;
+    double tagsizeY;
+
+    private float decimation;
+    private boolean needToSetDecimation;
+    private final Object decimationSync = new Object();
+
+    public AprilTagDetectionPipeline(double tagsize, double fx, double fy, double cx, double cy)
+    {
+        this.tagsize = tagsize;
+        this.tagsizeX = tagsize;
+        this.tagsizeY = tagsize;
+        this.fx = fx;
+        this.fy = fy;
+        this.cx = cx;
+        this.cy = cy;
+
+        constructMatrix();
+
+        // Allocate a native context object. See the corresponding deletion in the finalizer
+        nativeApriltagPtr = AprilTagDetectorJNI.createApriltagDetector(AprilTagDetectorJNI.TagFamily.TAG_36h11.string, 3, 3);
+    }
+
+    @Override
+    public void finalize()
+    {
+        // Might be null if createApriltagDetector() threw an exception
+        if(nativeApriltagPtr != 0)
+        {
+            // Delete the native context we created in the constructor
+            AprilTagDetectorJNI.releaseApriltagDetector(nativeApriltagPtr);
+            nativeApriltagPtr = 0;
+        }
+        else
+        {
+            System.out.println("AprilTagDetectionPipeline.finalize(): nativeApriltagPtr was NULL");
+        }
+    }
+
+    @Override
+    public Mat processFrame(Mat input)
+    {
+        // Convert to greyscale
+        Imgproc.cvtColor(input, grey, Imgproc.COLOR_RGBA2GRAY);
+
+        synchronized (decimationSync)
+        {
+            if(needToSetDecimation)
+            {
+                AprilTagDetectorJNI.setApriltagDetectorDecimation(nativeApriltagPtr, decimation);
+                needToSetDecimation = false;
+            }
+        }
+
+        // Run AprilTag
+        detections = AprilTagDetectorJNI.runAprilTagDetectorSimple(nativeApriltagPtr, grey, tagsize, fx, fy, cx, cy);
+
+        synchronized (detectionsUpdateSync)
+        {
+            detectionsUpdate = detections;
+        }
+
+        // For fun, use OpenCV to draw 6DOF markers on the image. We actually recompute the pose using
+        // OpenCV because I haven't yet figured out how to re-use AprilTag's pose in OpenCV.
+        for(AprilTagDetection detection : detections)
+        {
+            Pose pose = poseFromTrapezoid(detection.corners, cameraMatrix, tagsizeX, tagsizeY);
+            drawAxisMarker(input, tagsizeY/2.0, 6, pose.rvec, pose.tvec, cameraMatrix);
+            draw3dCubeMarker(input, tagsizeX, tagsizeX, tagsizeY, 5, pose.rvec, pose.tvec, cameraMatrix);
+        }
+
+        return input;
+    }
+
+    public void setDecimation(float decimation)
+    {
+        synchronized (decimationSync)
+        {
+            this.decimation = decimation;
+            needToSetDecimation = true;
+        }
+    }
+
+    public ArrayList<AprilTagDetection> getLatestDetections()
+    {
+        return detections;
+    }
+
+    public ArrayList<AprilTagDetection> getDetectionsUpdate()
+    {
+        synchronized (detectionsUpdateSync)
+        {
+            ArrayList<AprilTagDetection> ret = detectionsUpdate;
+            detectionsUpdate = null;
+            return ret;
+        }
+    }
+
+    void constructMatrix()
+    {
+        //     Construct the camera matrix.
+        //
+        //      --         --
+        //     | fx   0   cx |
+        //     | 0    fy  cy |
+        //     | 0    0   1  |
+        //      --         --
+        //
+
+        cameraMatrix = new Mat(3,3, CvType.CV_32FC1);
+
+        cameraMatrix.put(0,0, fx);
+        cameraMatrix.put(0,1,0);
+        cameraMatrix.put(0,2, cx);
+
+        cameraMatrix.put(1,0,0);
+        cameraMatrix.put(1,1,fy);
+        cameraMatrix.put(1,2,cy);
+
+        cameraMatrix.put(2, 0, 0);
+        cameraMatrix.put(2,1,0);
+        cameraMatrix.put(2,2,1);
+    }
+
+    /**
+     * Draw a 3D axis marker on a detection. (Similar to what Vuforia does)
+     *
+     * @param buf the RGB buffer on which to draw the marker
+     * @param length the length of each of the marker 'poles'
+     * @param rvec the rotation vector of the detection
+     * @param tvec the translation vector of the detection
+     * @param cameraMatrix the camera matrix used when finding the detection
+     */
+    void drawAxisMarker(Mat buf, double length, int thickness, Mat rvec, Mat tvec, Mat cameraMatrix)
+    {
+        // The points in 3D space we wish to project onto the 2D image plane.
+        // The origin of the coordinate space is assumed to be in the center of the detection.
+        MatOfPoint3f axis = new MatOfPoint3f(
+                new Point3(0,0,0),
+                new Point3(length,0,0),
+                new Point3(0,length,0),
+                new Point3(0,0,-length)
+        );
+
+        // Project those points
+        MatOfPoint2f matProjectedPoints = new MatOfPoint2f();
+        Calib3d.projectPoints(axis, rvec, tvec, cameraMatrix, new MatOfDouble(), matProjectedPoints);
+        Point[] projectedPoints = matProjectedPoints.toArray();
+
+        // Draw the marker!
+        Imgproc.line(buf, projectedPoints[0], projectedPoints[1], red, thickness);
+        Imgproc.line(buf, projectedPoints[0], projectedPoints[2], green, thickness);
+        Imgproc.line(buf, projectedPoints[0], projectedPoints[3], blue, thickness);
+
+        Imgproc.circle(buf, projectedPoints[0], thickness, white, -1);
+    }
+
+    void draw3dCubeMarker(Mat buf, double length, double tagWidth, double tagHeight, int thickness, Mat rvec, Mat tvec, Mat cameraMatrix)
+    {
+        //axis = np.float32([[0,0,0], [0,3,0], [3,3,0], [3,0,0],
+        //       [0,0,-3],[0,3,-3],[3,3,-3],[3,0,-3] ])
+
+        // The points in 3D space we wish to project onto the 2D image plane.
+        // The origin of the coordinate space is assumed to be in the center of the detection.
+        MatOfPoint3f axis = new MatOfPoint3f(
+                new Point3(-tagWidth/2, tagHeight/2,0),
+                new Point3( tagWidth/2, tagHeight/2,0),
+                new Point3( tagWidth/2,-tagHeight/2,0),
+                new Point3(-tagWidth/2,-tagHeight/2,0),
+                new Point3(-tagWidth/2, tagHeight/2,-length),
+                new Point3( tagWidth/2, tagHeight/2,-length),
+                new Point3( tagWidth/2,-tagHeight/2,-length),
+                new Point3(-tagWidth/2,-tagHeight/2,-length));
+
+        // Project those points
+        MatOfPoint2f matProjectedPoints = new MatOfPoint2f();
+        Calib3d.projectPoints(axis, rvec, tvec, cameraMatrix, new MatOfDouble(), matProjectedPoints);
+        Point[] projectedPoints = matProjectedPoints.toArray();
+
+        // Pillars
+        for(int i = 0; i < 4; i++)
+        {
+            Imgproc.line(buf, projectedPoints[i], projectedPoints[i+4], blue, thickness);
+        }
+
+        // Base lines
+        //Imgproc.line(buf, projectedPoints[0], projectedPoints[1], blue, thickness);
+        //Imgproc.line(buf, projectedPoints[1], projectedPoints[2], blue, thickness);
+        //Imgproc.line(buf, projectedPoints[2], projectedPoints[3], blue, thickness);
+        //Imgproc.line(buf, projectedPoints[3], projectedPoints[0], blue, thickness);
+
+        // Top lines
+        Imgproc.line(buf, projectedPoints[4], projectedPoints[5], green, thickness);
+        Imgproc.line(buf, projectedPoints[5], projectedPoints[6], green, thickness);
+        Imgproc.line(buf, projectedPoints[6], projectedPoints[7], green, thickness);
+        Imgproc.line(buf, projectedPoints[4], projectedPoints[7], green, thickness);
+    }
+
+    /**
+     * Extracts 6DOF pose from a trapezoid, using a camera intrinsics matrix and the
+     * original size of the tag.
+     *
+     * @param points the points which form the trapezoid
+     * @param cameraMatrix the camera intrinsics matrix
+     * @param tagsizeX the original width of the tag
+     * @param tagsizeY the original height of the tag
+     * @return the 6DOF pose of the camera relative to the tag
+     */
+    Pose poseFromTrapezoid(Point[] points, Mat cameraMatrix, double tagsizeX , double tagsizeY)
+    {
+        // The actual 2d points of the tag detected in the image
+        MatOfPoint2f points2d = new MatOfPoint2f(points);
+
+        // The 3d points of the tag in an 'ideal projection'
+        Point3[] arrayPoints3d = new Point3[4];
+        arrayPoints3d[0] = new Point3(-tagsizeX/2, tagsizeY/2, 0);
+        arrayPoints3d[1] = new Point3(tagsizeX/2, tagsizeY/2, 0);
+        arrayPoints3d[2] = new Point3(tagsizeX/2, -tagsizeY/2, 0);
+        arrayPoints3d[3] = new Point3(-tagsizeX/2, -tagsizeY/2, 0);
+        MatOfPoint3f points3d = new MatOfPoint3f(arrayPoints3d);
+
+        // Using this information, actually solve for pose
+        Pose pose = new Pose();
+        Calib3d.solvePnP(points3d, points2d, cameraMatrix, new MatOfDouble(), pose.rvec, pose.tvec, false);
+
+        return pose;
+    }
+
+    /*
+     * A simple container to hold both rotation and translation
+     * vectors, which together form a 6DOF pose.
+     */
+    class Pose
+    {
+        Mat rvec;
+        Mat tvec;
+
+        public Pose()
+        {
+            rvec = new Mat();
+            tvec = new Mat();
+        }
+
+        public Pose(Mat rvec, Mat tvec)
+        {
+            this.rvec = rvec;
+            this.tvec = tvec;
+        }
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonLeft.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutonLeft.java
similarity index 63%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonLeft.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutonLeft.java
index 34b0e14..3c53bd0 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonLeft.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutonLeft.java
@@ -1,9 +1,13 @@
-package org.firstinspires.ftc.teamcode;
+package org.firstinspires.ftc.teamcode.auto;
 
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
-@Autonomous(name = "idk something left")
+import org.firstinspires.ftc.teamcode.Hardware;
+import org.firstinspires.ftc.teamcode.MovementAPI;
+import org.firstinspires.ftc.teamcode.Strings;
+
+@Autonomous(name = "idk something left side", group = Strings.leftSideGroup, preselectTeleOp = Strings.mainOpModeName)
 public class AutonLeft extends LinearOpMode {
     Hardware hardware;
     MovementAPI movement;
@@ -15,7 +19,7 @@ public class AutonLeft extends LinearOpMode {
 
         hardware.wrist.setPosition(0.5);
         sleep(500);
-        hardware.claw.setPosition(1);
+        hardware.setClawPosition(1);
 
         waitForStart();
 
@@ -23,13 +27,13 @@ public class AutonLeft extends LinearOpMode {
         sleep(1000);
         movement.stop();
         sleep(1000);
-        hardware.claw.setPosition(0);
+        hardware.setClawPosition(0);
         sleep(1000);
         movement.move(0, -1, 0, 0.4);
         sleep(1000);
         movement.stop();
         sleep(1000);
-        movement.move(-1, 0, 0, 0.4);
+        movement.move(1, 0, 0, 0.4);
         sleep(2500);
         movement.stop();
     }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutonMovement.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutonMovement.java
new file mode 100644
index 0000000..80636ed
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutonMovement.java
@@ -0,0 +1,165 @@
+package org.firstinspires.ftc.teamcode.auto;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.kinematics.Distance;
+import org.firstinspires.ftc.teamcode.Hardware;
+import org.firstinspires.ftc.teamcode.kinematics.LiftPosition;
+import org.firstinspires.ftc.teamcode.MovementAPI;
+
+public class AutonMovement {
+    Hardware hardware;
+    MovementAPI movement;
+    Telemetry telemetry;
+
+    double speed;
+
+    public final double ENCODER_TICKS_PER_MM = 537.7 / (96 * Math.PI);
+
+    public AutonMovement(Hardware hardware, double speed) {
+        this.hardware = hardware;
+        this.movement = new MovementAPI(hardware);
+        this.speed = speed;
+        this.telemetry = hardware.opMode.telemetry;
+    }
+
+    private void sleep(double seconds) {
+        double startTime = System.nanoTime();
+        double targetTime = startTime + seconds * 1000 * 1000 * 1000;
+
+        while (System.nanoTime() < targetTime);
+    }
+
+    private void movementHelper(Distance distance, boolean strafe) {
+        int flStart = hardware.fl.getCurrentPosition();
+        int frStart = hardware.fr.getCurrentPosition();
+
+        int targetDisplacement = (int) (distance.valInMM() * ENCODER_TICKS_PER_MM);
+
+        if (strafe) targetDisplacement *= (60/52.);
+
+        int loopsCorrect = 0;
+
+        while (true) {
+            int flDisplacement = hardware.fl.getCurrentPosition() - flStart;
+            int frDisplacement = hardware.fr.getCurrentPosition() - frStart;
+
+            if (strafe) frDisplacement *= -1;
+
+            int totalDisplacement = (flDisplacement + frDisplacement) / 2;
+            int displacementDiff = targetDisplacement - totalDisplacement;
+
+            if (Math.abs(displacementDiff) < 10) {
+                loopsCorrect += 1;
+            } else {
+                loopsCorrect = 0;
+            }
+
+            if (loopsCorrect >= 80) {
+                movement.stop();
+                break;
+            }
+
+            double pow;
+            if (Math.abs(displacementDiff) > 300) {
+                pow = 1;
+            } else {
+                pow = Math.sqrt(1 - Math.pow((Math.abs(displacementDiff) / 300.) - 1, 2));
+            }
+
+            telemetry.addLine("pow = " + pow);
+            telemetry.addLine("loopsCorrect = " + loopsCorrect);
+            telemetry.update();
+
+            if (strafe) {
+                movement.move(Math.signum(displacementDiff) * pow, 0, 0, speed);
+            } else {
+                movement.move(0, Math.signum(displacementDiff) * pow, 0, speed);
+            }
+        }
+    }
+
+    public void straight(Distance distance) {
+        this.movementHelper(distance, false);
+    }
+
+    public void strafe(Distance distance) {
+        this.movementHelper(distance, true);
+    }
+
+    private double modAngle(double angle) {
+        double angle2 = angle % 360;
+        if (angle2 >= 180) {
+            return angle2 - 360;
+        } else {
+            return angle2;
+        }
+    }
+
+    public void turn(double degrees) {
+        double startYaw = hardware.getImuYaw();
+        double targetYaw = startYaw + degrees;
+
+        int loopsCorrect = 0;
+
+        while (true) {
+            double yaw = hardware.getImuYaw();
+
+            double yawDiff = modAngle(targetYaw - yaw);
+
+            if (Math.abs(yawDiff) < 1) {
+                loopsCorrect += 1;
+            } else {
+                loopsCorrect = 0;
+            }
+
+            if (loopsCorrect >= 100) {
+                movement.stop();
+                break;
+            }
+
+            double pow;
+            if (Math.abs(yawDiff) > 60) {
+                pow = 1;
+            } else {
+                pow = Math.sqrt(1 - Math.pow((Math.abs(yawDiff) / 60.) - 1, 2));
+            }
+
+            movement.move(0, 0, Math.signum(yawDiff) * pow, speed);
+        }
+    }
+
+    public void liftTo(LiftPosition height) {
+        int loopsCorrect = 0;
+
+        while (true) {
+            int spoolPosition = hardware.getLiftEncoder();
+
+            int spoolPosDiff = height.getEncoderVal() - spoolPosition;
+
+            if (Math.abs(spoolPosDiff) < 10) {
+                loopsCorrect += 1;
+            } else {
+                loopsCorrect = 0;
+            }
+
+            if (loopsCorrect >= 100) {
+                hardware.spool.setPower(0);
+                break;
+            }
+
+            double pow;
+            if (Math.abs(spoolPosDiff) > 300) {
+                pow = 1;
+            } else {
+                pow = Math.sqrt(1 - Math.pow((Math.abs(spoolPosDiff) / 400.) - 1, 2));
+            }
+
+            telemetry.addLine("spool pos = " + spoolPosition);
+            telemetry.addLine("pow = " + pow);
+            telemetry.addLine("loopsCorrect = " + loopsCorrect);
+            telemetry.update();
+
+            hardware.spool.setPower(Math.signum(spoolPosDiff) * pow);
+        }
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonRight.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutonRight.java
similarity index 62%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonRight.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutonRight.java
index 8899967..e9b9440 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonRight.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutonRight.java
@@ -1,9 +1,13 @@
-package org.firstinspires.ftc.teamcode;
+package org.firstinspires.ftc.teamcode.auto;
 
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
-@Autonomous(name = "idk something right")
+import org.firstinspires.ftc.teamcode.Hardware;
+import org.firstinspires.ftc.teamcode.MovementAPI;
+import org.firstinspires.ftc.teamcode.Strings;
+
+@Autonomous(name = "idk something right side", group = Strings.rightSideGroup, preselectTeleOp = Strings.mainOpModeName)
 public class AutonRight extends LinearOpMode {
     Hardware hardware;
     MovementAPI movement;
@@ -15,7 +19,7 @@ public class AutonRight extends LinearOpMode {
 
         hardware.wrist.setPosition(0.5);
         sleep(500);
-        hardware.claw.setPosition(1);
+        hardware.setClawPosition(1);
 
         waitForStart();
 
@@ -23,13 +27,13 @@ public class AutonRight extends LinearOpMode {
         sleep(1000);
         movement.stop();
         sleep(1000);
-        hardware.claw.setPosition(0);
+        hardware.setClawPosition(0);
         sleep(1000);
         movement.move(0, -1, 0, 0.4);
         sleep(1000);
         movement.stop();
         sleep(1000);
-        movement.move(1, 0, 0, 0.4);
+        movement.move(-1, 0, 0, 0.4);
         sleep(2500);
         movement.stop();
     }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/MultiConeAutonLeft.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/MultiConeAutonLeft.java
new file mode 100644
index 0000000..cd07fa4
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/MultiConeAutonLeft.java
@@ -0,0 +1,89 @@
+package org.firstinspires.ftc.teamcode.auto;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.firstinspires.ftc.teamcode.kinematics.Distance;
+import org.firstinspires.ftc.teamcode.Hardware;
+import org.firstinspires.ftc.teamcode.kinematics.LiftPosition;
+import org.firstinspires.ftc.teamcode.Strings;
+
+@Autonomous(name = "Multi-Cone Left Side", group = "don't use me", preselectTeleOp = Strings.mainOpModeName)
+public class MultiConeAutonLeft extends AprilTagAutonBase {
+    Hardware hardware;
+    AutonMovement movement;
+
+    @Override
+    void onInit() {
+        hardware = new Hardware(this);
+        movement = new AutonMovement(hardware, 0.6);
+        hardware.setClawPosition(1);
+        hardware.setWristPosition(-2/3.);
+    }
+
+    @Override
+    void onPlay(int signalZone) {
+        // move to the junction
+        movement.straight(Distance.inTiles(1).sub(Distance.ROBOT_LENGTH).div(2));
+        movement.strafe(Distance.inTiles(1).sub(Distance.ONE_TILE_WITHOUT_BORDER.sub(Distance.ROBOT_WIDTH).div(2)));
+        movement.straight(Distance.inTiles(1.5));
+        movement.turn(90);
+
+        // lift and move forward
+        movement.liftTo(LiftPosition.HIGH);
+        movement.straight(Distance.inInches(4));
+
+        // wrist down, drop, wrist up
+        hardware.setWristPosition(0);
+        sleep(500);
+        hardware.setClawPosition(0);
+        sleep(500);
+        hardware.setClawPosition(1);
+
+        // move back and lower
+        movement.straight(Distance.inInches(-4));
+        movement.liftTo(LiftPosition.STACK);
+
+        movement.turn(180);
+        movement.strafe(Distance.inTiles(0.5));
+
+        hardware.setClawPosition(0);
+
+        movement.straight(Distance.inTiles(2).add(Distance.inTiles(1).sub(Distance.ROBOT_LENGTH).div(2)));
+
+        hardware.setClawPosition(1);
+
+        movement.liftTo(LiftPosition.HIGH);
+        hardware.setWristPosition(-2/3.);
+
+        movement.straight(Distance.inTiles(-1.5).sub(Distance.inTiles(1).sub(Distance.ROBOT_LENGTH).div(2)));
+        movement.turn(90);
+
+        // lift and move forward
+        movement.liftTo(LiftPosition.HIGH);
+        movement.straight(Distance.inInches(4));
+
+        // wrist down, drop, wrist up
+        hardware.setWristPosition(0);
+        sleep(500);
+        hardware.setClawPosition(0);
+        sleep(500);
+        hardware.setClawPosition(1);
+        hardware.setWristPosition(-2/3.);
+
+        // move back and lower
+        movement.straight(Distance.inInches(-4));
+        movement.liftTo(LiftPosition.FLOOR);
+
+//        switch (signalZone) {
+//            case 1:
+//                movement.strafe(Distance.inTiles(-2.5));
+//                break;
+//            case 2:
+//                movement.strafe(Distance.inTiles(-1.5));
+//                break;
+//            case 3:
+//                movement.strafe(Distance.inTiles(-0.5));
+//                break;
+//        }
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/OneConeAutonLeft.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/OneConeAutonLeft.java
new file mode 100644
index 0000000..8d08c13
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/OneConeAutonLeft.java
@@ -0,0 +1,59 @@
+package org.firstinspires.ftc.teamcode.auto;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.firstinspires.ftc.teamcode.kinematics.Distance;
+import org.firstinspires.ftc.teamcode.Hardware;
+import org.firstinspires.ftc.teamcode.kinematics.LiftPosition;
+import org.firstinspires.ftc.teamcode.Strings;
+
+@Autonomous(name = "1-Cone Left Side", group = Strings.leftSideGroup, preselectTeleOp = Strings.mainOpModeName)
+public class OneConeAutonLeft extends AprilTagAutonBase {
+    Hardware hardware;
+    AutonMovement movement;
+
+    @Override
+    void onInit() {
+        hardware = new Hardware(this);
+        movement = new AutonMovement(hardware, 0.6);
+        hardware.setClawPosition(1);
+        hardware.setWristPosition(-2/3.);
+    }
+
+    @Override
+    void onPlay(int signalZone) {
+        // move to the junction
+        movement.straight(Distance.inTiles(1).sub(Distance.ROBOT_LENGTH).div(2));
+        movement.strafe(Distance.inTiles(1).sub(Distance.ONE_TILE_WITHOUT_BORDER.sub(Distance.ROBOT_WIDTH).div(2)));
+        movement.straight(Distance.inTiles(1));
+        movement.strafe(Distance.inTiles(0.5));
+
+        // lift and move forward
+        movement.liftTo(LiftPosition.HIGH);
+        movement.straight(Distance.inInches(2));
+
+        // wrist down, drop, wrist up
+        hardware.setWristPosition(0);
+        sleep(500);
+        hardware.setClawPosition(0);
+        sleep(500);
+        hardware.setClawPosition(1);
+        hardware.setWristPosition(-2/3.);
+
+        // move back and lower
+        movement.straight(Distance.inInches(-2));
+        movement.liftTo(LiftPosition.FLOOR);
+
+        switch (signalZone) {
+            case 1:
+                movement.strafe(Distance.inTiles(-2.5));
+                break;
+            case 2:
+                movement.strafe(Distance.inTiles(-1.5));
+                break;
+            case 3:
+                movement.strafe(Distance.inTiles(-0.5));
+                break;
+        }
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/OneConeAutonRight.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/OneConeAutonRight.java
new file mode 100644
index 0000000..a216550
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/OneConeAutonRight.java
@@ -0,0 +1,59 @@
+package org.firstinspires.ftc.teamcode.auto;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.firstinspires.ftc.teamcode.kinematics.Distance;
+import org.firstinspires.ftc.teamcode.Hardware;
+import org.firstinspires.ftc.teamcode.kinematics.LiftPosition;
+import org.firstinspires.ftc.teamcode.Strings;
+
+@Autonomous(name = "1-Cone Right Side", group = Strings.rightSideGroup, preselectTeleOp = Strings.mainOpModeName)
+public class OneConeAutonRight extends AprilTagAutonBase {
+    Hardware hardware;
+    AutonMovement movement;
+
+    @Override
+    void onInit() {
+        hardware = new Hardware(this);
+        movement = new AutonMovement(hardware, 0.6);
+        hardware.setClawPosition(1);
+        hardware.setWristPosition(-2/3.);
+    }
+
+    @Override
+    void onPlay(int signalZone) {
+        // move to the junction
+        movement.straight(Distance.inTiles(1).sub(Distance.ROBOT_LENGTH).div(2));
+        movement.strafe(Distance.inTiles(-1).sub(Distance.ONE_TILE_WITHOUT_BORDER.sub(Distance.ROBOT_WIDTH).div(2)));
+        movement.straight(Distance.inTiles(1));
+        movement.strafe(Distance.inTiles(-0.5));
+
+        // lift and move forward
+        movement.liftTo(LiftPosition.HIGH);
+        movement.straight(Distance.inInches(2));
+
+        // wrist down, drop, wrist up
+        hardware.setWristPosition(0);
+        sleep(500);
+        hardware.setClawPosition(0);
+        sleep(500);
+        hardware.setClawPosition(1);
+        hardware.setWristPosition(-2/3.);
+
+        // move back and lower
+        movement.straight(Distance.inInches(-2));
+        movement.liftTo(LiftPosition.FLOOR);
+
+        switch (signalZone) {
+            case 1:
+                movement.strafe(Distance.inTiles(0.5));
+                break;
+            case 2:
+                movement.strafe(Distance.inTiles(1.5));
+                break;
+            case 3:
+                movement.strafe(Distance.inTiles(2.5));
+                break;
+        }
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/ResetEncoders.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/ResetEncoders.java
new file mode 100644
index 0000000..b6af035
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/ResetEncoders.java
@@ -0,0 +1,21 @@
+package org.firstinspires.ftc.teamcode.auto;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.firstinspires.ftc.teamcode.Hardware;
+
+@Autonomous(name = "Reset encoders + imu yaw")
+public class ResetEncoders extends LinearOpMode {
+    @Override
+    public void runOpMode() {
+        Hardware hardware = new Hardware(this);
+        hardware.fl.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        hardware.fr.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        hardware.bl.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        hardware.br.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        hardware.spool.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        hardware.imu.resetYaw();
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/Test.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/Test.java
new file mode 100644
index 0000000..9fe1516
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/Test.java
@@ -0,0 +1,36 @@
+package org.firstinspires.ftc.teamcode.auto;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.Hardware;
+import org.firstinspires.ftc.teamcode.kinematics.LiftPosition;
+import org.firstinspires.ftc.teamcode.kinematics.NullablePose;
+import org.firstinspires.ftc.teamcode.kinematics.FieldVector;
+import org.firstinspires.ftc.teamcode.kinematics.Poser;
+
+@Autonomous(name = "test")
+public class Test extends LinearOpMode {
+    Hardware hardware;
+    Poser poser;
+
+    @Override
+    public void runOpMode() {
+        hardware = new Hardware(this);
+        hardware.setWristPosition(-0.6);
+        poser = new Poser(hardware, 0.3, FieldVector.ZERO, 0);
+
+        poser.goToNullablePose(new NullablePose());
+
+        waitForStart();
+
+        while (!isStopRequested()) {
+            poser.goToNullablePose(new NullablePose(FieldVector.inTiles(2, 0), 90., LiftPosition.FLOOR));
+            sleep(1000);
+            poser.goToNullablePose(new NullablePose(FieldVector.inTiles(0, 1), -135., LiftPosition.HIGH));
+            sleep(1000);
+            poser.goToNullablePose(new NullablePose(FieldVector.inTiles(0, 0), 0., LiftPosition.MEDIUM));
+            sleep(1000);
+        }
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/ZeroConeAutonLeft.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/ZeroConeAutonLeft.java
new file mode 100644
index 0000000..864aa15
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/ZeroConeAutonLeft.java
@@ -0,0 +1,41 @@
+package org.firstinspires.ftc.teamcode.auto;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.firstinspires.ftc.teamcode.kinematics.Distance;
+import org.firstinspires.ftc.teamcode.Hardware;
+import org.firstinspires.ftc.teamcode.Strings;
+
+@Autonomous(name = "0-Cone Left Side", group = Strings.leftSideGroup, preselectTeleOp = Strings.mainOpModeName)
+public class ZeroConeAutonLeft extends AprilTagAutonBase {
+    Hardware hardware;
+    AutonMovement movement;
+
+    @Override
+    void onInit() {
+        hardware = new Hardware(this);
+        movement = new AutonMovement(hardware, 0.6);
+        hardware.setClawPosition(1);
+        hardware.setWristPosition(-2/3.);
+    }
+
+    @Override
+    void onPlay(int signalZone) {
+        // move to the junction
+        movement.straight(Distance.inTiles(1).sub(Distance.ROBOT_LENGTH).div(2));
+        movement.strafe(Distance.inTiles(1).sub(Distance.ONE_TILE_WITHOUT_BORDER.sub(Distance.ROBOT_WIDTH).div(2)));
+        movement.straight(Distance.inTiles(1));
+
+        switch (signalZone) {
+            case 1:
+                movement.strafe(Distance.inTiles(-2));
+                break;
+            case 2:
+                movement.strafe(Distance.inTiles(-1));
+                break;
+            case 3:
+                // do nothing
+                break;
+        }
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/ZeroConeAutonRight.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/ZeroConeAutonRight.java
new file mode 100644
index 0000000..91651f2
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/ZeroConeAutonRight.java
@@ -0,0 +1,40 @@
+package org.firstinspires.ftc.teamcode.auto;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.firstinspires.ftc.teamcode.kinematics.Distance;
+import org.firstinspires.ftc.teamcode.Hardware;
+import org.firstinspires.ftc.teamcode.Strings;
+
+@Autonomous(name = "0-Cone Right Side", group = Strings.rightSideGroup, preselectTeleOp = Strings.mainOpModeName)
+public class ZeroConeAutonRight extends AprilTagAutonBase {
+    Hardware hardware;
+    AutonMovement movement;
+
+    @Override
+    void onInit() {
+        hardware = new Hardware(this);
+        movement = new AutonMovement(hardware, 0.6);
+        hardware.setClawPosition(1);
+        hardware.setWristPosition(-2/3.);
+    }
+
+    @Override
+    void onPlay(int signalZone) {
+        movement.straight(Distance.inTiles(1).sub(Distance.ROBOT_LENGTH).div(2));
+        movement.strafe(Distance.inTiles(-1).sub(Distance.ONE_TILE_WITHOUT_BORDER.sub(Distance.ROBOT_WIDTH).div(2)));
+        movement.straight(Distance.inTiles(1));
+
+        switch (signalZone) {
+            case 1:
+                // do nothing
+                break;
+            case 2:
+                movement.strafe(Distance.inTiles(1));
+                break;
+            case 3:
+                movement.strafe(Distance.inTiles(2));
+                break;
+        }
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/config/ConfigSleeve123.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/config/ConfigSleeve123.java
new file mode 100644
index 0000000..d7fa06f
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/config/ConfigSleeve123.java
@@ -0,0 +1,12 @@
+package org.firstinspires.ftc.teamcode.config;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+@Autonomous(name = "Signal Sleeve With Tags 1, 2, 3", group = "Config")
+public class ConfigSleeve123 extends LinearOpMode {
+    @Override
+    public void runOpMode() {
+        RuntimeConfig.SIGNAL_SLEEVE_VARIANT = 3;
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/config/ConfigSleeveVariantA.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/config/ConfigSleeveVariantA.java
new file mode 100644
index 0000000..1c7d282
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/config/ConfigSleeveVariantA.java
@@ -0,0 +1,12 @@
+package org.firstinspires.ftc.teamcode.config;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+@Autonomous(name = "Signal Sleeve Variant A", group = "Config")
+public class ConfigSleeveVariantA extends LinearOpMode {
+    @Override
+    public void runOpMode() {
+        RuntimeConfig.SIGNAL_SLEEVE_VARIANT = 0;
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/config/ConfigSleeveVariantB.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/config/ConfigSleeveVariantB.java
new file mode 100644
index 0000000..b01e116
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/config/ConfigSleeveVariantB.java
@@ -0,0 +1,12 @@
+package org.firstinspires.ftc.teamcode.config;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+@Autonomous(name = "Signal Sleeve Variant B", group = "Config")
+public class ConfigSleeveVariantB extends LinearOpMode {
+    @Override
+    public void runOpMode() {
+        RuntimeConfig.SIGNAL_SLEEVE_VARIANT = 1;
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/config/ConfigSleeveVariantC.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/config/ConfigSleeveVariantC.java
new file mode 100644
index 0000000..02c6586
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/config/ConfigSleeveVariantC.java
@@ -0,0 +1,12 @@
+package org.firstinspires.ftc.teamcode.config;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+@Autonomous(name = "Signal Sleeve Variant C", group = "Config")
+public class ConfigSleeveVariantC extends LinearOpMode {
+    @Override
+    public void runOpMode() {
+        RuntimeConfig.SIGNAL_SLEEVE_VARIANT = 2;
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/config/RuntimeConfig.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/config/RuntimeConfig.java
new file mode 100644
index 0000000..eb82334
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/config/RuntimeConfig.java
@@ -0,0 +1,24 @@
+package org.firstinspires.ftc.teamcode.config;
+
+public class RuntimeConfig {
+    public static Integer SIGNAL_SLEEVE_VARIANT = null;
+
+    public static String signalSleeveVariantName() {
+        return RuntimeConfig.signalSleeveVariantName(SIGNAL_SLEEVE_VARIANT);
+    }
+
+    public static String signalSleeveVariantName(int variant) {
+        switch (variant) {
+            case 0:
+                return "A";
+            case 1:
+                return "B";
+            case 2:
+                return "C";
+            case 3:
+                return "123";
+            default:
+                return SIGNAL_SLEEVE_VARIANT + "";
+        }
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/Distance.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/Distance.java
new file mode 100644
index 0000000..338b957
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/Distance.java
@@ -0,0 +1,89 @@
+package org.firstinspires.ftc.teamcode.kinematics;
+
+public class Distance {
+    private final double val; // in millimeters
+
+    public static final double IN_PER_TILE = 23.625;
+    public static final double MM_PER_IN = 25.4;
+    public static final double MM_PER_FOOT = 304.8; // 25.4 * 12 but without floating point problems
+
+    public static final Distance ZERO = new Distance(0);
+    public static final Distance ROBOT_LENGTH = Distance.inInches(18);
+    public static final Distance ROBOT_WIDTH = Distance.inInches(15.5);
+
+    public static final Distance TILE_BORDER = Distance.inInches(0.75);
+    public static final Distance ONE_TILE_WITHOUT_BORDER = Distance.inTiles(1).sub(Distance.TILE_BORDER);
+    public static final Distance ONE_TILE_WITH_BORDER = Distance.inTiles(1).add(Distance.TILE_BORDER);
+
+    public static Distance inDefaultUnits(double val) {
+        return new Distance(val);
+    }
+
+    public static Distance inTiles(double val) {
+        return Distance.inInches(val * IN_PER_TILE);
+    }
+
+    public static Distance inFeet(double val) {
+        return Distance.inMM(val * MM_PER_FOOT);
+    }
+
+    public static Distance inInches(double val) {
+        return Distance.inMM(val * MM_PER_IN);
+    }
+
+    public static Distance inMM(double val) {
+        return new Distance(val);
+    }
+
+    public double valInDefaultUnits() {
+        return this.val;
+    }
+
+    public double valInTiles() {
+        return this.valInInches() / IN_PER_TILE;
+    }
+
+    public double valInFeet() {
+        return this.valInMM() / MM_PER_FOOT;
+    }
+
+    public double valInInches() {
+        return this.valInMM() / MM_PER_IN;
+    }
+
+    public double valInMM() {
+        return this.val;
+    }
+
+    private Distance(double val) {
+        this.val = val;
+    }
+
+    public String toString() {
+        return Double.toString(this.val);
+    }
+
+    public boolean isZero() {
+        return this.val == 0;
+    }
+
+    public Distance add(Distance rhs) {
+        return new Distance(this.val + rhs.val);
+    }
+
+    public Distance sub(Distance rhs) {
+        return new Distance(this.val - rhs.val);
+    }
+
+    public Distance mul(double rhs) {
+        return new Distance(this.val * rhs);
+    }
+
+    public Distance div(double rhs) {
+        return new Distance(this.val / rhs);
+    }
+
+    public Distance neg() {
+        return new Distance(-this.val);
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/EncoderIntegrator.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/EncoderIntegrator.java
new file mode 100644
index 0000000..4e167d0
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/EncoderIntegrator.java
@@ -0,0 +1,117 @@
+package org.firstinspires.ftc.teamcode.kinematics;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.Hardware;
+
+public class EncoderIntegrator {
+    Hardware hardware;
+
+    FieldVector pos;
+    // `yaw` is the robot angle. `imuYaw` is the last measured value from the imu. usually just
+    // offset by a constant but that could be different in the future, i.e. radians or smth.
+    double yaw;
+
+    int fl;
+    int fr;
+    int bl;
+    int br;
+    double imuYaw;
+
+    Telemetry telemetry;
+
+    public final double MM_PER_ENCODER_TICK = (96 * Math.PI) / 537.7;
+    public final double WHEELBASE_LENGTH = 337;
+    public final double WHEELBASE_WIDTH = 355;
+    public final double WHEELBASE_DIAGONAL = Math.hypot(WHEELBASE_LENGTH, WHEELBASE_WIDTH );
+    public final double DEGREES_PER_MM = 360 / (WHEELBASE_DIAGONAL * Math.PI);
+
+    public EncoderIntegrator(Hardware hardware, FieldVector initialPos, double initialYaw, Telemetry telemetry) {
+        this.hardware = hardware;
+        this.pos = initialPos;
+        this.yaw = initialYaw;
+        this.telemetry = telemetry;
+
+        this.fl = hardware.fl.getCurrentPosition();
+        this.fr = hardware.fr.getCurrentPosition();
+        this.bl = hardware.bl.getCurrentPosition();
+        this.br = hardware.br.getCurrentPosition();
+        this.imuYaw = hardware.getImuYaw();
+    }
+
+    private static double sinc(double x) {
+        if (x == 0) {
+            return 1;
+        } else {
+            return Math.sin(x) / x;
+        }
+    }
+
+    // this function has no generally well-known name, but its similar to `sinc` so im calling it `cosc`
+    private static double cosc(double x) {
+        if (x == 0) {
+            return 0;
+        } else {
+            return (1 - Math.cos(x)) / x;
+        }
+    }
+
+    public void update() {
+        int newFl = hardware.fl.getCurrentPosition();
+        int newFr = hardware.fr.getCurrentPosition();
+        int newBl = hardware.bl.getCurrentPosition();
+        int newBr = hardware.br.getCurrentPosition();
+        double newImuYaw = hardware.getImuYaw();
+
+        double flDiff = newFl - this.fl;
+        double frDiff = newFr - this.fr;
+        double blDiff = newBl - this.bl;
+        double brDiff = newBr - this.br;
+        double imuYawDiff = newImuYaw - this.imuYaw;
+
+//        telemetry.addData("flDiff", flDiff);
+//        telemetry.addData("frDiff", frDiff);
+//        telemetry.addData("blDiff", blDiff);
+//        telemetry.addData("brDiff", brDiff);
+//        telemetry.addData("imuYawDiff", imuYawDiff);
+
+//        fl = powerY + powerX + turn + noop
+//        fr = powerY - powerX - turn + noop
+//        bl = powerY - powerX + turn - noop
+//        br = powerY + powerX - turn - noop
+
+        double relativeYDiff = ((flDiff + frDiff + blDiff + brDiff) / 4.) * MM_PER_ENCODER_TICK;
+        double relativeXDiff = ((flDiff - frDiff - blDiff + brDiff) / 4.) * MM_PER_ENCODER_TICK;
+        double turnDiff = ((flDiff - frDiff + blDiff - brDiff) / 4.) * MM_PER_ENCODER_TICK * DEGREES_PER_MM;
+        double noopDiff = (flDiff + frDiff - blDiff - brDiff) / 4.;
+
+        // aaaaand then we are just going to ignore `turnDiff` and `noopDiff`. there is nothing
+        // meaningful we can do with these values to help correct the other values. the noop value
+        // is ignored for obvious reasons. the turn value is ignored since we get the turn from the
+        // imu and using encoders for turn has proven to work poorly.
+
+//        telemetry.addData("relativeYDiff", relativeYDiff);
+//        telemetry.addData("relativeXDiff", relativeXDiff);
+
+        double imuYawDiffRadians = Math.toRadians(imuYawDiff);
+        double poseExponentiationX = cosc(imuYawDiffRadians);
+        double poseExponentiationY = sinc(imuYawDiffRadians);
+
+        FieldVector relativePosDiff = FieldVector.inMM(
+                relativeYDiff * poseExponentiationX + relativeXDiff * poseExponentiationY,
+                relativeYDiff * poseExponentiationY - relativeXDiff * poseExponentiationX
+        );
+
+        FieldVector posDiff = relativePosDiff.rot(this.imuYaw);
+        double yawDiff = imuYawDiff;
+
+        this.pos = this.pos.add(posDiff);
+        this.yaw += yawDiff;
+
+        this.fl = newFl;
+        this.fr = newFr;
+        this.bl = newBl;
+        this.br = newBr;
+        this.imuYaw = newImuYaw;
+        this.imuYaw %= 360;
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/FieldVector.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/FieldVector.java
new file mode 100644
index 0000000..bec854f
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/FieldVector.java
@@ -0,0 +1,79 @@
+package org.firstinspires.ftc.teamcode.kinematics;
+
+public class FieldVector {
+    public final Distance x;
+    public final Distance y;
+
+    public static final FieldVector ZERO = new FieldVector(Distance.ZERO, Distance.ZERO);
+//    public static final FieldVector RED_LEFT_SIDE_START = FieldVector.inTiles(-1.5, -2.5);
+
+    public static FieldVector inTiles(double x, double y) {
+        return new FieldVector(Distance.inTiles(x), Distance.inTiles(y));
+    }
+
+    public static FieldVector inFeet(double x, double y) {
+        return new FieldVector(Distance.inFeet(x), Distance.inFeet(y));
+    }
+
+    public static FieldVector inMM(double x, double y) {
+        return new FieldVector(Distance.inMM(x), Distance.inMM(y));
+    }
+
+    public FieldVector(Distance x, Distance y) {
+        this.x = x;
+        this.y = y;
+    }
+
+    public String toString() {
+        return "(" + this.x + ", " + this.y + ")";
+    }
+
+    public FieldVector add(FieldVector rhs) {
+        return new FieldVector(this.x.add(rhs.x), this.y.add(rhs.y));
+    }
+
+    public FieldVector sub(FieldVector rhs) {
+        return new FieldVector(this.x.sub(rhs.x), this.y.sub(rhs.y));
+    }
+
+    public FieldVector mul(double rhs) {
+        return new FieldVector(this.x.mul(rhs), this.y.mul(rhs));
+    }
+
+    public FieldVector div(double rhs) {
+        return new FieldVector(this.x.div(rhs), this.y.div(rhs));
+    }
+
+    public FieldVector rot(double rhs) {
+        double sin = Math.sin(Math.toRadians(rhs));
+        double cos = Math.cos(Math.toRadians(rhs));
+        return new FieldVector(this.y.mul(sin).add(this.x.mul(cos)), this.y.mul(cos).sub(this.x.mul(sin)));
+    }
+
+    public FieldVector neg() {
+        return new FieldVector(this.x.neg(), this.y.neg());
+    }
+
+    public Distance magnitude() {
+        return Distance.inDefaultUnits(Math.sqrt(this.sqMagnitude().valInDefaultUnits()));
+    }
+
+    public Distance sqMagnitude() {
+        return Distance.inDefaultUnits(
+                this.x.valInDefaultUnits() * this.x.valInDefaultUnits()
+                        + this.y.valInDefaultUnits() * this.y.valInDefaultUnits()
+        );
+    }
+
+    public FieldVector normalized() {
+        if (this.magnitude().isZero()) {
+            return FieldVector.ZERO;
+        } else {
+            return this.div(this.magnitude().valInDefaultUnits());
+        }
+    }
+
+    public double angle() {
+        return 90 - Math.toDegrees(Math.atan2(this.y.valInDefaultUnits(), this.x.valInDefaultUnits()));
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/LiftPosition.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/LiftPosition.java
new file mode 100644
index 0000000..ab0ecaf
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/LiftPosition.java
@@ -0,0 +1,57 @@
+package org.firstinspires.ftc.teamcode.kinematics;
+
+import com.acmerobotics.dashboard.config.Config;
+
+@Config
+public enum LiftPosition {
+    FLOOR, STACK, GROUND, LOW, MEDIUM, HIGH;
+
+    public static int FLOOR_ENCODER_VAL = 10;
+    public static int STACK_ENCODER_VAL = 600;
+    public static int GROUND_ENCODER_VAL = 300;
+    public static int LOW_ENCODER_VAL = 1000;
+    public static int MEDIUM_ENCODER_VAL = 2000;
+    public static int HIGH_ENCODER_VAL = 2900;
+
+    LiftPosition() {
+
+    }
+
+    public int getEncoderVal() {
+        switch (this) {
+            case FLOOR:
+                return FLOOR_ENCODER_VAL;
+            case STACK:
+                return STACK_ENCODER_VAL;
+            case GROUND:
+                return GROUND_ENCODER_VAL;
+            case LOW:
+                return LOW_ENCODER_VAL;
+            case MEDIUM:
+                return MEDIUM_ENCODER_VAL;
+            case HIGH:
+                return HIGH_ENCODER_VAL;
+            default:
+                return 0;
+        }
+    }
+
+    public String toString() {
+        switch (this) {
+            case FLOOR:
+                return "LiftPosition.FLOOR";
+            case STACK:
+                return "LiftPosition.STACK";
+            case GROUND:
+                return "LiftPosition.GROUND";
+            case LOW:
+                return "LiftPosition.LOW";
+            case MEDIUM:
+                return "LiftPosition.MEDIUM";
+            case HIGH:
+                return "LiftPosition.HIGH";
+            default:
+                return "uh... we dont know?";
+        }
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/NullablePose.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/NullablePose.java
new file mode 100644
index 0000000..3a2d254
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/NullablePose.java
@@ -0,0 +1,96 @@
+package org.firstinspires.ftc.teamcode.kinematics;
+
+import androidx.annotation.Nullable;
+
+public class NullablePose {
+    @Nullable
+    public final FieldVector pos;
+    @Nullable
+    public final Double yaw;
+    @Nullable
+    public final Integer liftPos;
+
+    public NullablePose(FieldVector pos, double yaw, int liftPos) {
+        this.pos = pos;
+        this.yaw = yaw;
+        this.liftPos = liftPos;
+    }
+
+    public NullablePose(FieldVector pos, double yaw, LiftPosition liftPos) {
+        this.pos = pos;
+        this.yaw = yaw;
+        this.liftPos = liftPos.getEncoderVal();
+    }
+
+    public NullablePose(FieldVector pos, double yaw) {
+        this.pos = pos;
+        this.yaw = yaw;
+        this.liftPos = null;
+    }
+
+    public NullablePose(double yaw, int liftPos) {
+        this.pos = null;
+        this.yaw = yaw;
+        this.liftPos = liftPos;
+    }
+
+    public NullablePose(double yaw, LiftPosition liftPos) {
+        this.pos = null;
+        this.yaw = yaw;
+        this.liftPos = liftPos.getEncoderVal();
+    }
+
+    public NullablePose(FieldVector pos, int liftPos) {
+        this.pos = pos;
+        this.yaw = null;
+        this.liftPos = liftPos;
+    }
+
+    public NullablePose(FieldVector pos, LiftPosition liftPos) {
+        this.pos = pos;
+        this.yaw = null;
+        this.liftPos = liftPos.getEncoderVal();
+    }
+
+    public NullablePose(FieldVector pos) {
+        this.pos = pos;
+        this.yaw = null;
+        this.liftPos = null;
+    }
+
+    public NullablePose(double yaw) {
+        this.pos = null;
+        this.yaw = yaw;
+        this.liftPos = null;
+    }
+
+    public NullablePose(int liftPos) {
+        this.pos = null;
+        this.yaw = null;
+        this.liftPos = liftPos;
+    }
+
+    public NullablePose(LiftPosition liftPos) {
+        this.pos = null;
+        this.yaw = null;
+        this.liftPos = liftPos.getEncoderVal();
+    }
+
+    public NullablePose() {
+        this.pos = null;
+        this.yaw = null;
+        this.liftPos = null;
+    }
+
+    public NullablePose or(NullablePose other) {
+        return new NullablePose(
+                this.pos != null ? this.pos : other.pos,
+                this.yaw != null ? this.yaw : other.yaw,
+                this.liftPos != null ? this.liftPos : other.liftPos
+        );
+    }
+
+    public NullablePose and(NullablePose other) {
+        return other.or(this);
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/Pose.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/Pose.java
new file mode 100644
index 0000000..3f279b1
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/Pose.java
@@ -0,0 +1,19 @@
+package org.firstinspires.ftc.teamcode.kinematics;
+
+public class Pose {
+    public final FieldVector pos;
+    public final double yaw;
+    public final int liftPos;
+
+    public final static Pose ZERO = new Pose(
+            FieldVector.ZERO,
+            0,
+            0
+    );
+
+    public Pose(FieldVector pos, double yaw, int liftPos) {
+        this.pos = pos;
+        this.yaw = yaw;
+        this.liftPos = liftPos;
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/Poser.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/Poser.java
new file mode 100644
index 0000000..a23f8d4
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/Poser.java
@@ -0,0 +1,118 @@
+package org.firstinspires.ftc.teamcode.kinematics;
+
+import com.acmerobotics.dashboard.config.Config;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.Hardware;
+import org.firstinspires.ftc.teamcode.MovementAPI;
+
+@Config
+public class Poser {
+    Hardware hardware;
+    MovementAPI movement;
+    Telemetry telemetry;
+    EncoderIntegrator integrator;
+
+    double speed;
+
+    public static double POS_STOP_THRESHOLD = 5;
+    public static double YAW_STOP_THRESHOLD = 0.5;
+    public static int LIFT_STOP_THRESHOLD = 7;
+    public static int SLOWDOWN_THRESHOLD_FACTOR = 40;
+
+    public Poser(Hardware hardware, double speed, FieldVector initalPos, double initialYaw) {
+        this.hardware = hardware;
+        this.movement = new MovementAPI(hardware);
+        this.speed = speed;
+        this.telemetry = hardware.opMode.telemetry;
+        this.integrator = new EncoderIntegrator(hardware, initalPos, initialYaw, telemetry);
+    }
+
+    private Pose getPose() {
+        return new Pose(
+                integrator.pos,
+                integrator.imuYaw,
+                hardware.getLiftEncoder()
+        );
+    }
+
+    public static double ellipticalCurve(double x, double slowdownThreshold) {
+        if (Math.abs(x) > slowdownThreshold) {
+            return Math.signum(x);
+        } else {
+            return Math.signum(x) * Math.sqrt(1 - Math.pow((Math.abs(x) / slowdownThreshold) - 1, 2));
+        }
+    }
+
+    public void goToNullablePose(NullablePose targetPose) {
+        Pose currentPose = this.getPose();
+
+        this.goToPose(new Pose(
+                targetPose.pos != null ? targetPose.pos : currentPose.pos,
+                targetPose.yaw != null ? targetPose.yaw : currentPose.yaw,
+                targetPose.liftPos != null ? targetPose.liftPos : currentPose.liftPos
+        ));
+    }
+
+    public void goToPose(Pose targetPose) {
+        int loopsCorrect = 0;
+
+        while (true) {
+            this.integrator.update();
+
+            Pose currentPose = this.getPose();
+
+            FieldVector posDiff = targetPose.pos.sub(currentPose.pos);
+            double yawDiff = targetPose.yaw - currentPose.yaw;
+            int liftDiff = targetPose.liftPos - currentPose.liftPos;
+
+            if (
+                    Math.abs(posDiff.magnitude().valInMM()) < POS_STOP_THRESHOLD
+                            && Math.abs(yawDiff) < YAW_STOP_THRESHOLD
+                            && Math.abs(liftDiff) < LIFT_STOP_THRESHOLD
+            ) {
+                loopsCorrect += 1;
+            } else {
+                loopsCorrect = 0;
+            }
+
+            if (loopsCorrect >= 120) {
+                movement.stop();
+                hardware.spool.setPower(0);
+                break;
+            }
+
+            telemetry.addData("posX", currentPose.pos.x);
+            telemetry.addData("posY", currentPose.pos.y);
+
+            telemetry.addData("posDiff", posDiff);
+            telemetry.addData("posXDiff", posDiff.x);
+            telemetry.addData("posYDiff", posDiff.y);
+            telemetry.addData("yawDiff", yawDiff);
+            telemetry.addData("liftDiff", liftDiff);
+
+            FieldVector posPower = posDiff.normalized().mul(ellipticalCurve(posDiff.magnitude().valInMM(), POS_STOP_THRESHOLD * SLOWDOWN_THRESHOLD_FACTOR));
+            double yawPower = ellipticalCurve(yawDiff, YAW_STOP_THRESHOLD * SLOWDOWN_THRESHOLD_FACTOR);
+            double liftPower = ellipticalCurve(liftDiff, LIFT_STOP_THRESHOLD * SLOWDOWN_THRESHOLD_FACTOR);
+
+            telemetry.addData("posPower", posPower);
+            telemetry.addData("posXPower", posPower.x);
+            telemetry.addData("posYPower", posPower.y);
+            telemetry.addData("yawPower", yawPower);
+            telemetry.addData("liftPower", liftPower);
+            telemetry.addData("loopsCorrect", loopsCorrect);
+            hardware.dashboardTelemetryWrapper.drawTargetOnDashboard(targetPose, currentPose);
+            hardware.dashboardTelemetryWrapper.drawRobot(currentPose.pos, currentPose.yaw);
+            telemetry.update();
+
+            posPower = posPower.rot(-currentPose.yaw);
+            movement.move(
+                    posPower.x.valInDefaultUnits(),
+                    posPower.y.valInDefaultUnits(),
+                    yawPower,
+                    speed
+            );
+            hardware.spool.setPower(liftPower);
+        }
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Gamepad.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Gamepad.java
new file mode 100644
index 0000000..484218b
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Gamepad.java
@@ -0,0 +1,115 @@
+package org.firstinspires.ftc.teamcode.teleop;
+
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.Hardware;
+import org.firstinspires.ftc.teamcode.MotorPowers;
+import org.firstinspires.ftc.teamcode.MovementAPI;
+import org.firstinspires.ftc.teamcode.Strings;
+import org.firstinspires.ftc.teamcode.kinematics.LiftPosition;
+import org.firstinspires.ftc.teamcode.kinematics.Poser;
+
+@TeleOp(name = Strings.dualGamepadOpModeName)
+public class Gamepad extends OpMode {
+    Hardware hardware;
+    MovementAPI movement;
+    double speed = 0.6;
+    LiftPosition targetLiftPos = null;
+
+    @Override
+    public void init() {
+        hardware = new Hardware(this);
+        movement = new MovementAPI(hardware);
+
+        hardware.setClawPosition(1);
+        hardware.wrist.setPosition(0.5);
+    }
+
+    @Override
+    public void loop() {
+        if (gamepad1.left_bumper) {
+            speed -= 0.005;
+        }
+
+        if (gamepad1.right_bumper) {
+            speed += 0.005;
+        }
+
+        speed = Math.max(0, Math.min(speed, 1));
+
+        if (gamepad2.left_stick_y != 0 || gamepad2.left_stick_button) {
+            targetLiftPos = null;
+        } else if (gamepad2.a) {
+            targetLiftPos = LiftPosition.HIGH;
+        } else if (gamepad2.b) {
+            targetLiftPos = LiftPosition.FLOOR;
+        }
+
+        int spoolPos = hardware.getLiftEncoder();
+
+        double pow;
+        if (targetLiftPos != null) {
+            int spoolPosDiff = targetLiftPos.getEncoderVal() - spoolPos;
+            pow = Poser.ellipticalCurve(spoolPosDiff, Poser.LIFT_STOP_THRESHOLD * Poser.SLOWDOWN_THRESHOLD_FACTOR);
+        } else {
+            pow = -gamepad2.left_stick_y;
+        }
+
+        if (gamepad2.y) {
+            telemetry.addLine("lift status: !!! override set !!!");
+        } else {
+            if (spoolPos < -50) {
+                telemetry.addLine("lift status: too far below min, moving up");
+                pow = Math.max(pow, 0.2);
+            } else if (spoolPos < 0) {
+                telemetry.addLine("lift status: below min, stopping downward motion");
+                pow = Math.max(pow, 0);
+            } else if (spoolPos < 100) {
+                telemetry.addLine("lift status: close to min, slowing downward motion");
+                pow = Math.max(pow, -0.5);
+            } else if (spoolPos > LiftPosition.HIGH_ENCODER_VAL + 50) {
+                telemetry.addLine("lift status: too far above max, moving down");
+                pow = Math.min(pow, -0.2);
+            } else if (spoolPos > LiftPosition.HIGH_ENCODER_VAL) {
+                telemetry.addLine("lift status: above max, stopping upward motion");
+                pow = Math.min(pow, 0);
+            } else if (spoolPos > LiftPosition.HIGH_ENCODER_VAL - 100) {
+                telemetry.addLine("lift status: close to max, slowing upward motion");
+                pow = Math.min(pow, 0.5);
+            } else {
+                telemetry.addLine("lift status: ok");
+            }
+        }
+
+        telemetry.addLine("lift power: " + pow);
+        hardware.spool.setPower(pow);
+
+        hardware.setClawPosition(1 - gamepad2.left_trigger);
+        telemetry.addLine("wrist pos: " + gamepad2.right_stick_y);
+        hardware.setWristPosition(gamepad2.right_stick_y);
+
+        MotorPowers powers = MotorPowers.calculate(
+                gamepad1.left_stick_x,
+                -gamepad1.left_stick_y,
+                gamepad1.right_stick_x,
+                speed
+        );
+
+        telemetry.addLine("fl = " + powers.fl);
+        telemetry.addLine("fr = " + powers.fr);
+        telemetry.addLine("bl = " + powers.bl);
+        telemetry.addLine("br = " + powers.br);
+
+        movement.move(powers);
+
+        telemetry.addLine("speed = " + speed);
+        telemetry.addLine("fl encoder: " + hardware.fl.getCurrentPosition());
+        telemetry.addLine("fr encoder: " + hardware.fr.getCurrentPosition());
+        telemetry.addLine("bl encoder: " + hardware.bl.getCurrentPosition());
+        telemetry.addLine("br encoder: " + hardware.br.getCurrentPosition());
+
+        telemetry.addLine("lift encoder = " + spoolPos);
+        telemetry.addLine("target lift position = " + (targetLiftPos == null ? "none" : targetLiftPos.toString()));
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/MonoGamepad.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/MonoGamepad.java
new file mode 100644
index 0000000..42f17c9
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/MonoGamepad.java
@@ -0,0 +1,114 @@
+package org.firstinspires.ftc.teamcode.teleop;
+
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.Hardware;
+import org.firstinspires.ftc.teamcode.MotorPowers;
+import org.firstinspires.ftc.teamcode.MovementAPI;
+import org.firstinspires.ftc.teamcode.Strings;
+import org.firstinspires.ftc.teamcode.kinematics.LiftPosition;
+import org.firstinspires.ftc.teamcode.kinematics.Poser;
+
+@TeleOp(name = Strings.monoGamepadOpModeName)
+public class MonoGamepad extends OpMode {
+    Hardware hardware;
+    MovementAPI movement;
+    double speed = 0.6;
+    LiftPosition targetLiftPos = null;
+
+    @Override
+    public void init() {
+        hardware = new Hardware(this);
+        movement = new MovementAPI(hardware);
+
+        hardware.setClawPosition(1);
+        hardware.wrist.setPosition(0.5);
+    }
+
+    @Override
+    public void loop() {
+        if (gamepad1.left_bumper) {
+            speed -= 0.005;
+        }
+
+        if (gamepad1.right_bumper) {
+            speed += 0.005;
+        }
+
+        speed = Math.max(0, Math.min(speed, 1));
+
+        if (gamepad1.dpad_up || gamepad1.dpad_down) {
+            targetLiftPos = null;
+        } else if (gamepad1.a) {
+            targetLiftPos = LiftPosition.HIGH;
+        } else if (gamepad1.b) {
+            targetLiftPos = LiftPosition.FLOOR;
+        }
+
+        int spoolPos = hardware.getLiftEncoder();
+
+        double pow;
+        if (targetLiftPos != null) {
+            int spoolPosDiff = targetLiftPos.getEncoderVal() - spoolPos;
+            pow = Poser.ellipticalCurve(spoolPosDiff, Poser.LIFT_STOP_THRESHOLD * Poser.SLOWDOWN_THRESHOLD_FACTOR);
+        } else {
+            pow = (gamepad1.dpad_up ? 1 : 0) + (gamepad1.dpad_down ? -1 : 0);
+        }
+
+        if (gamepad1.y) {
+            telemetry.addLine("lift status: !!! override set !!!");
+        } else {
+            if (spoolPos < -50) {
+                telemetry.addLine("lift status: too far below min, moving up");
+                pow = Math.max(pow, 0.2);
+            } else if (spoolPos < 0) {
+                telemetry.addLine("lift status: below min, stopping downward motion");
+                pow = Math.max(pow, 0);
+            } else if (spoolPos < 100) {
+                telemetry.addLine("lift status: close to min, slowing downward motion");
+                pow = Math.max(pow, -0.5);
+            } else if (spoolPos > LiftPosition.HIGH_ENCODER_VAL + 50) {
+                telemetry.addLine("lift status: too far above max, moving down");
+                pow = Math.min(pow, -0.2);
+            } else if (spoolPos > LiftPosition.HIGH_ENCODER_VAL) {
+                telemetry.addLine("lift status: above max, stopping upward motion");
+                pow = Math.min(pow, 0);
+            } else if (spoolPos > LiftPosition.HIGH_ENCODER_VAL - 100) {
+                telemetry.addLine("lift status: close to max, slowing upward motion");
+                pow = Math.min(pow, 0.5);
+            } else {
+                telemetry.addLine("lift status: ok");
+            }
+        }
+
+        telemetry.addLine("foobarbaz: " + pow);
+        hardware.spool.setPower(pow);
+
+        hardware.setClawPosition(1 - gamepad1.left_trigger);
+        hardware.setWristPosition((gamepad1.dpad_left ? 1 : 0) + (gamepad1.dpad_right ? -1 : 0));
+
+        MotorPowers powers = MotorPowers.calculate(
+                gamepad1.left_stick_x,
+                -gamepad1.left_stick_y,
+                gamepad1.right_stick_x,
+                speed
+        );
+
+        telemetry.addLine("fl = " + powers.fl);
+        telemetry.addLine("fr = " + powers.fr);
+        telemetry.addLine("bl = " + powers.bl);
+        telemetry.addLine("br = " + powers.br);
+
+        movement.move(powers);
+
+        telemetry.addLine("speed = " + speed);
+        telemetry.addLine("fl encoder: " + hardware.fl.getCurrentPosition());
+        telemetry.addLine("fr encoder: " + hardware.fr.getCurrentPosition());
+        telemetry.addLine("bl encoder: " + hardware.bl.getCurrentPosition());
+        telemetry.addLine("br encoder: " + hardware.br.getCurrentPosition());
+
+        telemetry.addLine("lift encoder = " + spoolPos);
+        telemetry.addLine("target lift position = " + (targetLiftPos == null ? "none" : targetLiftPos.toString()));
+    }
+}
diff --git a/build.common.gradle b/build.common.gradle
index 0c19292..6c71b39 100644
--- a/build.common.gradle
+++ b/build.common.gradle
@@ -94,14 +94,14 @@ android {
             signingConfig signingConfigs.release
 
             ndk {
-                abiFilters "armeabi-v7a", "arm64-v8a"
+                abiFilters "armeabi-v7a"
             }
         }
         debug {
             debuggable true
             jniDebuggable true
             ndk {
-                abiFilters "armeabi-v7a", "arm64-v8a"
+                abiFilters "armeabi-v7a"
             }
         }
     }
diff --git a/build.dependencies.gradle b/build.dependencies.gradle
index a108f41..5bade28 100644
--- a/build.dependencies.gradle
+++ b/build.dependencies.gradle
@@ -4,6 +4,7 @@ repositories {
     flatDir {
         dirs rootProject.file('libs')
     }
+    maven { url = 'https://maven.brott.dev/' }
 }
 
 dependencies {
@@ -18,5 +19,6 @@ dependencies {
     implementation 'org.tensorflow:tensorflow-lite-task-vision:0.2.0'
     implementation 'androidx.appcompat:appcompat:1.2.0'
     implementation 'org.firstinspires.ftc:gameAssets-PowerPlay:1.0.0'
+    implementation 'com.acmerobotics.dashboard:dashboard:0.4.6'
 }
 
diff --git a/build.gradle b/build.gradle
index 2ce4e62..e4490fc 100644
--- a/build.gradle
+++ b/build.gradle
@@ -29,4 +29,4 @@ repositories {
     flatDir {
         dirs '../libs'
     }
-}
+}
\ No newline at end of file