Merge remote-tracking branch 'origin/master'

TeamCode/src/main/java/dev/cyberarm/engine/V2/CyberarmEngine.java

@@ -41,6 +41,8 @@ public abstract class CyberarmEngine extends OpMode {
   private GamepadChecker gamepadCheckerGamepad1, gamepadCheckerGamepad2;
   private boolean useThreads = true;
 
+  private long startTime;
+
   /**
    * Called when INIT button on Driver Station is pushed
    * ENSURE to call super.init() if you override this method
@@ -84,6 +86,8 @@ public abstract class CyberarmEngine extends OpMode {
    * ENSURE to call super.start() if you override this method
    */
   public void start() {
+    startTime = System.currentTimeMillis();
+
     if (cyberarmStates.size() > 0) {
       runState(cyberarmStates.get(0));
     }
@@ -534,4 +538,12 @@ public abstract class CyberarmEngine extends OpMode {
   public void threadless() {
     useThreads = false;
   }
+
+  /**
+   * Time in milliseconds since START button was pushed
+   * @return runTime
+   */
+  public double runTime() {
+    return (System.currentTimeMillis() - startTime);
+  }
 }

TeamCode/src/main/java/dev/cyberarm/engine/V2/Vector2D.java

@@ -0,0 +1,108 @@
+package dev.cyberarm.engine.V2;
+
+public class Vector2D {
+    double x, y;
+    public Vector2D() {
+        this.x = 0;
+        this.y = 0;
+    }
+
+    public Vector2D(double x, double y) {
+        this.x = x;
+        this.y = y;
+    }
+
+    public double x() {
+        return x;
+    }
+
+    public double y() {
+        return y;
+    }
+
+    public Vector2D plus(Vector2D other) {
+        return new Vector2D(
+                this.x + other.x(),
+                this.y + other.y()
+        );
+    }
+
+    public Vector2D plus(double other) {
+        return new Vector2D(
+                this.x + other,
+                this.y + other
+        );
+    }
+
+    public Vector2D minus(Vector2D other) {
+        return new Vector2D(
+                this.x - other.x(),
+                this.y - other.y()
+        );
+    }
+
+    public Vector2D minus(double other) {
+        return new Vector2D(
+                this.x - other,
+                this.y - other
+        );
+    }
+
+    public Vector2D multiply(Vector2D other) {
+        return new Vector2D(
+                this.x * other.x(),
+                this.y * other.y()
+        );
+    }
+
+    public Vector2D multiply(double other) {
+        return new Vector2D(
+                this.x * other,
+                this.y * other
+        );
+    }
+
+    public Vector2D divide(Vector2D other) {
+        return new Vector2D(
+                (this.x == 0 ? 0.0 : this.x / other.x()),
+                (this.y == 0 ? 0.0 : this.y / other.y())
+        );
+    }
+
+    public Vector2D divide(double other) {
+        return new Vector2D(
+                (this.x == 0 ? 0.0 : this.x / other),
+                (this.y == 0 ? 0.0 : this.y / other)
+        );
+    }
+
+    public double magnitude() {
+        return Math.sqrt((this.x * this.x) + (this.y * this.y));
+    }
+
+    public Vector2D normalize() {
+        double mag = magnitude();
+
+        return divide(mag);
+    }
+
+    public double distance(Vector2D other) {
+        return Math.sqrt((this.x - other.x) * (this.x - other.x) + (this.y - other.y) * (this.y - other.y));
+    }
+
+    public Vector2D inverse() {
+        return new Vector2D(1.0 / this.x, 1.0 / this.y);
+    }
+
+    public double dot(Vector2D other) {
+        double product = 0.0;
+        product += this.x * other.x;
+        product += this.y * other.y;
+
+        return product;
+    }
+
+    public double sum() {
+        return this.x + this.y;
+    }
+}

TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/AlertManagementSystem.java

@@ -0,0 +1,91 @@
+package dev.cyberarm.minibots.red_crab;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+
+import java.util.Collections;
+import java.util.Comparator;
+import java.util.concurrent.ConcurrentHashMap;
+import java.util.concurrent.CopyOnWriteArrayList;
+
+public class AlertManagementSystem {
+    private CopyOnWriteArrayList<Alert> alerts = new CopyOnWriteArrayList<>();
+
+    public enum Priority {
+        CRITICAL, // Something is on "fire"
+        WARNING, // Something is soon to be on "fire"
+        ALERT, // Something isn't quite right
+        NOTICE // Informational message
+    }
+
+    public  AlertManagementSystem() {
+    }
+
+    public void report(Telemetry telemetry) {
+        if (alerts.size() == 0) {
+            // Pad a few lines so telemetry doesn't jump about (as much)
+            telemetry.addLine();
+            telemetry.addLine();
+            telemetry.addLine();
+            return;
+        }
+
+        telemetry.addLine("------ ALERT MANAGEMENT SYSTEM ------");
+        alerts.stream().sorted(Comparator.comparing(Alert::getPriority)).filter(Alert::valid).forEach(
+                alert -> telemetry.addData("", "    %s: %s", alert.priority.name(), alert.message));
+        telemetry.addLine("-------------------------------------------------------------"); // Gotta love non-monospace fonts, eh?
+
+        alerts.stream().filter(Alert::expired).forEach(alert -> alerts.remove(alert));
+    }
+
+    public void addCritical(String category, String message) {
+        alerts.add(
+                new Alert(Priority.CRITICAL, category, message)
+        );
+    }
+
+    public void addWarning(String category, String message) {
+        alerts.add(
+                new Alert(Priority.WARNING, category, message)
+        );
+    }
+
+    public void addAlert(String category, String message) {
+        alerts.add(
+                new Alert(Priority.ALERT, category, message)
+        );
+    }
+
+    public void addNotice(String category, String message) {
+        alerts.add(
+                new Alert(Priority.NOTICE, category, message)
+        );
+    }
+
+    public class Alert {
+        private final Priority priority;
+        private final String category;
+        private final String message;
+        private final long timeToLive;
+        private final long bornAt;
+        private Alert(Priority priority, String category, String message) {
+            this.priority = priority;
+            this.category = category;
+            this.message = message;
+
+            this.bornAt = System.currentTimeMillis();
+            this.timeToLive = 5_000;
+        }
+
+        private int getPriority() {
+            return priority.ordinal();
+        }
+
+        private boolean expired() {
+            return System.currentTimeMillis() - bornAt >= timeToLive;
+        }
+
+        private boolean valid() {
+            return !expired();
+        }
+    }
+}

TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/Localizer.java

@@ -1,5 +1,9 @@
 package dev.cyberarm.minibots.red_crab;
 
+import com.arcrobotics.ftclib.geometry.Pose2d;
+import com.arcrobotics.ftclib.kinematics.HolonomicOdometry;
+import com.arcrobotics.ftclib.kinematics.Odometry;
+
 import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
 import org.slf4j.helpers.Util;
@@ -8,80 +12,150 @@ import dev.cyberarm.engine.V2.Utilities;
 
 public class Localizer {
     private final RedCrabMinibot robot;
-    private double rawX = 0, rawY = 0, rawR = 0;
-    private double trackWidthMM = 365.0, forwardOffsetMM = -140.0, wheelDiameterMM = 90.0;
-    private int lastEncoderXLeft = 0, lastEncoderXRight = 0, lastEncoderYCenter = 0;
-
+    private double rawX = 0, rawY = 0, rawR = 0, offsetX = 0, offsetY = 0;
+    private double trackWidthMM = 387.35, forwardOffsetMM = 133.35, wheelDiameterMM = 90.0;
+    private final int encoderTicksPerRevolution = 8192;
+    private final double encoderGearRatio = 1;
+    private double lastEncoderXLeftMM, lastEncoderXRightMM, lastEncoderYCenterMM;
+//    private double xDeltaMultiplier = 0.87012987, yDeltaMultiplier = 0.25;
+    private double xPosMultiplier = 0.675956739, yPosMultiplier = 0.675956739;
+    private HolonomicOdometry odometry;
     public Localizer(RedCrabMinibot robot) {
         this.robot = robot;
 
+        loadConfigConstants();
+
         // Preset last encoder to current location to not require resetting encoders, ever. (🤞)
-        this.lastEncoderXLeft = robot.deadWheelXLeft.getCurrentPosition();
-        this.lastEncoderXRight = robot.deadWheelXRight.getCurrentPosition();
-        this.lastEncoderYCenter = robot.deadWheelYCenter.getCurrentPosition();
+        this.lastEncoderXLeftMM = ticksToMM(robot.deadWheelXLeft.getCurrentPosition());
+        this.lastEncoderXRightMM = ticksToMM(robot.deadWheelXRight.getCurrentPosition());
+        this.lastEncoderYCenterMM = ticksToMM(robot.deadWheelYCenter.getCurrentPosition());
+
+        this.odometry = new HolonomicOdometry(
+                this::leftDistance,
+                this::rightDistance,
+                this::centerDistance,
+                trackWidthMM,
+                forwardOffsetMM
+        );
+    }
+
+    public void loadConfigConstants() {
+//        trackWidthMM = 387.35;
+//        forwardOffsetMM = 133.35;
+//        wheelDiameterMM = 90.0;
+//
+//        xPosMultiplier = 0.675956739;
+//        yPosMultiplier = 0.675956739;
+
+        trackWidthMM = robot.config.variable("Robot", "Localizer", "track_width_mm").value();
+        forwardOffsetMM = robot.config.variable("Robot", "Localizer", "forward_offset_mm").value();
+        wheelDiameterMM = robot.config.variable("Robot", "Localizer", "wheel_diameter_mm").value();
+
+        xPosMultiplier = robot.config.variable("Robot", "Localizer", "x_position_multiplier").value();
+        yPosMultiplier = robot.config.variable("Robot", "Localizer", "y_position_multiplier").value();
     }
 
     public void reset() {
+        robot.resetDeadWheels();
+
+        odometry = new HolonomicOdometry(
+                this::leftDistance,
+                this::rightDistance,
+                this::centerDistance,
+                trackWidthMM,
+                forwardOffsetMM
+        );
+
         rawX = 0;
         rawY = 0;
         rawR = 0;
+
+        offsetX = 0;
+        offsetY = 0;
     }
 
-    // FIXME
+    // Meant for setting starting location offset
     public void teleport(double xMM, double yMM) {
-        this.rawX = xMM;
-        this.rawY = yMM;
+        this.offsetX = xMM;
+        this.offsetY = yMM;
     }
 
-    // FIXME
+    // FIXME: We need to be able to set rotation to +/- 180 so we can use absolute field coordinates for target location(s)
+    //          and use odometry position as "true" field location.
     public void teleport(double xMM, double yMM, double headingDegrees) {
-        this.rawX = xMM;
-        this.rawY = yMM;
-        this.rawR = (AngleUnit.DEGREES).toRadians(AngleUnit.normalizeDegrees(headingDegrees)); // cursed :(
+        this.offsetX = xMM;
+        this.offsetY = yMM;
+        this.rawR = 0; // FIXME HERE // (AngleUnit.DEGREES).toRadians(AngleUnit.normalizeDegrees(headingDegrees)); // cursed :(
     }
 
     public void integrate() {
-        int leftEncoder = robot.deadWheelXLeft.getCurrentPosition();
-        int rightEncoder = robot.deadWheelXRight.getCurrentPosition();
-        int centerEncoder = robot.deadWheelYCenter.getCurrentPosition();
+        odometry.updatePose();
 
-        double heading = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
-
-        int deltaLeft = leftEncoder - lastEncoderXLeft;
-        int deltaRight = rightEncoder - lastEncoderXRight;
-        int deltaCenter = centerEncoder - lastEncoderYCenter;
-
-        double phi = (deltaLeft - deltaRight) / trackWidthMM;
-        double deltaMiddle = (deltaLeft + deltaRight) / 2.0;
-        double deltaPerp = deltaCenter - forwardOffsetMM * phi;
-
-        double deltaX = deltaMiddle * Math.cos(heading) - deltaPerp * Math.sin(heading);
-        double deltaY = deltaMiddle * Math.sin(heading) + deltaPerp * Math.cos(heading);
-
-        rawX += deltaX;
-        rawY += deltaY;
-        rawR += phi;
-
-        lastEncoderXLeft = leftEncoder;
-        lastEncoderXRight = rightEncoder;
-        lastEncoderYCenter = centerEncoder;
+//        double leftEncoder = ticksToMM(robot.deadWheelXLeft.getCurrentPosition());
+//        double rightEncoder = ticksToMM(robot.deadWheelXRight.getCurrentPosition());
+//        double centerEncoder = ticksToMM(robot.deadWheelYCenter.getCurrentPosition());
+//
+//        double deltaLeft = leftEncoder - lastEncoderXLeftMM;
+//        double deltaRight = rightEncoder - lastEncoderXRightMM;
+//        double deltaCenter = centerEncoder - lastEncoderYCenterMM;
+//
+//        double phi = (deltaLeft - deltaRight) / trackWidthMM;
+//        double deltaMiddle = (deltaLeft + deltaRight) / 2.0;
+//        //  double deltaPerp = deltaCenter - forwardOffsetMM * phi;
+//        double deltaPerp = deltaCenter - (deltaRight - deltaLeft) * forwardOffsetMM / trackWidthMM;
+//
+//        double heading = rawR + (phi / 2.0);
+//        double deltaX = (deltaMiddle * Math.cos(heading) - deltaPerp * Math.sin(heading)) * xDeltaMultiplier;
+//        double deltaY = (deltaMiddle * Math.sin(heading) + deltaPerp * Math.cos(heading)) * yDeltaMultiplier;
+//
+//        rawX += deltaX;
+//        rawY += deltaY;
+//        rawR += phi;
+//
+//        lastEncoderXLeftMM = leftEncoder;
+//        lastEncoderXRightMM = rightEncoder;
+//        lastEncoderYCenterMM = centerEncoder;
     }
 
     public double xMM() {
-        return Utilities.ticksToUnit(8192, 1, wheelDiameterMM, DistanceUnit.MM, (int)rawX);
+        return odometry.getPose().getX() * xPosMultiplier + offsetX; //rawX;
     }
 
     public double yMM() {
-        return Utilities.ticksToUnit(8192, 1, wheelDiameterMM, DistanceUnit.MM, (int)rawY);
+        return odometry.getPose().getY() * yPosMultiplier + offsetY;  //rawY;
     }
 
-    // FIXME
+    public double xIn() {
+        return xMM() * 0.03937008;
+    }
+    public double yIn() {
+        return yMM() * 0.03937008;
+    }
+
+    // Returns true 360 degrees
     public double headingDegrees() {
-        return rawR;
+        double degrees = headingRadians() * 180.0 / Math.PI;
+        return ((degrees + 360.0) % 360.0) % 360.0;
     }
 
-    // FIXME? (report radians as halves or proper whole?)
+    // Returns annoying half-split +/- PI radians
     public double headingRadians() {
-        return rawR;
+        return odometry.getPose().getHeading(); // rawR;
+    }
+
+    private double ticksToMM(int ticks) {
+        return Utilities.ticksToUnit(encoderTicksPerRevolution, encoderGearRatio, wheelDiameterMM, DistanceUnit.MM, ticks);
+    }
+
+    public double leftDistance() {
+        return ticksToMM(robot.deadWheelXLeft.getCurrentPosition());
+    }
+
+    public double rightDistance() {
+        return ticksToMM(robot.deadWheelXRight.getCurrentPosition());
+    }
+
+    public double centerDistance() {
+        return ticksToMM(robot.deadWheelYCenter.getCurrentPosition());
     }
 }

TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java

@@ -6,7 +6,9 @@ import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.DigitalChannel;
 import com.qualcomm.robotcore.hardware.IMU;
+import com.qualcomm.robotcore.hardware.LED;
 import com.qualcomm.robotcore.hardware.PIDFCoefficients;
 import com.qualcomm.robotcore.hardware.Servo;
 
@@ -20,6 +22,8 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfig
 import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action;
 import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable;
 
+import java.util.ArrayList;
+
 import dev.cyberarm.drivers.EncoderCustomKB2040;
 import dev.cyberarm.engine.V2.CyberarmEngine;
 import dev.cyberarm.engine.V2.Utilities;
@@ -85,12 +89,25 @@ public class RedCrabMinibot {
     public final Servo leftClaw, rightClaw, clawWrist, droneLatch, hookArm;
     public final DcMotorEx deadWheelXLeft, deadWheelXRight;
     public final EncoderCustomKB2040 deadWheelYCenter;
+    public final DigitalChannel ledTopGreen, ledTopRed, ledRail0Green, ledRail0Red, ledRail1Green, ledRail1Red, ledRail2Green, ledRail2Red,
+            ledRail3Green, ledRail3Red;
+    public final ArrayList<DigitalChannel> railGreenLEDs = new ArrayList<>(), railRedLEDs = new ArrayList<>();
+    public int ledChaserIndex = 0;
+    public final long ledChaserIntervalMS;
+    public long lastLedChaserTimeMS;
+    public final boolean LED_OFF = true;
+    public final boolean LED_ON = false;
+    public boolean ledChaseUp = true;
 
     final CyberarmEngine engine;
+    public final boolean autonomous;
 
     public TimeCraftersConfiguration config;
     private final PIDFController clawArmPIDFController;
     public final String webcamName = "Webcam 1";
+    public boolean droneLaunchRequested = false;
+    public double droneLastLaunchRequestStartMS = 0;
+    public boolean droneLaunchAuthorized = false;
 
     private long lastClawArmOverCurrentAnnounced = 0;
     private boolean clawArmOverCurrent = false;
@@ -113,10 +130,15 @@ public class RedCrabMinibot {
     /// Doohickey
     public VisionPortal visionPortal = null;
 
+    /* --- Localizer / Odometry --- */
     public static Localizer localizer;
 
+    /// Alert Management System ///
+    public AlertManagementSystem ams = new AlertManagementSystem();
+
     public RedCrabMinibot(boolean autonomous) {
         engine = CyberarmEngine.instance;
+        this.autonomous = autonomous;
 
         config = new TimeCraftersConfiguration("cyberarm_RedCrab");
         loadConstants();
@@ -238,9 +260,55 @@ public class RedCrabMinibot {
         deadWheelXRight = (DcMotorEx) engine.hardwareMap.dcMotor.get("deadwheel_x_right");
         deadWheelYCenter = engine.hardwareMap.get(EncoderCustomKB2040.class, "deadwheel_y_center");
 
-        deadWheelXLeft.setDirection(DcMotorSimple.Direction.REVERSE);
-        deadWheelXRight.setDirection(DcMotorSimple.Direction.FORWARD);
-//        deadWheelYCenter.setDirection(DcMotorSimple.Direction.FORWARD);
+        if (autonomous)
+            resetDeadWheels();
+
+        /// LED(s) ///
+        ledTopGreen = engine.hardwareMap.get(DigitalChannel.class, "led_top_green");
+        ledTopRed = engine.hardwareMap.get(DigitalChannel.class, "led_top_red");
+        // RED and GREEN are swapped for the 'new' LEDs, and I'm to lazy to fix the robot config
+        ledRail0Green = engine.hardwareMap.get(DigitalChannel.class, "led_rail_0_red");
+        ledRail0Red = engine.hardwareMap.get(DigitalChannel.class, "led_rail_0_green");
+        ledRail1Green = engine.hardwareMap.get(DigitalChannel.class, "led_rail_1_red");
+        ledRail1Red = engine.hardwareMap.get(DigitalChannel.class, "led_rail_1_green");
+        ledRail2Green = engine.hardwareMap.get(DigitalChannel.class, "led_rail_2_red");
+        ledRail2Red = engine.hardwareMap.get(DigitalChannel.class, "led_rail_2_green");
+        ledRail3Green = engine.hardwareMap.get(DigitalChannel.class, "led_rail_3_red");
+        ledRail3Red = engine.hardwareMap.get(DigitalChannel.class, "led_rail_3_green");
+
+        railGreenLEDs.add(ledRail0Green);
+        railGreenLEDs.add(ledRail1Green);
+        railGreenLEDs.add(ledRail2Green);
+        railGreenLEDs.add(ledRail3Green);
+
+        railRedLEDs.add(ledRail0Red);
+        railRedLEDs.add(ledRail1Red);
+        railRedLEDs.add(ledRail2Red);
+        railRedLEDs.add(ledRail3Red);
+
+        ledTopGreen.setMode(DigitalChannel.Mode.OUTPUT);
+        ledTopRed.setMode(DigitalChannel.Mode.OUTPUT);
+        ledRail0Green.setMode(DigitalChannel.Mode.OUTPUT);
+        ledRail0Red.setMode(DigitalChannel.Mode.OUTPUT);
+        ledRail1Green.setMode(DigitalChannel.Mode.OUTPUT);
+        ledRail1Red.setMode(DigitalChannel.Mode.OUTPUT);
+        ledRail2Green.setMode(DigitalChannel.Mode.OUTPUT);
+        ledRail2Red.setMode(DigitalChannel.Mode.OUTPUT);
+        ledRail3Green.setMode(DigitalChannel.Mode.OUTPUT);
+        ledRail3Red.setMode(DigitalChannel.Mode.OUTPUT);
+
+        /// Turn off green LEDs and turn red ones on
+        ledTopGreen.setState(LED_OFF);
+        ledTopRed.setState(LED_ON);
+        for (DigitalChannel led : railGreenLEDs) {
+            led.setState(LED_OFF);
+        }
+        for (DigitalChannel led : railRedLEDs) {
+            led.setState(LED_ON);
+        }
+
+        ledChaserIntervalMS = 250;
+        lastLedChaserTimeMS = System.currentTimeMillis();
 
         // Bulk read from hubs
         Utilities.hubsBulkReadMode(engine.hardwareMap, LynxModule.BulkCachingMode.MANUAL);
@@ -254,6 +322,20 @@ public class RedCrabMinibot {
             RedCrabMinibot.localizer.reset();
     }
 
+    public void shutdown() {
+        // Prevent LED(s) from being on while idle
+        ledTopGreen.setMode(DigitalChannel.Mode.INPUT);
+        ledTopRed.setMode(DigitalChannel.Mode.INPUT);
+        ledRail0Green.setMode(DigitalChannel.Mode.INPUT);
+        ledRail0Red.setMode(DigitalChannel.Mode.INPUT);
+        ledRail1Green.setMode(DigitalChannel.Mode.INPUT);
+        ledRail1Red.setMode(DigitalChannel.Mode.INPUT);
+        ledRail2Green.setMode(DigitalChannel.Mode.INPUT);
+        ledRail2Red.setMode(DigitalChannel.Mode.INPUT);
+        ledRail3Green.setMode(DigitalChannel.Mode.INPUT);
+        ledRail3Red.setMode(DigitalChannel.Mode.INPUT);
+    }
+
     public void reloadConfig() {
         config = new TimeCraftersConfiguration("cyberarm_RedCrab");
 
@@ -307,15 +389,33 @@ public class RedCrabMinibot {
         RedCrabMinibot.DRONE_LATCH_INITIAL_POSITION = config.variable("Robot", "DroneLauncher_Tuning", "initial_position").value();
         RedCrabMinibot.DRONE_LATCH_LAUNCH_POSITION = config.variable("Robot", "DroneLauncher_Tuning", "launch_position").value();
         RedCrabMinibot.DRONE_LAUNCH_CONFIRMATION_TIME_MS = config.variable("Robot", "DroneLauncher_Tuning", "launch_confirmation_time_ms").value();
+
+        if (RedCrabMinibot.localizer != null)
+            RedCrabMinibot.localizer.loadConfigConstants();
+    }
+
+    public void resetDeadWheels() {
+        deadWheelXLeft.setDirection(DcMotorSimple.Direction.REVERSE);
+        deadWheelXRight.setDirection(DcMotorSimple.Direction.FORWARD);
+//        deadWheelYCenter.setDirection(DcMotorSimple.Direction.FORWARD);
+
+        deadWheelXLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        deadWheelXRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        deadWheelYCenter.reset();
+
+        deadWheelXLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        deadWheelXRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
     }
 
     public void standardTelemetry() {
         engine.telemetry.addLine();
 
+        this.ams.report(engine.telemetry);
+
         if (RedCrabMinibot.localizer != null) {
             engine.telemetry.addLine("Localizer");
-            engine.telemetry.addData("X (MM)", "%.2fmm", RedCrabMinibot.localizer.xMM());
-            engine.telemetry.addData("Y (MM)", "%.2fmm", RedCrabMinibot.localizer.yMM());
+            engine.telemetry.addData("X (MM)", "%.2fmm (%.2f IN)", RedCrabMinibot.localizer.xMM(), RedCrabMinibot.localizer.xIn());
+            engine.telemetry.addData("Y (MM)", "%.2fmm (%.2f IN)", RedCrabMinibot.localizer.yMM(), RedCrabMinibot.localizer.yIn());
             engine.telemetry.addData("R (De)", "%.2fdeg", RedCrabMinibot.localizer.headingDegrees());
             engine.telemetry.addLine();
         }
@@ -457,6 +557,7 @@ public class RedCrabMinibot {
             if (milliseconds - lastClawArmOverCurrentAnnounced >= CLAW_ARM_WARN_OVERCURRENT_AFTER_MS) {
                 lastClawArmOverCurrentAnnounced = System.currentTimeMillis();
 
+                this.ams.addWarning("CLAW ARM", String.format("Claw Arm Over Current (%.0f mAmp)", clawArm.getCurrent(CurrentUnit.MILLIAMPS)));
                 engine.telemetry.speak("WARNING. ARM. OVER. CURRENT.");
             }
         } else {
@@ -531,4 +632,149 @@ public class RedCrabMinibot {
 
         return Utilities.isBetween(distanceMM, travelledDistanceMM - toleranceMM, travelledDistanceMM + toleranceMM);
     }
+
+    public void ledController() {
+        if (autonomous) {
+            if (engine.runTime() < 20_000.0) { // KEEP CALM and CARRY ON
+                ledTopRed.setState(LED_OFF);
+                ledTopGreen.setState(LED_ON);
+
+                ledAnimateChaser(LED_OFF, LED_ON, false);
+            } else if (engine.runTime() < 23_000.0) { // RUNNING LOW ON TIME
+                ledTopRed.setState(LED_OFF);
+                ledTopGreen.setState(LED_ON);
+
+                ledAnimateChaser(LED_ON, LED_ON, false);
+            } else { // 5 SECONDS LEFT!
+                if (engine.runTime() < 29_000.0) {
+                    ledTopRed.setState(LED_ON);
+                    ledTopGreen.setState(LED_ON);
+                } else {
+                    ledTopRed.setState(LED_ON);
+                    ledTopGreen.setState(LED_OFF);
+                }
+
+                ledAnimateProgress(LED_ON, LED_OFF, (engine.runTime() - 23_000.0) / 5_000.0);
+            }
+        } else {
+            // Show progress of drone launch authorization
+            if (droneLaunchRequested) {
+                if (droneLaunchAuthorized) {
+                    ledTopRed.setState(LED_OFF);
+                    ledTopGreen.setState(LED_ON);
+
+                    ledSetRailRedLEDs(LED_ON);
+                    ledSetRailRedLEDs(LED_ON);
+                } else {
+                    ledTopRed.setState(LED_ON);
+                    ledTopGreen.setState(LED_ON);
+
+                    ledAnimateProgress(LED_ON, LED_ON, (engine.runTime() - droneLastLaunchRequestStartMS) / RedCrabMinibot.DRONE_LAUNCH_CONFIRMATION_TIME_MS);
+                }
+
+                return;
+            }
+
+            if (engine.runTime() >= 90_000.0) { // LAUNCH DRONE and DO CHIN UP
+                ledTopRed.setState(LED_OFF);
+                ledTopGreen.setState(LED_ON);
+
+                ledSetRailRedLEDs(LED_OFF);
+                ledSetRailGreenLEDs(LED_ON);
+            } else if (engine.runTime() >= 80_000.0) { // GET READY
+                if (ledChaserIndex == railRedLEDs.size() - 1) {
+                    ledTopRed.setState(LED_OFF);
+                    ledTopGreen.setState(LED_OFF);
+                } else {
+                    ledTopRed.setState(LED_ON);
+                    ledTopGreen.setState(LED_ON);
+                }
+
+                ledAnimateChaser(LED_ON, LED_ON, false);
+            } else { // KEEP CALM and CARRY ON
+                ledTopRed.setState(LED_ON);
+                ledTopGreen.setState(LED_OFF);
+
+                ledAnimateChaser(LED_ON, LED_OFF, false);
+            }
+        }
+    }
+
+    public void ledSetRailGreenLEDs(boolean ledState) {
+        for (DigitalChannel led : railGreenLEDs) {
+            led.setState(ledState);
+        }
+    }
+
+    public void ledSetRailRedLEDs(boolean ledState) {
+        for (DigitalChannel led : railRedLEDs) {
+            led.setState(ledState);
+        }
+    }
+
+    public void ledAnimateChaser(boolean ledRed, boolean ledGreen, boolean invert) {
+        if (System.currentTimeMillis() - lastLedChaserTimeMS >= ledChaserIntervalMS) {
+            lastLedChaserTimeMS = System.currentTimeMillis();
+        } else {
+            return;
+        }
+
+        // Turn off current LED
+        if (invert) {
+            railRedLEDs.get(ledChaserIndex).setState(ledRed);
+            railGreenLEDs.get(ledChaserIndex).setState(ledGreen);
+        } else {
+            railRedLEDs.get(ledChaserIndex).setState(LED_OFF);
+            railGreenLEDs.get(ledChaserIndex).setState(LED_OFF);
+        }
+
+        // Cycle up/down
+        if (ledChaseUp) {
+            ledChaserIndex++;
+        } else {
+            ledChaserIndex--;
+        }
+
+        // Switch active LED
+        if (ledChaserIndex >= railRedLEDs.size()) {
+            ledChaseUp = !ledChaseUp;
+            ledChaserIndex = railRedLEDs.size() - 2;
+        } else if (ledChaserIndex < 0) {
+            ledChaseUp = !ledChaseUp;
+            ledChaserIndex = 1;
+        }
+
+        // Control active LED
+        if (!invert) {
+            railRedLEDs.get(ledChaserIndex).setState(ledRed);
+            railGreenLEDs.get(ledChaserIndex).setState(ledGreen);
+        } else {
+            railRedLEDs.get(ledChaserIndex).setState(LED_OFF);
+            railGreenLEDs.get(ledChaserIndex).setState(LED_OFF);
+        }
+    }
+
+    public void ledAnimateProgress(boolean progressLEDsRed, boolean progressLEDsGreen, double ratio) {
+        double i = 0.0;
+        for (DigitalChannel led : railRedLEDs) {
+            i += 1.0 / railRedLEDs.size();
+
+            if (ratio < i) {
+                led.setState(LED_OFF);
+            } else {
+                led.setState(progressLEDsRed);
+            }
+        }
+
+        i = 0.0;
+        for (DigitalChannel led : railGreenLEDs) {
+            i += 1.0 / railGreenLEDs.size();
+
+            if (ratio < i) {
+                led.setState(LED_OFF);
+            } else {
+                led.setState(progressLEDsGreen);
+            }
+        }
+    }
 }

TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/LEDControllerTask.java

@@ -0,0 +1,16 @@
+package dev.cyberarm.minibots.red_crab.engines;
+
+import dev.cyberarm.engine.V2.CyberarmState;
+import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
+
+public class LEDControllerTask extends CyberarmState {
+    private final RedCrabMinibot robot;
+    public LEDControllerTask(RedCrabMinibot robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void exec() {
+        robot.ledController();
+    }
+}

TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueAudienceEngine.java

@@ -4,16 +4,18 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 
 import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
 
-import dev.cyberarm.engine.V2.CyberarmEngine;
 import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
 import dev.cyberarm.minibots.red_crab.states.ClawArmTask;
+import dev.cyberarm.minibots.red_crab.states.LocalizerTask;
 
 @Autonomous(name = "Cyberarm Red Crab BLUE AUDIENCE", group = "MINIBOT", preselectTeleOp = "Cyberarm Red Crab TeleOp")
-public class RedCrabAutonomousBlueAudienceEngine extends RedCrabAutonomousEngine {
+public class RedCrabAutonomousBlueAudienceEngine extends RedCrabEngine {
     @Override
     public void setup() {
         robot = new RedCrabMinibot(true);
         addTask(new ClawArmTask(robot));
+        addTask(new LEDControllerTask(robot));
+        addTask(new LocalizerTask(robot));
 
         setupFromConfig(
                 new TimeCraftersConfiguration("cyberarm_RedCrab"),

TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueBackstageEngine.java

@@ -4,16 +4,18 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 
 import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
 
-import dev.cyberarm.engine.V2.CyberarmEngine;
 import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
 import dev.cyberarm.minibots.red_crab.states.ClawArmTask;
+import dev.cyberarm.minibots.red_crab.states.LocalizerTask;
 
 @Autonomous(name = "Cyberarm Red Crab BLUE BACKSTAGE", group = "MINIBOT", preselectTeleOp = "Cyberarm Red Crab TeleOp")
-public class RedCrabAutonomousBlueBackstageEngine extends RedCrabAutonomousEngine {
+public class RedCrabAutonomousBlueBackstageEngine extends RedCrabEngine {
     @Override
     public void setup() {
         robot = new RedCrabMinibot(true);
         addTask(new ClawArmTask(robot));
+        addTask(new LEDControllerTask(robot));
+        addTask(new LocalizerTask(robot));
 
         setupFromConfig(
                 new TimeCraftersConfiguration("cyberarm_RedCrab"),

TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousDebuggingOnlyEngine.java

@@ -0,0 +1,28 @@
+package dev.cyberarm.minibots.red_crab.engines;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
+
+import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
+import dev.cyberarm.minibots.red_crab.states.ClawArmTask;
+import dev.cyberarm.minibots.red_crab.states.LocalizerTask;
+
+@Autonomous(name = "Cyberarm Red Crab Auto DEBUGGING", group = "MINIBOT", preselectTeleOp = "Cyberarm Red Crab TeleOp")
+public class RedCrabAutonomousDebuggingOnlyEngine extends RedCrabEngine {
+    @Override
+    public void setup() {
+        robot = new RedCrabMinibot(true);
+        addTask(new ClawArmTask(robot));
+        addTask(new LEDControllerTask(robot));
+        addTask(new LocalizerTask(robot));
+
+        setupFromConfig(
+                new TimeCraftersConfiguration("cyberarm_RedCrab"),
+                "dev.cyberarm.minibots.red_crab.states",
+                robot,
+                RedCrabMinibot.class,
+                "Autonomous_Z_DEBUG"
+        );
+    }
+}

TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedAudienceEngine.java

@@ -4,16 +4,18 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 
 import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
 
-import dev.cyberarm.engine.V2.CyberarmEngine;
 import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
 import dev.cyberarm.minibots.red_crab.states.ClawArmTask;
+import dev.cyberarm.minibots.red_crab.states.LocalizerTask;
 
 @Autonomous(name = "Cyberarm Red Crab RED AUDIENCE", group = "MINIBOT", preselectTeleOp = "Cyberarm Red Crab TeleOp")
-public class RedCrabAutonomousRedAudienceEngine extends RedCrabAutonomousEngine {
+public class RedCrabAutonomousRedAudienceEngine extends RedCrabEngine {
     @Override
     public void setup() {
         robot = new RedCrabMinibot(true);
         addTask(new ClawArmTask(robot));
+        addTask(new LEDControllerTask(robot));
+        addTask(new LocalizerTask(robot));
 
         setupFromConfig(
                 new TimeCraftersConfiguration("cyberarm_RedCrab"),

TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedBackstageEngine.java

@@ -4,16 +4,18 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 
 import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
 
-import dev.cyberarm.engine.V2.CyberarmEngine;
 import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
 import dev.cyberarm.minibots.red_crab.states.ClawArmTask;
+import dev.cyberarm.minibots.red_crab.states.LocalizerTask;
 
 @Autonomous(name = "Cyberarm Red Crab RED BACKSTAGE", group = "MINIBOT", preselectTeleOp = "Cyberarm Red Crab TeleOp")
-public class RedCrabAutonomousRedBackstageEngine extends RedCrabAutonomousEngine {
+public class RedCrabAutonomousRedBackstageEngine extends RedCrabEngine {
     @Override
     public void setup() {
         robot = new RedCrabMinibot(true);
         addTask(new ClawArmTask(robot));
+        addTask(new LEDControllerTask(robot));
+        addTask(new LocalizerTask(robot));
 
         setupFromConfig(
                 new TimeCraftersConfiguration("cyberarm_RedCrab"),

TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabEngine.java

@@ -4,19 +4,22 @@ import dev.cyberarm.engine.V2.CyberarmEngine;
 import dev.cyberarm.engine.V2.Utilities;
 import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
 
-public abstract class RedCrabAutonomousEngine extends CyberarmEngine {
+public abstract class RedCrabEngine extends CyberarmEngine {
     protected RedCrabMinibot robot;
 
     @Override
     public void loop() {
         Utilities.hubsClearBulkReadCache(hardwareMap);
-        if (RedCrabMinibot.localizer != null) {
-            RedCrabMinibot.localizer.integrate();
-        }
 
         super.loop();
 
-        if (robot != null)
-            robot.standardTelemetry();
+        robot.standardTelemetry();
+    }
+
+    @Override
+    public void stop() {
+        robot.shutdown();
+
+        super.stop();
     }
 }

TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabTeleOpDebuggingOnlyEngine.java

@@ -8,15 +8,20 @@ import dev.cyberarm.engine.V2.CyberarmEngine;
 import dev.cyberarm.engine.V2.CyberarmState;
 import dev.cyberarm.engine.V2.Utilities;
 import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
+import dev.cyberarm.minibots.red_crab.states.LocalizerTask;
 
 @TeleOp(name = "Cyberarm Red Crab TeleOp DEBUGGING", group = "MINIBOT")
-public class RedCrabTeleOpDebuggingOnlyEngine extends CyberarmEngine {
+public class RedCrabTeleOpDebuggingOnlyEngine extends RedCrabEngine {
     @Override
     public void setup() {
         threadless();
 
+        robot = new RedCrabMinibot(false);
+
+        addTask(new LEDControllerTask(robot));
+        addTask(new LocalizerTask(robot));
+
         addState(new CyberarmState() {
-            final RedCrabMinibot robot = new RedCrabMinibot(false);
 
             @Override
             public void exec() {
@@ -30,18 +35,6 @@ public class RedCrabTeleOpDebuggingOnlyEngine extends CyberarmEngine {
                 robot.clawArm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
                 robot.clawArm.setPower(-engine.gamepad1.left_stick_y * 0.5);
             }
-
-            @Override
-            public void telemetry() {
-                robot.standardTelemetry();
-            }
         });
     }
-
-    @Override
-    public void loop() {
-        Utilities.hubsClearBulkReadCache(hardwareMap);
-
-        super.loop();
-    }
 }

TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabTeleOpEngine.java

@@ -6,26 +6,19 @@ import dev.cyberarm.engine.V2.CyberarmEngine;
 import dev.cyberarm.engine.V2.Utilities;
 import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
 import dev.cyberarm.minibots.red_crab.states.ClawArmTask;
+import dev.cyberarm.minibots.red_crab.states.LocalizerTask;
 import dev.cyberarm.minibots.red_crab.states.Pilot;
 
 @TeleOp(name = "Cyberarm Red Crab TeleOp", group = "MINIBOT")
-public class RedCrabTeleOpEngine extends CyberarmEngine {
+public class RedCrabTeleOpEngine extends RedCrabEngine {
     @Override
     public void setup() {
-        RedCrabMinibot robot = new RedCrabMinibot(false);
+        robot = new RedCrabMinibot(false);
 
         addTask(new ClawArmTask(robot));
+        addTask(new LEDControllerTask(robot));
+        addTask(new LocalizerTask(robot));
 
         addState(new Pilot(robot));
     }
-
-    @Override
-    public void loop() {
-        Utilities.hubsClearBulkReadCache(hardwareMap);
-        if (RedCrabMinibot.localizer != null) {
-            RedCrabMinibot.localizer.integrate();
-        }
-
-        super.loop();
-    }
 }

TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/LocalizerTask.java

@@ -0,0 +1,21 @@
+package dev.cyberarm.minibots.red_crab.states;
+
+import dev.cyberarm.engine.V2.CyberarmState;
+import dev.cyberarm.engine.V2.Utilities;
+import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
+
+public class LocalizerTask extends CyberarmState {
+    private final RedCrabMinibot robot;
+
+    public LocalizerTask(RedCrabMinibot robot) {
+        this.robot = robot;
+    }
+    @Override
+    public void exec() {
+        Utilities.hubsClearBulkReadCache(engine.hardwareMap);
+
+        if (RedCrabMinibot.localizer != null) {
+            RedCrabMinibot.localizer.integrate();
+        }
+    }
+}

TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/MoveToCoordinate.java

@@ -0,0 +1,164 @@
+package dev.cyberarm.minibots.red_crab.states;
+
+import com.qualcomm.robotcore.util.Range;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+
+import dev.cyberarm.engine.V2.CyberarmState;
+import dev.cyberarm.engine.V2.Utilities;
+import dev.cyberarm.engine.V2.Vector2D;
+import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
+
+public class MoveToCoordinate extends CyberarmState {
+    private final RedCrabMinibot robot;
+    private final String groupName, actionName;
+    private final boolean stopDrivetrain;
+    private final int timeoutMS;
+    private final double targetAngleDegrees, targetAngleToleranceDegrees, minDistanceMM;
+    private final double maxVelocityMM, minVelocityMM, lerpMM_UP, lerpMM_DOWN, lerpDegrees;
+    private final Vector2D targetPosMM;
+    private Vector2D robotInitialPosMM = new Vector2D(), robotPosMM = new Vector2D();
+    private Vector2D direction = new Vector2D();
+    private double distanceFromTargetMM = 0;
+    private double velocity = 0;
+    private double angleDiffDegrees = 0;
+    private double rotationStrength = 0;
+
+    public MoveToCoordinate(RedCrabMinibot robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.groupName = groupName;
+        this.actionName = actionName;
+
+        this.timeoutMS = robot.config.variable(groupName, actionName, "timeoutMS").value();
+        this.stopDrivetrain = robot.config.variable(groupName, actionName, "stopDrivetrain").value();
+
+        this.targetAngleToleranceDegrees = robot.config.variable(groupName, actionName, "targetAngleToleranceDEGREES").value();
+        this.targetAngleDegrees = robot.config.variable(groupName, actionName, "targetAngleDEGREES").value();
+
+        this.lerpDegrees = robot.config.variable(groupName, actionName, "lerpDEGREES").value();
+        this.lerpMM_UP = robot.config.variable(groupName, actionName, "lerpMM_UP").value();
+        this.lerpMM_DOWN = robot.config.variable(groupName, actionName, "lerpMM_DOWN").value();
+
+        this.maxVelocityMM = robot.config.variable(groupName, actionName, "maxVelocityMM").value();
+        this.minVelocityMM = robot.config.variable(groupName, actionName, "minVelocityMM").value();
+
+        this.minDistanceMM = robot.config.variable(groupName, actionName, "minDistanceMM").value();
+        String targetPosMM = robot.config.variable(groupName, actionName, "targetPosMM").value();
+        String[] targetPos = targetPosMM.split("x");
+        this.targetPosMM = new Vector2D(Double.parseDouble(targetPos[0]), Double.parseDouble(targetPos[1]));
+    }
+
+    @Override
+    public void start() {
+        this.robotInitialPosMM = new Vector2D(RedCrabMinibot.localizer.xMM(), RedCrabMinibot.localizer.yMM());
+    }
+
+    @Override
+    public void exec() {
+        this.robotPosMM = new Vector2D(RedCrabMinibot.localizer.xMM(), RedCrabMinibot.localizer.yMM());
+        this.direction = targetPosMM.minus(robotPosMM).normalize();
+        this.distanceFromTargetMM = robotPosMM.distance(this.targetPosMM);
+        double newAngleDiffDegrees = Utilities.angleDiff(Utilities.facing(robot.imu), targetAngleDegrees);
+        // FIXME: Test this!
+        // Ignore new angle diff since it appears to be a numeric sign flip and not a useful value (prevent toggling shortest rotation decision.)
+        if (!(Math.abs(newAngleDiffDegrees) - Math.abs(this.angleDiffDegrees) <= 2.0 && Math.abs(newAngleDiffDegrees) >= 178.0))
+            this.angleDiffDegrees = newAngleDiffDegrees;
+
+
+        boolean rotationGood = Utilities.isBetween(
+                angleDiffDegrees,
+                targetAngleDegrees - targetAngleToleranceDegrees,
+                targetAngleDegrees + targetAngleToleranceDegrees);
+
+        if ((distanceFromTargetMM <= minDistanceMM && rotationGood) || runTime() >= timeoutMS) {
+            if (stopDrivetrain) {
+                robot.frontLeft.setVelocity(0);
+                robot.frontRight.setVelocity(0);
+                robot.backLeft.setVelocity(0);
+                robot.backRight.setVelocity(0);
+            }
+
+            finished();
+            return;
+        }
+
+        drivetrain(direction);
+    }
+
+    private void drivetrain(Vector2D direction) {
+        // rotationStrength = Utilities.lerp(
+        //         minVelocityMM,
+        //         maxVelocityMM,
+        //         Range.clip(Math.abs(angleDiffDegrees) / lerpDegrees, 0.0, 1.0));
+        rotationStrength = Range.clip(Math.abs(angleDiffDegrees) / lerpDegrees, 0.0, 1.0);
+        if (angleDiffDegrees < 0)
+            rotationStrength *= -1;
+
+        // https://gm0.org/en/latest/docs/software/tutorials/mecanum-drive.html#field-centric
+
+        double y = direction.x(); // robot forward is in the X axis
+        double x = direction.y(); // robot side to side is on the Y axis
+        double rx = 0; // -rotationStrength; //engine.gamepad1.right_stick_x;
+
+        double botHeading = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
+
+        // Rotate the movement direction counter to the bot's rotation
+        double rotX = x * Math.cos(-botHeading) - y * Math.sin(-botHeading);
+        double rotY = x * Math.sin(-botHeading) + y * Math.cos(-botHeading);
+
+        rotX = rotX * 1.1;  // Counteract imperfect strafing
+
+        // Denominator is the largest motor power (absolute value) or 1
+        // This ensures all the powers maintain the same ratio,
+        // but only if at least one is out of the range [-1, 1]
+        double denominator = Math.max(Math.abs(rotY) + Math.abs(rotX) + Math.abs(rx), 1);
+        double frontLeftPower = (rotY + rotX + rx) / denominator;
+        double backLeftPower = (rotY - rotX + rx) / denominator;
+        double frontRightPower = (rotY - rotX - rx) / denominator;
+        double backRightPower = (rotY + rotX - rx) / denominator;
+
+        velocity = lerpVelocity();
+
+        robot.frontLeft.setVelocity(frontLeftPower * velocity);
+        robot.backLeft.setVelocity(backLeftPower * velocity);
+        robot.frontRight.setVelocity(frontRightPower * velocity);
+        robot.backRight.setVelocity(backRightPower * velocity);
+    }
+
+    private double lerpVelocity() {
+        double distanceFromInitialPosMM = robotInitialPosMM.distance(robotPosMM);
+        double lerpVelocity;
+
+        // Ease power up
+        if (distanceFromInitialPosMM < lerpMM_UP) {
+            lerpVelocity = Utilities.lerp(
+                    minVelocityMM,
+                    maxVelocityMM,
+                    Range.clip(
+                            distanceFromInitialPosMM / lerpMM_UP, 0.0, 1.0));
+            // Cruising power
+        } else if (distanceFromTargetMM > lerpMM_DOWN) {
+            lerpVelocity = maxVelocityMM;
+            // Ease power down
+        } else {
+            lerpVelocity = Utilities.lerp(
+                    minVelocityMM,
+                    maxVelocityMM,
+                    Range.clip(
+                            distanceFromTargetMM / lerpMM_DOWN, 0.0, 1.0));
+        }
+
+        return lerpVelocity;
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("Current Position MM", "X: %.2f Y: %.2f", robotPosMM.x(), robotPosMM.y());
+        engine.telemetry.addData("Target Position MM", "X: %.2f Y: %.2f", targetPosMM.x(), targetPosMM.y());
+        engine.telemetry.addData("Move NORMAL", "X: %.2f Y: %.2f R: %.2f", direction.x(), direction.y(), rotationStrength);
+        engine.telemetry.addData("Distance MM", "%.2fmm", distanceFromTargetMM);
+        engine.telemetry.addData("Angle Diff DEGREES", "%.2f degrees", angleDiffDegrees);
+//        engine.telemetry.addData("Velocity", "%.2f T/s", velocity);
+//        engine.telemetry.addData("LERP DOWN", "%.2f (%.2fmm)", distanceFromTargetMM / lerpMM_DOWN, distanceFromTargetMM);
+    }
+}

TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Pilot.java

@@ -17,9 +17,10 @@ public class Pilot extends CyberarmState {
     private boolean rightClawOpen = false;
     private int clawArmPosition = RedCrabMinibot.ClawArm_INITIAL;
     private boolean hookArmUp = false;
-    private boolean droneLaunchAuthorized = false;
-    private boolean droneLaunchRequested = false;
-    private double droneLastLaunchRequestStartMS = 0;
+    private double odometryResetRequestStartMS = 0;
+    private boolean odometryResetRequested = false;
+    private boolean odometryResetRequestLeftStick = false;
+    private boolean odometryResetRequestRightStick = false;
     private boolean robotSlowMode = false;
     private boolean triggersControlClawArm = false;
     private double deltaTime = 0;
@@ -38,8 +39,9 @@ public class Pilot extends CyberarmState {
         clawArmAndWristController();
         clawController();
         droneLatchController();
-        hookArmController(); // disabled for swrist debug
+        hookArmController();
         winchController();
+        odometryDebugController();
 
         lastLoopTimeMS = runTime();
     }
@@ -49,8 +51,8 @@ public class Pilot extends CyberarmState {
         if (gamepad == engine.gamepad1) {
             switch (button) {
                 case "y":
-                    droneLaunchRequested = true;
-                    droneLastLaunchRequestStartMS = runTime();
+                    robot.droneLaunchRequested = true;
+                    robot.droneLastLaunchRequestStartMS = runTime();
                     break;
                 case "x":
                     hookArmUp = true;
@@ -60,6 +62,8 @@ public class Pilot extends CyberarmState {
                     break;
                 case "guide":
                     robot.imu.resetYaw();
+                    robot.ams.addNotice("IMU", "IMU RESET");
+                    engine.telemetry.speak("IMU. RESET.");
                     break;
                 case "left_bumper":
                     leftClawOpen = !leftClawOpen;
@@ -85,14 +89,22 @@ public class Pilot extends CyberarmState {
 
                     if (triggersControlClawArm)
                     {
-                        engine.telemetry.speak("Claw Arm Manual");
+                        robot.ams.addNotice("CLAW ARM", "Claw Arm Manual");
+                        engine.telemetry.speak("Claw. Arm. Manual.");
                     } else {
-                        engine.telemetry.speak("Winch Manual");
+                        robot.ams.addNotice("WINCH", "Winch Manual");
+                        engine.telemetry.speak("Winch. Manual.");
                     }
                     break;
                 case "start":
                     robot.reloadConfig();
                     break;
+                case "left_stick_button":
+                    odometryResetRequestLeftStick = true;
+                    break;
+                case "right_stick_button":
+                    odometryResetRequestRightStick = true;
+                    break;
             }
         }
     }
@@ -102,18 +114,19 @@ public class Pilot extends CyberarmState {
         if (gamepad == engine.gamepad1) {
             switch (button) {
                 case "y":
-                    droneLaunchRequested = false;
-                    droneLastLaunchRequestStartMS = runTime();
+                    robot.droneLaunchRequested = false;
+                    robot.droneLastLaunchRequestStartMS = runTime();
+                    break;
+                case "left_stick_button":
+                    odometryResetRequestLeftStick = false;
+                    break;
+                case "right_stick_button":
+                    odometryResetRequestRightStick = false;
                     break;
             }
         }
     }
 
-    @Override
-    public void telemetry() {
-        robot.standardTelemetry();
-    }
-
     private void drivetrain() {
         // https://gm0.org/en/latest/docs/software/tutorials/mecanum-drive.html#field-centric
 
@@ -218,12 +231,19 @@ public class Pilot extends CyberarmState {
     }
 
     private void droneLatchController() {
-        if (droneLaunchRequested && runTime() - droneLastLaunchRequestStartMS >= RedCrabMinibot.DRONE_LAUNCH_CONFIRMATION_TIME_MS)
-            droneLaunchAuthorized = true;
+        if (robot.droneLaunchRequested && runTime() - robot.droneLastLaunchRequestStartMS >= RedCrabMinibot.DRONE_LAUNCH_CONFIRMATION_TIME_MS)
+            robot.droneLaunchAuthorized = true;
 
-        if (droneLaunchAuthorized) {
+        if (robot.droneLaunchAuthorized) {
             robot.droneLatch.setPosition(RedCrabMinibot.DRONE_LATCH_LAUNCH_POSITION);
         }
+
+        // Auto reset drone latch after DRONE_LAUNCH_CONFIRMATION_TIME_MS + 1 second
+        if (!robot.droneLaunchRequested && runTime() - robot.droneLastLaunchRequestStartMS >= RedCrabMinibot.DRONE_LAUNCH_CONFIRMATION_TIME_MS + 1_000) {
+            robot.droneLaunchAuthorized = false;
+
+            robot.droneLatch.setPosition(RedCrabMinibot.DRONE_LATCH_INITIAL_POSITION);
+        }
     }
 
     private void hookArmController() {
@@ -249,4 +269,22 @@ public class Pilot extends CyberarmState {
             robot.winch.setPower(RedCrabMinibot.WINCH_MAX_SPEED);
         }
     }
+
+    private void odometryDebugController() {
+        if (odometryResetRequestLeftStick && odometryResetRequestRightStick) {
+            if (!odometryResetRequested) {
+                odometryResetRequested = true;
+                odometryResetRequestStartMS = runTime();
+            } else if (runTime() - odometryResetRequestStartMS >= 1_000) {
+                RedCrabMinibot.localizer.reset();
+                odometryResetRequestStartMS = runTime();
+                odometryResetRequested = false;
+                robot.ams.addNotice("ODOMETRY", "Odometry Reset");
+                engine.telemetry.speak("ODOMETRY. RESET.");
+            }
+        } else {
+            odometryResetRequested = false;
+            odometryResetRequestStartMS = runTime();
+        }
+    }
 }

TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleAudienceBlue.java

@@ -1,6 +1,7 @@
 package org.timecrafters.CenterStage.Autonomous.CompetitionEngines;
 
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.hardware.DcMotor;
 
 import org.timecrafters.CenterStage.Autonomous.CompetitionStates.CameraVisionState;
@@ -15,7 +16,7 @@ import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
 import dev.cyberarm.engine.V2.CyberarmEngine;
 
 @Autonomous(name = "Burnsville audience blue", preselectTeleOp = "Competition V1 TeleOp")
-
+@Disabled
 public class CompetitionBurnsvilleAudienceBlue extends CyberarmEngine {
 
     CompetitionRobotV1 robot;

TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleAudienceRed.java

@@ -1,6 +1,7 @@
 package org.timecrafters.CenterStage.Autonomous.CompetitionEngines;
 
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.hardware.DcMotor;
 
 import org.timecrafters.CenterStage.Autonomous.CompetitionStates.CameraVisionState;
@@ -14,6 +15,7 @@ import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
 import dev.cyberarm.engine.V2.CyberarmEngine;
 
 @Autonomous(name = "Burnsville audience red", preselectTeleOp = "Competition V1 TeleOp")
+@Disabled
 
 public class CompetitionBurnsvilleAudienceRed extends CyberarmEngine {
 

TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleBackDropBlue.java

@@ -1,6 +1,7 @@
 package org.timecrafters.CenterStage.Autonomous.CompetitionEngines;
 
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.hardware.DcMotor;
 
 import org.timecrafters.CenterStage.Autonomous.CompetitionStates.CameraVisionState;
@@ -15,6 +16,7 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfig
 import dev.cyberarm.engine.V2.CyberarmEngine;
 
 @Autonomous(name = "Burnsville BackDrop blue", preselectTeleOp = "Competition V1 TeleOp")
+@Disabled
 
 public class CompetitionBurnsvilleBackDropBlue extends CyberarmEngine {
 
@@ -41,17 +43,17 @@ public class CompetitionBurnsvilleBackDropBlue extends CyberarmEngine {
 //        addTask(new ClawArmControlTask(robot));
 
         this.robot.setup();
-        addState(new ClawArmState(robot,"Burnsville BackDrop blue", "0-01-0"));
+//        addState(new ClawArmState(robot,"Burnsville BackDrop blue", "0-01-0"));
 
-        addState(new CameraVisionState(robot));
+//        addState(new CameraVisionState(robot));
 
-        addState(new ClawArmState(robot,"Burnsville BackDrop blue", "0-01-1"));
+//        addState(new ClawArmState(robot,"Burnsville BackDrop blue", "0-01-1"));
 
         // drive to the left, center, or right spike mark
         addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop blue", "3-02-0"));
-        addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop blue", "3-02-1"));
-        addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop blue", "2-02-0"));
-        addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop blue", "1-02-0"));
+//        addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop blue", "3-02-1"));
+//        addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop blue", "2-02-0"));
+//        addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop blue", "1-02-0"));
 
         addState(new ClawArmState(robot,"Burnsville BackDrop blue", "0-02-4"));
 

TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleBackDropRed.java

@@ -1,6 +1,7 @@
 package org.timecrafters.CenterStage.Autonomous.CompetitionEngines;
 
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.hardware.DcMotor;
 
 import org.timecrafters.CenterStage.Autonomous.CompetitionStates.CameraVisionState;
@@ -14,6 +15,7 @@ import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
 import dev.cyberarm.engine.V2.CyberarmEngine;
 
 @Autonomous(name = "Burnsville BackDrop red", preselectTeleOp = "Competition V1 TeleOp")
+@Disabled
 
 public class CompetitionBurnsvilleBackDropRed extends CyberarmEngine {
 

TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/StateAudienceBlue.java

@@ -0,0 +1,73 @@
+package org.timecrafters.CenterStage.Autonomous.CompetitionEngines;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.CameraVisionState;
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawArmState;
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawFingerState;
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesState;
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesTask;
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.OdometryLocalizerTask;
+import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
+
+import dev.cyberarm.engine.V2.CyberarmEngine;
+
+@Autonomous(name = "State Audience blue", preselectTeleOp = "Competition V1 TeleOp")
+
+public class StateAudienceBlue extends CyberarmEngine {
+
+    CompetitionRobotV1 robot;
+
+
+    @Override
+    public void init() {
+        super.init();
+        robot.clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.clawArm.setTargetPosition(0);
+        robot.clawArm.setPower(0);
+        robot.clawArm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        robot.imu.resetYaw();
+        robot.leftClaw.setPosition(0.25);
+        robot.rightClaw.setPosition(0.6);
+    }
+
+    @Override
+    public void setup() {
+        this.robot = new CompetitionRobotV1("State BackDrop red");
+        addTask(new DriveToCoordinatesTask(robot));
+        addTask(new OdometryLocalizerTask(robot));
+
+        this.robot.setup();
+
+        addState(new CameraVisionState(robot));
+
+        addState(new ClawArmState(robot,"State BackDrop red", "1-00-0"));
+
+        // drive to the left, center, or right spike mark
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "2-01-0"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "2-01-1"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "2-01-2"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "2-02-0"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "2-03-0"));
+
+        // bring arm to hover
+        addState(new ClawArmState(robot,"State BackDrop red", "3-00-0"));
+
+        //open claw
+        addState(new ClawFingerState(robot,"State BackDrop red", "4-00-0"));
+
+        addState(new ClawArmState(robot,"State BackDrop red", "9-00-0"));
+
+
+
+
+
+
+
+
+
+
+    }
+
+}

TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/StateAudienceRed.java

@@ -0,0 +1,70 @@
+package org.timecrafters.CenterStage.Autonomous.CompetitionEngines;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.CameraVisionState;
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawArmState;
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawFingerState;
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesState;
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesTask;
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.OdometryLocalizerTask;
+import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
+
+import dev.cyberarm.engine.V2.CyberarmEngine;
+
+@Autonomous(name = "State Audience red", preselectTeleOp = "Competition V1 TeleOp")
+
+public class StateAudienceRed extends CyberarmEngine {
+
+    CompetitionRobotV1 robot;
+
+
+    @Override
+    public void init() {
+        super.init();
+        robot.clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.clawArm.setTargetPosition(0);
+        robot.clawArm.setPower(0);
+        robot.clawArm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        robot.imu.resetYaw();
+        robot.leftClaw.setPosition(0.25);
+        robot.rightClaw.setPosition(0.6);
+    }
+
+    @Override
+    public void setup() {
+        this.robot = new CompetitionRobotV1("State BackDrop blue");
+        addTask(new DriveToCoordinatesTask(robot));
+        addTask(new OdometryLocalizerTask(robot));
+
+        this.robot.setup();
+
+        addState(new CameraVisionState(robot));
+
+        addState(new ClawArmState(robot,"State BackDrop blue", "1-00-0"));
+
+        // drive to the left, center, or right spike mark
+        addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-03-0"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-03-1"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-03-2"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-02-0"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-01-0"));
+
+        // bring arm to hover
+        addState(new ClawArmState(robot,"State BackDrop blue", "3-00-0"));
+
+        //open claw
+        addState(new ClawFingerState(robot,"State BackDrop blue", "4-00-0"));
+
+        addState(new ClawArmState(robot,"State BackDrop blue", "9-00-0"));
+
+
+
+
+
+
+
+    }
+
+}

TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/StateBackDropBlue.java

@@ -0,0 +1,106 @@
+package org.timecrafters.CenterStage.Autonomous.CompetitionEngines;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.CameraVisionState;
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawArmState;
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawFingerState;
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesState;
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesTask;
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.OdometryLocalizerTask;
+import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
+
+import dev.cyberarm.engine.V2.CyberarmEngine;
+
+@Autonomous(name = "State BackDrop blue", preselectTeleOp = "Competition V1 TeleOp")
+
+public class StateBackDropBlue extends CyberarmEngine {
+
+    CompetitionRobotV1 robot;
+
+
+    @Override
+    public void init() {
+        super.init();
+        robot.clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.clawArm.setTargetPosition(0);
+        robot.clawArm.setPower(0);
+        robot.clawArm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        robot.imu.resetYaw();
+        robot.leftClaw.setPosition(0.25);
+        robot.rightClaw.setPosition(0.6);
+    }
+
+    @Override
+    public void setup() {
+        this.robot = new CompetitionRobotV1("State BackDrop blue");
+        addTask(new DriveToCoordinatesTask(robot));
+        addTask(new OdometryLocalizerTask(robot));
+
+        this.robot.setup();
+
+        addState(new CameraVisionState(robot));
+
+        addState(new ClawArmState(robot,"State BackDrop blue", "1-00-0"));
+
+        // drive to the left, center, or right spike mark
+        addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-03-0"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-03-1"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-03-2"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-02-0"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-01-0"));
+
+        // bring arm to hover
+        addState(new ClawArmState(robot,"State BackDrop blue", "3-00-0"));
+
+        //open claw
+        addState(new ClawFingerState(robot,"State BackDrop blue", "4-00-0"));
+
+        addState(new ClawArmState(robot,"State BackDrop blue", "5-00-0"));
+
+        // drive towards backboard
+        addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "6-01-0"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "6-01-1"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "6-02-0"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "6-02-1"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "6-03-0"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "6-03-1"));
+
+        // pause
+        addState(new ClawArmState(robot,"State BackDrop blue", "6-00-0"));
+
+        // drive into board
+        addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "6-01-2"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "6-02-2"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "6-03-2"));
+
+        // pause
+        addState(new ClawArmState(robot,"State BackDrop blue", "6-00-1"));
+
+        //open right close left
+        addState(new ClawFingerState(robot,"State BackDrop blue", "7-00-0"));
+
+        // bring arm up
+        addState(new ClawArmState(robot,"State BackDrop blue", "8-00-0"));
+
+        // drivw to park
+        addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "9-01-0"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "9-02-0"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "9-03-0"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "9-03-1"));
+
+        addState(new ClawArmState(robot,"State BackDrop blue", "9-00-0"));
+
+
+
+    // 65, on the left, 235 on the right
+
+
+
+
+
+
+    }
+
+}

TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/StateBackDropRed.java

@@ -0,0 +1,106 @@
+package org.timecrafters.CenterStage.Autonomous.CompetitionEngines;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.CameraVisionState;
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawArmState;
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawFingerState;
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesState;
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesTask;
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.OdometryLocalizerTask;
+import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
+
+import dev.cyberarm.engine.V2.CyberarmEngine;
+
+@Autonomous(name = "State BackDrop red", preselectTeleOp = "Competition V1 TeleOp")
+
+public class StateBackDropRed extends CyberarmEngine {
+
+    CompetitionRobotV1 robot;
+
+
+    @Override
+    public void init() {
+        super.init();
+        robot.clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.clawArm.setTargetPosition(0);
+        robot.clawArm.setPower(0);
+        robot.clawArm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        robot.imu.resetYaw();
+        robot.leftClaw.setPosition(0.25);
+        robot.rightClaw.setPosition(0.6);
+    }
+
+    @Override
+    public void setup() {
+        this.robot = new CompetitionRobotV1("State BackDrop red");
+        addTask(new DriveToCoordinatesTask(robot));
+        addTask(new OdometryLocalizerTask(robot));
+
+        this.robot.setup();
+
+        addState(new CameraVisionState(robot));
+
+        addState(new ClawArmState(robot,"State BackDrop red", "1-00-0"));
+
+        // drive to the left, center, or right spike mark
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "2-01-0"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "2-01-1"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "2-01-2"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "2-02-0"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "2-03-0"));
+
+        // bring arm to hover
+        addState(new ClawArmState(robot,"State BackDrop red", "3-00-0"));
+
+        //open claw
+        addState(new ClawFingerState(robot,"State BackDrop red", "4-00-0"));
+
+        addState(new ClawArmState(robot,"State BackDrop red", "5-00-0"));
+
+        // drive towards backboard
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-01-0"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-01-1"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-02-0"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-02-1"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-03-0"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-03-1"));
+
+        // pause
+        addState(new ClawArmState(robot,"State BackDrop red", "6-00-0"));
+
+        // drive into board
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-01-2"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-02-2"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-03-2"));
+
+        // pause
+        addState(new ClawArmState(robot,"State BackDrop red", "6-00-1"));
+
+        //open right close left
+        addState(new ClawFingerState(robot,"State BackDrop red", "7-00-0"));
+
+        // bring arm up
+        addState(new ClawArmState(robot,"State BackDrop red", "8-00-0"));
+
+        // drivw to park
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "9-01-0"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "9-02-0"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "9-03-0"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "9-03-1"));
+
+        addState(new ClawArmState(robot,"State BackDrop red", "9-00-0"));
+
+
+
+
+
+
+
+
+
+
+    }
+
+}

TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/CameraVisionState.java

@@ -54,7 +54,6 @@ public class CameraVisionState extends CyberarmState {
 
     @Override
     public void exec() {
-        robot.clawArmControl();
         if (System.currentTimeMillis() - initTime > 4000){
             robot.objectPos = pipeline.objectPos();
             setHasFinished(true);
@@ -84,6 +83,7 @@ public class CameraVisionState extends CyberarmState {
         Mat leftCrop;
         Mat middleCrop;
         Mat rightCrop;
+        Mat rotatedMat = new Mat();
         double leftavgfin;
         double middleavgfin;
         double rightavgfin;
@@ -91,13 +91,14 @@ public class CameraVisionState extends CyberarmState {
         Scalar rectColor = new Scalar(255.0, 0.0, 0.0);
 
         public Mat processFrame(Mat input) {
-            Imgproc.cvtColor(input, HSV, Imgproc.COLOR_RGB2HSV);
+             Core.rotate(input, rotatedMat,Core.ROTATE_180);
+            Imgproc.cvtColor(rotatedMat, HSV, Imgproc.COLOR_RGB2HSV);
 
             Rect leftRect = new Rect(1, 1, 212, 359);
             Rect rightRect = new Rect(213, 1, 212, 359);
             Rect middleRect = new Rect(426, 1, 212, 359);
 
-            input.copyTo(output);
+            rotatedMat.copyTo(output);
             Imgproc.rectangle(output, leftRect, rectColor, 2);
             Imgproc.rectangle(output, rightRect, rectColor, 2);
             Imgproc.rectangle(output, middleRect, rectColor, 2);

TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java

@@ -15,12 +15,12 @@ public class DriveToCoordinatesState extends CyberarmState {
     public double xTarget;
     public double yTarget;
     public double hTarget;
-    public boolean posAchieved = false;
     public boolean armDrive;
     public int objectPos;
     public boolean posSpecific;
     public double maxXPower;
     public double maxYPower;
+    public double maxHPower;
     private String actionName;
 
     public DriveToCoordinatesState(CompetitionRobotV1 robot, String groupName, String actionName) {
@@ -32,6 +32,7 @@ public class DriveToCoordinatesState extends CyberarmState {
         this.hTarget = robot.configuration.variable(groupName, actionName, "hTarget").value();
         this.maxXPower = robot.configuration.variable(groupName, actionName, "maxXPower").value();
         this.maxYPower = robot.configuration.variable(groupName, actionName, "maxYPower").value();
+        this.maxHPower = robot.configuration.variable(groupName, actionName, "maxHPower").value();
         this.armDrive = robot.configuration.variable(groupName, actionName, "armDrive").value();
         this.objectPos = robot.configuration.variable(groupName, actionName, "objectPos").value();
         this.posSpecific = robot.configuration.variable(groupName, actionName, "posSpecific").value();
@@ -40,23 +41,22 @@ public class DriveToCoordinatesState extends CyberarmState {
     @Override
     public void start() {
         super.start();
-        if (posSpecific) {
+        if (posSpecific && objectPos == robot.objectPos) {
             robot.hTarget = hTarget;
             robot.yTarget = yTarget;
             robot.xTarget = xTarget;
             robot.yMaxPower = maxYPower;
             robot.xMaxPower = maxXPower;
+            robot.hMaxPower = maxHPower;
         } else {
             setHasFinished(true);
         }
 
-        Log.d("TTT?", ""  + actionName + " CURRENT POSITION: x: " + robot.positionX + " Y: " + robot.positionY + "h: " + robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
-        Log.d("TTT?", ""  + actionName + " TARGET POSITION: x: " + robot.xTarget + " Y: " + robot.yTarget + "h: " + robot.hTarget);
-
     }
 
     @Override
     public void exec() {
+
         if (posSpecific) {
             if (objectPos != robot.objectPos) {
                 // enter the ending loop
@@ -68,7 +68,8 @@ public class DriveToCoordinatesState extends CyberarmState {
                 }
 
                 if (Math.abs(robot.positionX - robot.xTarget) < 5
-                        && Math.abs(robot.positionY - robot.yTarget) < 5) {
+                        && Math.abs(robot.positionY - robot.yTarget) < 5
+                        && Math.abs(robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)) - Math.abs(Math.toDegrees(robot.hTarget)) < 5) {
                     setHasFinished(true);
                 }
             }
@@ -78,7 +79,8 @@ public class DriveToCoordinatesState extends CyberarmState {
                 }
 
                 if (Math.abs(robot.positionX - robot.xTarget) < 5
-                        && Math.abs(robot.positionY - robot.yTarget) < 5) {
+                        && Math.abs(robot.positionY - robot.yTarget) < 5
+                        && Math.abs(robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)) - Math.abs(Math.toDegrees(robot.hTarget)) < 5) {
                     setHasFinished(true);
                 }
             }

TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesTask.java

@@ -18,6 +18,7 @@ public class DriveToCoordinatesTask extends CyberarmState {
     public void exec() {
         robot.XDrivePowerModifier();
         robot.YDrivePowerModifier();
+        robot.HDrivePowerModifier();
         robot.DriveToCoordinates();
     }
 }

TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java

@@ -51,10 +51,11 @@ public class CompetitionRobotV1 extends Robot {
 
     // ----------------------------------------------------------------------------------------------------------------- odometry variables:
     public static double Hp = 0.8, Hi = 0, Hd = 0;
-    public static double Xp = -0.03, Xi = 0, Xd = 0;
-    public static double xvp = -0.03, xvi = 0, xvd = 0;
-    public static double Yp = 0.03, Yi = 0, Yd = 0;
-    public static double yvp = 0.03, yvi = 0, yvd = 0;
+    public static double Xp = 0.03, Xi = 0, Xd = 0;
+    public static double Yp = -0.03, Yi = 0, Yd = 0;
+
+    public double Dnl1;
+    public double Dnr2;
     public double xMultiplier = 1;
     public double yMultiplier = 1;
     public double positionX = 1000;
@@ -72,8 +73,8 @@ public class CompetitionRobotV1 extends Robot {
     public int oldRightPosition = 0;
     public int oldLeftPosition = 0;
     public int oldAuxPosition = 0;
-    public final static double L = 22.5; // distance between left and right encoder in cm
-    final static double B = 15; // distance between the midpoint of the left and right encoder from the auxillary encoder in cm
+    public final static double L = 20.9; // distance between left and right encoder in cm
+    final static double B = 12.6; // distance between the midpoint of the left and right encoder from the auxillary encoder in cm
     public final static double R = 3; // wheel radius in cm
     final static double N = 8192; // encoder ticks per revolution (REV encoder)
 
@@ -102,26 +103,28 @@ public class CompetitionRobotV1 extends Robot {
 
     public double yMaxPower = 1;
     public double xMaxPower = 1;
+    public double hMaxPower = 1;
     public double pidX;
     public double pidY;
+    public double pidH;
     public double rawPidX;
     public double rawPidY;
-
+    public double rawPidH;
     public double xVelocity;
     public double yVelocity;
-    public double deltaTime;
+    public double deltaTime = 0;
 
 
     //-------------------------------------------------------------------------------------------------------------- arm sequence variables:
     PIDController pidController;
     public double power;
-    public String armPos;
+    public String armPos = "hover";
     public long armTime;
 
 
     public int target;
     public double p = 0.007, i = 0,  d = 0.0001, f = 0;
-    public double shoulderCollect = 1;
+    public double shoulderCollect = 0.86;
     public double shoulderDeposit = 1;
     public double shoulderPassive = 1;
     public double elbowCollect = 0.02;
@@ -167,7 +170,7 @@ public class CompetitionRobotV1 extends Robot {
         backRight.setDirection(DcMotorSimple.Direction.REVERSE);
         backLeft.setDirection(DcMotorSimple.Direction.REVERSE);
         frontLeft.setDirection(DcMotorSimple.Direction.REVERSE);
-        clawArm.setDirection(DcMotorSimple.Direction.FORWARD);
+        clawArm.setDirection(DcMotorSimple.Direction.REVERSE);
         lift.setDirection(DcMotorSimple.Direction.FORWARD);
         chinUp.setDirection(DcMotorSimple.Direction.FORWARD);
 
@@ -246,14 +249,17 @@ public class CompetitionRobotV1 extends Robot {
         oldLeftPosition = currentLeftPosition;
         oldAuxPosition = currentAuxPosition;
 
-        currentRightPosition = frontLeft.getCurrentPosition();
-        currentLeftPosition = -backRight.getCurrentPosition();
-        currentAuxPosition = backLeft.getCurrentPosition();
+        currentRightPosition = frontRight.getCurrentPosition();
+        currentLeftPosition = backLeft.getCurrentPosition();
+        currentAuxPosition = -frontLeft.getCurrentPosition();
 
         int dnl1 = currentLeftPosition - oldLeftPosition;
         int dnr2 = currentRightPosition - oldRightPosition;
         int dna3 = currentAuxPosition - oldAuxPosition;
 
+        Dnl1 = dnl1;
+        Dnr2 = dnr2;
+
         // the robot has turned and moved a tiny bit between two measurements
         double dtheta = cm_per_tick * (dnr2 - dnl1) / L;
         double dx = cm_per_tick * (dnl1 + dnr2) / 2.0;
@@ -280,7 +286,7 @@ public class CompetitionRobotV1 extends Robot {
      * A Controller taking error of the heading position and converting to a power in the direction of a target.
      * @param reference reference is the target position
      * @param current  current is the measured sensor value.
-     * @return A power to the target the position
+     * @return A power to the target position
      *
      */
     public double HeadingPIDControl(double reference, double current){
@@ -293,27 +299,6 @@ public class CompetitionRobotV1 extends Robot {
         double output = (error * Hp) + (derivative * Hd) + (headingIntegralSum * Hi);
         return output;
     }
-    public double XVeloPIDControl ( double reference, double current){
-        double error = (reference - current);
-        xVeloIntegralSum += error * xVeloTimer.seconds();
-        double derivative = (error - xVeloLastError) / xVeloTimer.seconds();
-
-        xTimer.reset();
-
-        double output = (error * xvp) + (derivative * xvd) + (xIntegralSum * xvi);
-        return output;
-    }
-
-    public double YVeloPIDControl ( double reference, double current){
-        double error = (reference - current);
-        yVeloIntegralSum += error * yVeloTimer.seconds();
-        double derivative = (error - yVeloLastError) / xTimer.seconds();
-
-        xTimer.reset();
-
-        double output = (error * yvp) + (derivative * yvd) + (yVeloIntegralSum * yvi);
-        return output;
-    }
 
     public double XPIDControl ( double reference, double current){
         double error = (reference - current);
@@ -366,17 +351,27 @@ public class CompetitionRobotV1 extends Robot {
             pidX = rawPidX;
         }
     }
+    public void HDrivePowerModifier () {
+
+        rawPidH = HeadingPIDControl(Math.toRadians(hTarget), imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
+        if (Math.abs(rawPidH) > hMaxPower) {
+            if (rawPidH < 0) {
+                pidH = -hMaxPower;
+            } else {
+                pidH = hMaxPower;
+            }
+        } else {
+            pidH = rawPidH;
+        }
+    }
 
     public void DriveToCoordinates () {
         // determine the powers needed for each direction
         // this uses PID to adjust needed Power for robot to move to target
-        XVeloPIDControl(targetVelocityX, xVelocity);
-        XVeloPIDControl(targetVelocityY, yVelocity);
 
         double heading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
-        double rx = HeadingPIDControl(Math.toRadians(hTarget), heading);
 
-        double denominator = Math.max(Math.abs(pidX) + Math.abs(pidY) + Math.abs(rx), 1);
+        double denominator = Math.max(Math.abs(pidX) + Math.abs(pidY) + Math.abs(pidH), 1);
 
         // field oriented math, (rotating the global field to the relative field)
         double rotY = pidY * Math.cos(heading) - pidX * Math.sin(heading);
@@ -384,10 +379,10 @@ public class CompetitionRobotV1 extends Robot {
 
 
         // finding approximate power for each wheel.
-        frontLeftPower = (rotY + rotX + rx) / denominator;
-        backLeftPower = (rotY - rotX + rx) / denominator;
-        frontRightPower = (rotY - rotX - rx) / denominator;
-        backRightPower = (rotY + rotX - rx) / denominator;
+        frontLeftPower = (rotY + rotX + pidH) / denominator;
+        backLeftPower = (rotY - rotX + pidH) / denominator;
+        frontRightPower = (rotY - rotX - pidH) / denominator;
+        backRightPower = (rotY + rotX - pidH) / denominator;
 
         // apply my powers
         frontLeft.setPower(frontLeftPower);
@@ -410,7 +405,7 @@ public class CompetitionRobotV1 extends Robot {
         if (Objects.equals(armPos, "deposit")) {
             shoulder.setPosition(shoulderDeposit);
             elbow.setPosition(elbowDeposit);
-            target = 400;
+            target = 300;
 
 
         }
@@ -424,19 +419,13 @@ public class CompetitionRobotV1 extends Robot {
 
         }
         if (armPos.equals("search")) {
-            shoulder.setPosition(0.48);
-            if (armTime > 400){
+            shoulder.setPosition(0.55);
+            if (armTime > 450){
                 target = 570;
             }
 
         }
 
-//        pidController.setPID(p, i, d);
-//        int armPos = clawArm.getCurrentPosition();
-//        double pid = pidController.calculate(armPos, target);
-//
-//        power = pid;
-
         clawArm.setTargetPosition(target);
         clawArm.setPower(0.4);
 

TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java

@@ -69,6 +69,11 @@ public class SodiPizzaMinibotObject extends Robot {
         leftBack.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
         rightBack.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
 
+        leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+
         //Servo Defining
         shoulder = engine.hardwareMap.servo.get("shoulder");
         gripper = engine.hardwareMap.servo.get("gripper");

TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/CompetitionTeleOpState.java

@@ -24,15 +24,18 @@ public class CompetitionTeleOpState extends CyberarmState {
     public int target = 0;
 
     // ---------------------------------------------------------------------------------------------------------------------shoot variables:
-    public static double releasePos = 0.95;
-    public static double holdPos = 0.55                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                    ;
+    public static double releasePos = 0.45;
+    public static double holdPos = 0.95                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                    ;
 
 
+    public double maxVelocityX;
+    public double maxVelocityY;
+
     // ------------------------------------------------------------------------------------------------------------- Heading lock variables:
     public double integralSum = 0;
     private double targetHeading;
-    public double collectLock = Math.toRadians(90);
-    public double backDropLock = Math.toRadians(-90);
+    public double collectLock = Math.toRadians(-90);
+    public double backDropLock = Math.toRadians(90);
     public double boost;
     public double armPower;
     private double currentHeading;
@@ -68,8 +71,9 @@ public class CompetitionTeleOpState extends CyberarmState {
     // ---------------------------------------------------------------------------------------------------------------Arm control Variables:
     public String armPos = "collect";
     // chin up servo
-    public static double chinUpServoUp = 0.58;
-    public static double chinUpServoDown = 1;
+    public static double chinUpServoUp = 0.55;
+    public static double chinUpServoDown = 0;
+    public long lastExecutedTime;
 
 
 
@@ -109,7 +113,6 @@ public class CompetitionTeleOpState extends CyberarmState {
         }
     }
 
-
     public void DriveTrainTeleOp () {
 
         boolean lbs1 = engine.gamepad1.left_stick_button;
@@ -201,30 +204,68 @@ public class CompetitionTeleOpState extends CyberarmState {
     }
 
     public void ArmPosControl() {
-
-
-        if (engine.gamepad2.a) {
+        if (engine.gamepad2.a && !engine.gamepad2.back) {
             armPos = "collect";
             depositMode = false;
-        } else if (engine.gamepad2.y) {
+        } else if (engine.gamepad2.y && !engine.gamepad2.back) {
             armPos = "deposit";
             depositMode = true;
-        } else if (engine.gamepad2.b) {
+        } else if (engine.gamepad2.b && !engine.gamepad2.start) {
             armPos = "hover";
             depositMode = true;
-        } else if (engine.gamepad2.x) {
+        } else if (engine.gamepad2.x && !engine.gamepad2.start) {
             armPos = "passive";
             depositMode = true;
-        } else if (engine.gamepad2.dpad_left) {
+        } else if (engine.gamepad2.dpad_left && !engine.gamepad2.start) {
             armPos = "lift up";
             depositMode = true;
-        } else if (engine.gamepad2.dpad_right) {
+        } else if (engine.gamepad2.dpad_right && !engine.gamepad2.start) {
             armPos = "lift down";
             depositMode = false;
         } else if (engine.gamepad2.back) {
             armPos = "reset";
+        } else if (engine.gamepad2.right_bumper) {
+            armPos = "tuning up";
+        } else if (engine.gamepad2.left_bumper) {
+            armPos = "tuning down";
         }
 
+        if (Objects.equals(armPos, "tuning up")) {
+            if (robot.lift.getCurrentPosition() >= 20) {
+                robot.chinUpServo.setPosition(chinUpServoDown);
+                robot.lift.setPower(-0.6);
+            } else {
+                robot.lift.setPower(0);
+                if (System.currentTimeMillis() - lastExecutedTime > 200){
+                    robot.shoulderCollect = robot.shoulderCollect + 0.02;
+                    lastExecutedTime = System.currentTimeMillis();
+                }
+                robot.shoulder.setPosition(robot.shoulderCollect);
+                robot.elbow.setPosition(robot.elbowCollect);
+                robot.chinUpServo.setPosition(chinUpServoDown);
+                target = 0;
+                armPos = "collect";
+
+            }
+        }
+        if (Objects.equals(armPos, "tuning down")) {
+            if (robot.lift.getCurrentPosition() >= 20) {
+                robot.chinUpServo.setPosition(chinUpServoDown);
+                robot.lift.setPower(-0.6);
+            } else {
+                robot.lift.setPower(0);
+                if (System.currentTimeMillis() - lastExecutedTime > 200) {
+                    robot.shoulderCollect = robot.shoulderCollect - 0.02;
+                    lastExecutedTime = System.currentTimeMillis();
+                }
+                robot.shoulder.setPosition(robot.shoulderCollect);
+                robot.elbow.setPosition(robot.elbowCollect);
+                robot.chinUpServo.setPosition(chinUpServoDown);
+                target = 0;
+                armPos = "collect";
+
+            }
+        }
         if (Objects.equals(armPos, "collect")) {
             if (robot.lift.getCurrentPosition() >= 20) {
                 robot.chinUpServo.setPosition(chinUpServoDown);
@@ -258,9 +299,8 @@ public class CompetitionTeleOpState extends CyberarmState {
         }
 
         if (Objects.equals(armPos, "lift up")) {
-            robot.shoulder.setPosition(robot.shoulderCollect);
-            robot.elbow.setPosition(robot.elbowDeposit);
-            target = 120;
+            robot.shoulder.setPosition(0.2);
+            target = 850;
             robot.chinUpServo.setPosition(chinUpServoUp);
         }
 
@@ -270,9 +310,8 @@ public class CompetitionTeleOpState extends CyberarmState {
                 robot.chinUpServo.setPosition(chinUpServoDown);
             } else {
                 robot.lift.setPower(0);
-                robot.chinUpServo.setPosition(chinUpServoDown);
-                robot.shoulder.setPosition(robot.shoulderCollect);
-                target = 120;
+                robot.shoulder.setPosition(0.2);
+                target = 850;
             }
         }
 
@@ -296,11 +335,20 @@ public class CompetitionTeleOpState extends CyberarmState {
         robot.clawArm.setTargetPosition(0);
         robot.clawArm.setPower(0);
         robot.clawArm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        lastExecutedTime = System.currentTimeMillis();
+
 
     }
 
     @Override
-    public void exec() {
+    public void exec() { //------------------------------------------------------------------------------------------------------ EXEC Start
+        if (robot.xVelocity > maxVelocityX){
+            maxVelocityX = robot.xVelocity;
+        }
+        if (robot.yVelocity > maxVelocityY){
+            maxVelocityY = robot.yVelocity;
+        }
+        robot.OdometryLocalizer();
 
         if (engine.gamepad2.start && engine.gamepad2.x){
             robot.shootServo.setPosition(releasePos);
@@ -368,10 +416,23 @@ public class CompetitionTeleOpState extends CyberarmState {
 
         ClawControlTeleOp();
 
+
+        robot.velocityChecker();
+
     }
 
         @Override
         public void telemetry () {
+            engine.telemetry.addData("x velocity max", maxVelocityX);
+            engine.telemetry.addData("y velocity max", maxVelocityY);
+            engine.telemetry.addData("Dnl1", robot.Dnl1);
+            engine.telemetry.addData("Dnr2", robot.Dnr2);
+            engine.telemetry.addData("x pos", robot.positionX);
+            engine.telemetry.addData("y pos", robot.positionY);
+            engine.telemetry.addData("h pos odo", Math.toDegrees(robot.positionH));
+            engine.telemetry.addData("aux encoder", robot.currentAuxPosition);
+            engine.telemetry.addData("left encoder", robot.currentLeftPosition);
+            engine.telemetry.addData("right encoder", robot.currentRightPosition);
             engine.telemetry.addData("Lift Encoder Pos", robot.lift.getCurrentPosition());
             engine.telemetry.addData("imu", -robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
             engine.telemetry.addData("imu", -robot.imu.getRobotYawPitchRollAngles().getPitch(AngleUnit.DEGREES));

TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/SodiPizzaTeleOPState.java

@@ -27,8 +27,6 @@ public class SodiPizzaTeleOPState extends CyberarmState {
     public final double minInput = 0.1 /* <- Minimum input from stick to send command */;
     public double lastToldAngle /* <- The angle the bot was last told to stop at */, proportion, integral, derivative;
     public double approxObjPos /* <- Approximate distance away the nearest obstruction is */, objData1, objData2, objData3;
-    private double lfPower, rfPower, lbPower, rbPower;
-    private float yTransitPercent, xTransitPercent, rotPercent, percentDenom;
     private int objectPos, armPos;
     private boolean droneLaunched;
     private char buttonPressed;
@@ -65,11 +63,15 @@ public class SodiPizzaTeleOPState extends CyberarmState {
         engine.telemetry.addData("Arm servo position", robot.shoulder.getPosition());
         engine.telemetry.addLine();
         engine.telemetry.addData("Approx Object Dist", approxObjPos);
+        engine.telemetry.addLine();
+        engine.telemetry.addData("Yaw", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
+        engine.telemetry.addData("Pitch", robot.imu.getRobotYawPitchRollAngles().getPitch(AngleUnit.DEGREES));
+        engine.telemetry.addData("Roll", robot.imu.getRobotYawPitchRollAngles().getRoll(AngleUnit.DEGREES));
     }
 
     @Override
     public void init() {
-        drivePower = 0;
+        drivePower = 1;
         robot.leftFront.setPower(0);
         robot.rightFront.setPower(0);
         robot.leftBack.setPower(0);
@@ -80,7 +82,7 @@ public class SodiPizzaTeleOPState extends CyberarmState {
 
         robot.imu.resetYaw();
         imuInitAngle = robot.imu.getRobotYawPitchRollAngles();
-        lastToldAngle = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
+//        lastToldAngle = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
 
         gp1checker = new GamepadChecker(engine, engine.gamepad1);
         gp2checker = new GamepadChecker(engine, engine.gamepad2);
@@ -98,33 +100,56 @@ public class SodiPizzaTeleOPState extends CyberarmState {
         if(System.currentTimeMillis() - startingTime <= 2000) {
             getApproxObjPos();
         }
-/*Driving is almost all normal hopefully.*/
 
-        double y = engine.gamepad1.left_stick_y; // Remember, Y stick value is reversed
+        double y = engine.gamepad1.left_stick_y;
         double x = engine.gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing
-        double rx = engine.gamepad1.right_stick_x;
+        double rx = -engine.gamepad1.right_stick_x;
 
 // Denominator is the largest motor power (absolute value) or 1
 // This ensures all the powers maintain the same ratio,
 // but only if at least one is out of the range [-1, 1]
+//        double frontLeftPower = (y + x + rx) / denominator;
+//        double backLeftPower = (y - x + rx) / denominator;
+//        double frontRightPower = (y - x - rx) / denominator;
+//        double backRightPower = (y + x - rx) / denominator;
+//
+//        robot.leftFront.setPower(frontLeftPower);
+//        robot.leftBack.setPower(backLeftPower);
+//        robot.rightFront.setPower(frontRightPower);
+//        robot.rightBack.setPower(backRightPower);
+
         double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
-        double frontLeftPower = (y + x + rx) / denominator;
-        double backLeftPower = (y - x + rx) / denominator;
-        double frontRightPower = (y - x - rx) / denominator;
-        double backRightPower = (y + x - rx) / denominator;
 
-        robot.leftFront.setPower(frontLeftPower);
-        robot.leftBack.setPower(backLeftPower);
-        robot.rightFront.setPower(frontRightPower);
-        robot.rightBack.setPower(backRightPower);
+        double heading = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
+        double rotX = x * Math.cos(heading) - y * Math.sin(heading);
+        double rotY = x * Math.sin(heading) + y * Math.cos(heading);
 
+        double frontLeftPower = (rotY + rotX + rx) / denominator;
+        double backLeftPower = (rotY - rotX + rx) / denominator;
+        double frontRightPower = (rotY - rotX - rx) / denominator;
+        double backRightPower = (rotY + rotX - rx) / denominator;
 
-        if (engine.gamepad2.left_stick_button) {
+        robot.leftBack.setPower(backLeftPower * drivePower);
+        robot.rightBack.setPower(backRightPower * drivePower);
+        robot.leftFront.setPower(frontLeftPower * drivePower);
+        robot.rightFront.setPower(frontRightPower * drivePower);
+
+        if (engine.gamepad1.start && !engine.gamepad1.a) {
+            robot.imu.resetYaw();
+        }
+
+        if (Math.abs(robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)) > 0.000001 &&
+            Math.abs(robot.imu.getRobotYawPitchRollAngles().getPitch(AngleUnit.DEGREES)) > 0.000001 &&
+            Math.abs(robot.imu.getRobotYawPitchRollAngles().getRoll(AngleUnit.DEGREES)) > 0.000001) {
+                robot.imu.resetYaw();
+        }
+
+        if (engine.gamepad1.left_stick_button) {
             if (System.currentTimeMillis() - lastMoveTime >= 200) {
                 robot.launcher.setPosition(0.98);
                 lastMoveTime = System.currentTimeMillis();
             }
-        } else if (engine.gamepad2.right_stick_button) {
+        } else if (engine.gamepad1.right_stick_button) {
             if (System.currentTimeMillis() - lastMoveTime >= 100) {
                 robot.launcher.setPosition(robot.launcher.getPosition() - 0.2);
                 lastMoveTime = System.currentTimeMillis();
@@ -136,27 +161,27 @@ public class SodiPizzaTeleOPState extends CyberarmState {
             }
         }
 
-        if (!engine.gamepad2.left_stick_button && droneLaunched) {
+        if (!engine.gamepad1.left_stick_button && droneLaunched) {
             if (System.currentTimeMillis() - lastMoveTime >= 200) {
                 robot.launcher.setPosition(robot.launcher.getPosition() - 0.025);
                 lastMoveTime = System.currentTimeMillis();
             }
         }
 
-        if (engine.gamepad2.left_stick_y > 0.1) {
-            if (System.currentTimeMillis() - lastMoveTime >= 200) {
+        if (engine.gamepad1.dpad_up) {
+            if (System.currentTimeMillis() - lastMoveTime >= 150) {
                 robot.shoulder.setPosition(robot.shoulder.getPosition() + 0.05);
                 lastMoveTime = System.currentTimeMillis();
             }
-        } else if (engine.gamepad2.left_stick_y < -0.1) {
-            if (System.currentTimeMillis() - lastMoveTime >= 200) {
+        } else if (engine.gamepad1.dpad_down) {
+            if (System.currentTimeMillis() - lastMoveTime >= 150) {
                 robot.shoulder.setPosition(robot.shoulder.getPosition() - 0.05);
                 lastMoveTime = System.currentTimeMillis();
             }
         }
 
         // This moves the arm to Collect position, which is at servo position 0.00.
-        if (engine.gamepad2.a && !engine.gamepad2.start) {
+        if (engine.gamepad1.a && !engine.gamepad1.start) {
             armPos = 1;
         }
 
@@ -187,7 +212,7 @@ public class SodiPizzaTeleOPState extends CyberarmState {
         //End of code for armPos = 1
 
         // This moves the arm to Precollect position, which is at servo position 0.05.
-        if (engine.gamepad2.x) {
+        if (engine.gamepad1.x) {
             armPos = 2;
         }
 
@@ -218,7 +243,7 @@ public class SodiPizzaTeleOPState extends CyberarmState {
         //End of code for armPos = 2
 
         // This moves the arm to Deliver position, which is at servo position 0.28.
-        if (engine.gamepad2.b && !engine.gamepad2.start) {
+        if (engine.gamepad1.b && !engine.gamepad1.start) {
             armPos = 3;
         }
 
@@ -253,7 +278,7 @@ public class SodiPizzaTeleOPState extends CyberarmState {
         //End of code for armPos = 3
 
         // This moves the arm to Stow position, which is at servo position 0.72.
-         if (engine.gamepad2.y) {
+         if (engine.gamepad1.y) {
             armPos = 4;
         }
 
@@ -284,9 +309,9 @@ public class SodiPizzaTeleOPState extends CyberarmState {
         }
 //        End of code for armPos = 4
 
-        if (engine.gamepad2.dpad_left) {
+        if (engine.gamepad1.dpad_left) {
             robot.gripper.setPosition(GRIPPER_OPEN);
-        } else if (engine.gamepad2.dpad_right) {
+        } else if (engine.gamepad1.dpad_right) {
             robot.gripper.setPosition(GRIPPER_CLOSED);
         }