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/minibots/red_crab/Localizer.java

@@ -13,16 +13,18 @@ import dev.cyberarm.engine.V2.Utilities;
 public class Localizer {
     private final RedCrabMinibot robot;
     private double rawX = 0, rawY = 0, rawR = 0, offsetX = 0, offsetY = 0;
-    private final double trackWidthMM = 365.0, forwardOffsetMM = 140.0, wheelDiameterMM = 90.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 xDeltaMultiplier = 1, yDeltaMultiplier = 1;
+    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.lastEncoderXLeftMM = ticksToMM(robot.deadWheelXLeft.getCurrentPosition());
         this.lastEncoderXRightMM = ticksToMM(robot.deadWheelXRight.getCurrentPosition());
@@ -37,6 +39,22 @@ public class Localizer {
         );
     }
 
+    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();
 
@@ -100,11 +118,11 @@ public class Localizer {
     }
 
     public double xMM() {
-        return odometry.getPose().getX() + offsetX; //rawX;
+        return odometry.getPose().getX() * xPosMultiplier + offsetX; //rawX;
     }
 
     public double yMM() {
-        return odometry.getPose().getY() + offsetY;  //rawY;
+        return odometry.getPose().getY() * yPosMultiplier + offsetY;  //rawY;
     }
 
     public double xIn() {

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

@@ -22,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;
@@ -87,13 +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 greenLED, redLED;
+    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;
@@ -124,6 +138,7 @@ public class RedCrabMinibot {
 
     public RedCrabMinibot(boolean autonomous) {
         engine = CyberarmEngine.instance;
+        this.autonomous = autonomous;
 
         config = new TimeCraftersConfiguration("cyberarm_RedCrab");
         loadConstants();
@@ -249,13 +264,51 @@ public class RedCrabMinibot {
             resetDeadWheels();
 
         /// LED(s) ///
-        greenLED = engine.hardwareMap.get(DigitalChannel.class, "led_green"); // GREEN LED NOT WORKING
-        redLED = engine.hardwareMap.get(DigitalChannel.class, "led_red");
+        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");
 
-        greenLED.setMode(DigitalChannel.Mode.OUTPUT);
-        redLED.setMode(DigitalChannel.Mode.OUTPUT);
-        greenLED.setState(true);
-        redLED.setState(true);
+        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);
@@ -269,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");
 
@@ -322,6 +389,9 @@ 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() {
@@ -562,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

@@ -6,13 +6,16 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfig
 
 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 RedCrabAutonomousEngine {
+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"),

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

@@ -22,6 +22,7 @@ public class MoveToCoordinate extends CyberarmState {
     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;
@@ -85,10 +86,11 @@ public class MoveToCoordinate extends CyberarmState {
     }
 
     private void drivetrain(Vector2D direction) {
-        double rotationStrength = Utilities.lerp(
-                minVelocityMM,
-                maxVelocityMM,
-                Range.clip(Math.abs(angleDiffDegrees) / lerpDegrees, 0.0, 1.0));
+        // 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;
 
@@ -96,7 +98,7 @@ public class MoveToCoordinate extends CyberarmState {
 
         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 = rotationStrength; //engine.gamepad1.right_stick_x;
+        double rx = 0; // -rotationStrength; //engine.gamepad1.right_stick_x;
 
         double botHeading = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
 
@@ -153,7 +155,7 @@ public class MoveToCoordinate extends CyberarmState {
     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", direction.x(), direction.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);

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

@@ -17,9 +17,6 @@ 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;
@@ -42,7 +39,7 @@ public class Pilot extends CyberarmState {
         clawArmAndWristController();
         clawController();
         droneLatchController();
-        hookArmController(); // disabled for swrist debug
+        hookArmController();
         winchController();
         odometryDebugController();
 
@@ -54,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;
@@ -117,8 +114,8 @@ 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;
@@ -130,11 +127,6 @@ public class Pilot extends CyberarmState {
         }
     }
 
-    @Override
-    public void telemetry() {
-        robot.standardTelemetry();
-    }
-
     private void drivetrain() {
         // https://gm0.org/en/latest/docs/software/tutorials/mecanum-drive.html#field-centric
 
@@ -239,16 +231,16 @@ 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 (!droneLaunchRequested && runTime() - droneLastLaunchRequestStartMS >= RedCrabMinibot.DRONE_LAUNCH_CONFIRMATION_TIME_MS + 1_000) {
-            droneLaunchAuthorized = false;
+        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);
         }

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);
@@ -106,24 +108,48 @@ public class SodiPizzaTeleOPState extends CyberarmState {
 // 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();
@@ -135,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;
         }
 
@@ -186,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;
         }
 
@@ -217,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;
         }
 
@@ -252,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;
         }
 
@@ -283,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);
         }