Merge remote-tracking branch 'origin/master'

TeamCode/src/main/java/dev/cyberarm/drivers/EncoderCustomKB2040.java

@@ -0,0 +1,67 @@
+package dev.cyberarm.drivers;
+
+import com.qualcomm.robotcore.hardware.I2cAddr;
+import com.qualcomm.robotcore.hardware.I2cDeviceSynchDevice;
+import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple;
+import com.qualcomm.robotcore.hardware.I2cWaitControl;
+import com.qualcomm.robotcore.hardware.configuration.annotations.DeviceProperties;
+import com.qualcomm.robotcore.hardware.configuration.annotations.I2cDeviceType;
+
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+
+@I2cDeviceType()
+@DeviceProperties(name = "Encoder Custom KB2040", description = "Non-competition legal i2c encoder", xmlTag = "ENCODER_CUSTOM_KB2040")
+public class EncoderCustomKB2040 extends I2cDeviceSynchDevice<I2cDeviceSynchSimple> {
+    enum Register {
+        REPORT_POSITION,
+        RESET_POSITION
+    }
+
+    private final static I2cAddr ADDRESS_I2C_DEFAULT = I2cAddr.create7bit(0x16);
+    private int position = -1;
+    public EncoderCustomKB2040(I2cDeviceSynchSimple i2cDeviceSynchSimple, boolean deviceClientIsOwned) {
+        super(i2cDeviceSynchSimple, deviceClientIsOwned);
+
+        this.deviceClient.setI2cAddress(ADDRESS_I2C_DEFAULT);
+        super.registerArmingStateCallback(false);
+        engage();
+    }
+
+    @Override
+    protected synchronized boolean doInitialize() {
+        return true;
+    }
+
+    @Override
+    public Manufacturer getManufacturer() {
+        return Manufacturer.Other;
+    }
+
+    @Override
+    public String getDeviceName() {
+        return "Encoder Custom KB2040";
+    }
+
+    public int getCurrentPosition() {
+//        deviceClient.write8(Register.READ.ordinal());
+
+        // Read int32_t
+        byte[] buffer = deviceClient.read(Register.REPORT_POSITION.ordinal(), 4);
+
+        // Reconstruct int32_t from 4 int8_t
+        int newPos = ByteBuffer.wrap(buffer).order(ByteOrder.BIG_ENDIAN).getInt();
+
+        position = newPos;
+
+        return position;
+    }
+
+    public int getLastPosition() {
+        return position;
+    }
+
+    public void reset() {
+        deviceClient.write8(Register.RESET_POSITION.ordinal(), I2cWaitControl.WRITTEN);
+    }
+}

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));
     }
@@ -514,11 +518,15 @@ public abstract class CyberarmEngine extends OpMode {
 
           RuntimeException exception = new RuntimeException(cause.getMessage(), cause.getCause());
           exception.setStackTrace(cause.getStackTrace());
+
+          throw(exception);
         } else {
           e.printStackTrace();
 
           RuntimeException exception = new RuntimeException(e.getMessage(), e.getCause());
           exception.setStackTrace(e.getStackTrace());
+
+          throw(exception);
         }
       }
     }
@@ -530,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/CyberarmTelemetry.java

@@ -0,0 +1,190 @@
+package dev.cyberarm.engine.V2;
+
+import android.util.Log;
+
+import java.io.ByteArrayOutputStream;
+import java.io.IOException;
+import java.net.DatagramPacket;
+import java.net.InetAddress;
+import java.net.MulticastSocket;
+import java.nio.charset.StandardCharsets;
+import java.util.ArrayList;
+import java.util.Locale;
+
+/**
+ * One way (Robot Controller -> Client), UDP based, telemetry via multicast.
+ */
+public class CyberarmTelemetry {
+    private static final int PROTOCOL_IDENTIFIER = 0x54494d45;
+    private static final int MAX_PACKET_RAW_SIZE = 508; // bytes
+    private static final int PACKET_HEADER_SIZE = 16; // bytes
+    private static final int MAX_PACKET_BODY_SIZE = MAX_PACKET_RAW_SIZE - PACKET_HEADER_SIZE; // bytes
+    private static final String MULTICAST_ADDRESS = "236.77.83.76";
+    private static final int MULTICAST_PORT = 6388;
+    private static final int MULTICAST_TTL = 5;
+    private static final String TAG = "CYBERARM_TELEMETRY";
+    private final ArrayList<ByteArrayOutputStream> queueBuffer = new ArrayList<>();
+    private InetAddress group;
+    private MulticastSocket multicastSocket;
+
+    private boolean usable = false;
+
+    enum Encode {
+        // Generic/Type encodings
+        INTEGER,
+        LONG,
+        FLOAT,
+        DOUBLE,
+        STRING,
+        BOOLEAN,
+
+        // Special encodings
+        POSE_POSITION, // position of robot in 2D space, relative to field origin
+        TELEMETRY, // string telemetry
+        GAMEPAD, // all 15 buttons + joysticks + triggers input values
+        MOTOR, // current power, velocity, position, target position, and current (amps)
+        SERVO, // current target position
+        CONTINUOUS_SERVO, // current power
+        SENSOR_2M_DISTANCE, // Rev 2 meter distance sensor
+        SENSOR_DIGITAL, // touch or other digital/binary sensor
+    }
+
+    CyberarmTelemetry() {
+        try {
+            this.group = InetAddress.getByName(MULTICAST_ADDRESS);
+            this.multicastSocket = new MulticastSocket(MULTICAST_PORT);
+            this.multicastSocket.setTimeToLive(MULTICAST_TTL);
+            this.multicastSocket.joinGroup(group);
+
+            this.usable = true;
+        } catch (IOException e) {
+            Log.e(TAG, "FAILED to create multicast socket!");
+            e.printStackTrace();
+
+            this.usable = false;
+        }
+    }
+
+    /**
+     * Whether the multicast socket was successfully created
+     * @return usable
+     */
+    public boolean isUsable() {
+        return usable;
+    }
+
+    public void publish() throws IOException
+    {
+        ByteArrayOutputStream buffer = new ByteArrayOutputStream();
+
+        for (ByteArrayOutputStream data : queueBuffer) {
+            if (buffer.size() + data.size() >= MAX_PACKET_BODY_SIZE) {
+                ByteArrayOutputStream output = new ByteArrayOutputStream();
+
+                packHeader(output, buffer);
+                commitPacket(output);
+
+                buffer.reset();
+            }
+
+            buffer.write(data.toByteArray());
+        }
+
+        // We're a lossy protocol, assume that all data in the buffer has been processed and committed
+        queueBuffer.clear();
+    }
+
+    public void addPose(double x, double y, double r) {
+        addPosition(x, y, r);
+    }
+
+    public void addPosition(double x, double y, double r) {
+        try {
+            ByteArrayOutputStream buffer = new ByteArrayOutputStream();
+
+            buffer.write(Encode.POSE_POSITION.ordinal());
+            packDouble(buffer, x);
+            packDouble(buffer, y);
+            packDouble(buffer, r);
+
+            queueBuffer.add(buffer);
+        } catch (IOException e) {
+            Log.e(TAG, "An error occurred while adding robot Pose (Position) to queue");
+            e.printStackTrace();
+        }
+    }
+
+    public void addTelemetry(String msg) {
+        try {
+            ByteArrayOutputStream buffer = new ByteArrayOutputStream();
+
+            buffer.write(Encode.TELEMETRY.ordinal());
+            packString(buffer, msg);
+
+            queueBuffer.add(buffer);
+        } catch (IOException e) {
+            Log.e(TAG, "An error occurred while adding Telemetry message to queue");
+            e.printStackTrace();
+        }
+    }
+
+    // ------- HELPERS -------- //
+    private void packHeader(ByteArrayOutputStream output, ByteArrayOutputStream buffer) throws IOException {
+        output.write(PROTOCOL_IDENTIFIER);
+        output.write(buffer.size()); // BUFFER SIZE (multiple buffers may fit in one packet)
+        output.write(buffer.toByteArray());
+    }
+
+    private void commitPacket(ByteArrayOutputStream buffer) throws IOException {
+        // TODO: send multicast packet(s) on LAN
+
+        // Drop packet if multicast socket is not yet created or failed to be created.
+        if (!isUsable())
+            return;
+
+        DatagramPacket packet = new DatagramPacket(buffer.toByteArray(), buffer.size(), group, MULTICAST_PORT);
+        multicastSocket.send(packet);
+    }
+
+    private void packInt(ByteArrayOutputStream stream, int i) throws IOException {
+        stream.write(Encode.INTEGER.ordinal());
+        stream.write(i);
+    }
+
+    private void packLong(ByteArrayOutputStream stream, long i) throws IOException {
+        stream.write(Encode.LONG.ordinal());
+        stream.write(Long.toBinaryString(i).getBytes());
+    }
+
+    private void packFloat(ByteArrayOutputStream stream, float i) throws IOException {
+        stream.write(Encode.FLOAT.ordinal());
+        String[] string = String.format(Locale.US, "%.8f", i).split("\\.");
+        int wholeNum = Integer.parseInt(string[0]);
+        int decimalPart = Integer.parseInt(string[1]);
+
+        stream.write(wholeNum);
+        stream.write(decimalPart);
+    }
+
+    private void packDouble(ByteArrayOutputStream stream, double i) throws IOException {
+        stream.write(Encode.DOUBLE.ordinal());
+        String[] string = String.format(Locale.US, "%.8f", i).split("\\.");
+        int wholeNum = Integer.parseInt(string[0]);
+        int decimalPart = Integer.parseInt(string[1]);
+
+        stream.write(wholeNum);
+        stream.write(decimalPart);
+    }
+
+    private void packString(ByteArrayOutputStream stream, String i) throws IOException {
+        stream.write(Encode.STRING.ordinal());
+        stream.write(i.length());
+        stream.write(i.getBytes(StandardCharsets.UTF_8));
+    }
+
+    private void packBoolean(ByteArrayOutputStream stream, boolean i) throws IOException {
+        stream.write(Encode.BOOLEAN.ordinal());
+        byte[] bit = {(byte) (i ? 1 : 0)};
+        stream.write(bit);
+    }
+}

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

@@ -0,0 +1,161 @@
+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;
+
+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 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.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;
+    }
+
+    // Meant for setting starting location offset
+    public void teleport(double xMM, double yMM) {
+        this.offsetX = xMM;
+        this.offsetY = yMM;
+    }
+
+    // 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.offsetX = xMM;
+        this.offsetY = yMM;
+        this.rawR = 0; // FIXME HERE // (AngleUnit.DEGREES).toRadians(AngleUnit.normalizeDegrees(headingDegrees)); // cursed :(
+    }
+
+    public void integrate() {
+        odometry.updatePose();
+
+//        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 odometry.getPose().getX() * xPosMultiplier + offsetX; //rawX;
+    }
+
+    public double yMM() {
+        return odometry.getPose().getY() * yPosMultiplier + offsetY;  //rawY;
+    }
+
+    public double xIn() {
+        return xMM() * 0.03937008;
+    }
+    public double yIn() {
+        return yMM() * 0.03937008;
+    }
+
+    // Returns true 360 degrees
+    public double headingDegrees() {
+        double degrees = headingRadians() * 180.0 / Math.PI;
+        return ((degrees + 360.0) % 360.0) % 360.0;
+    }
+
+    // Returns annoying half-split +/- PI radians
+    public double headingRadians() {
+        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

@@ -1,18 +1,16 @@
 package dev.cyberarm.minibots.red_crab;
 
 import com.arcrobotics.ftclib.controller.PIDFController;
-import com.arcrobotics.ftclib.hardware.motors.Motor;
-import com.arcrobotics.ftclib.hardware.motors.MotorEx;
-import com.arcrobotics.ftclib.hardware.motors.MotorGroup;
 import com.qualcomm.hardware.lynx.LynxModule;
 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;
-import com.qualcomm.robotcore.util.Range;
 
 import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
 import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit;
@@ -24,6 +22,9 @@ 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;
 
@@ -46,6 +47,7 @@ public class RedCrabMinibot {
     public static double CLAW_ARM_MAX_SPEED = 0.5;
     public static double CLAW_ARM_MAX_VELOCITY_DEGREES = 10;
     private static double CLAW_ARM_MOTOR_MAX_CURRENT_MILLIAMPS = 1588.0;
+    private static long CLAW_ARM_WARN_OVERCURRENT_AFTER_MS = 5000;
     public static double CLAW_ARM_kP = 0.0;
     public static double CLAW_ARM_kI = 0.0;
     public static double CLAW_ARM_kD = 0.0;
@@ -85,13 +87,30 @@ public class RedCrabMinibot {
     public final DcMotorEx frontLeft, frontRight, backLeft, backRight, winch;
     public final DcMotorEx clawArm;
     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;
     public enum Path {
         LEFT,
         CENTER,
@@ -111,8 +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();
@@ -195,9 +221,11 @@ public class RedCrabMinibot {
 
         /// --- Claw Arm Motor
         /// --- --- (SOFT) RESET MOTOR ENCODER
-        clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        // ONLY RESET ENCODER IN AUTONOMOUS
+        if (autonomous)
+            clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
         /// --- --- DIRECTION
-        clawArm.setDirection(DcMotorSimple.Direction.FORWARD);
+        clawArm.setDirection(DcMotorSimple.Direction.REVERSE);
         /// --- --- BRAKING
         /// --- --- NOTE: This won't hold back much, if anything, but its a small help, maybe? 😃
         clawArm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
@@ -227,8 +255,85 @@ public class RedCrabMinibot {
         hookArm.setDirection(Servo.Direction.FORWARD);
 //        hookArm.setPosition(HOOK_ARM_STOW_POSITION); // LEAVE OFF:
 
+        /// DEAD WHEELS ///
+        deadWheelXLeft = (DcMotorEx) engine.hardwareMap.dcMotor.get("deadwheel_x_left");
+        deadWheelXRight = (DcMotorEx) engine.hardwareMap.dcMotor.get("deadwheel_x_right");
+        deadWheelYCenter = engine.hardwareMap.get(EncoderCustomKB2040.class, "deadwheel_y_center");
+
+        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);
+
+        // Initialize localizer
+        if (autonomous || RedCrabMinibot.localizer == null) {
+            RedCrabMinibot.localizer = new Localizer(this);
+        }
+
+        if (autonomous)
+            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() {
@@ -252,12 +357,13 @@ public class RedCrabMinibot {
         RedCrabMinibot.CLAW_ARM_POSITION_TOLERANCE = config.variable("Robot", "ClawArm_Tuning", "tolerance").value();
         RedCrabMinibot.CLAW_ARM_STOW_ANGLE = config.variable("Robot", "ClawArm_Tuning", "stow_angle").value();
         RedCrabMinibot.CLAW_ARM_DEPOSIT_ANGLE = config.variable("Robot", "ClawArm_Tuning", "deposit_angle").value();
-        RedCrabMinibot.CLAW_ARM_COLLECT_FLOAT_ANGLE = config.variable("Robot", "ClawArm_Tuning", "collect_angle").value();
+        RedCrabMinibot.CLAW_ARM_COLLECT_FLOAT_ANGLE = config.variable("Robot", "ClawArm_Tuning", "collect_float_angle").value();
         RedCrabMinibot.CLAW_ARM_COLLECT_ANGLE = config.variable("Robot", "ClawArm_Tuning", "collect_angle").value();
 
         RedCrabMinibot.CLAW_ARM_MOTOR_GEAR_RATIO = config.variable("Robot", "ClawArm_Tuning", "gear_ratio").value();
         RedCrabMinibot.CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION = config.variable("Robot", "ClawArm_Tuning", "motor_ticks").value();
         RedCrabMinibot.CLAW_ARM_MOTOR_MAX_CURRENT_MILLIAMPS = config.variable("Robot", "ClawArm_Tuning", "max_current_milliamps").value();
+        RedCrabMinibot.CLAW_ARM_WARN_OVERCURRENT_AFTER_MS = config.variable("Robot", "ClawArm_Tuning", "warn_overcurrent_after_ms").value();
 
         /// WINCH
 
@@ -283,11 +389,44 @@ 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 (%.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();
+        }
+
+        engine.telemetry.addLine("Deadwheels");
+        engine.telemetry.addData("X Left", deadWheelXLeft.getCurrentPosition());
+        engine.telemetry.addData("X Right", deadWheelXRight.getCurrentPosition());
+        // Use .getLastPosition instead of .getCurrentPosition here to not make an additional round trip just for telemetry
+        engine.telemetry.addData("Y Center", deadWheelYCenter.getLastPosition());
+        engine.telemetry.addLine();
+
         engine.telemetry.addLine("Motors");
         engine.telemetry.addData(
                 "Front Left",
@@ -412,6 +551,19 @@ public class RedCrabMinibot {
     public void controlClawArm() {
         Action action = config.action("Robot", "ClawArm_Tuning");
 
+        long milliseconds = System.currentTimeMillis();
+        if (clawArm.isOverCurrent())
+        {
+            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 {
+            lastClawArmOverCurrentAnnounced = milliseconds;
+        }
+
         double ticksInDegree = Utilities.motorAngleToTicks(CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, CLAW_ARM_MOTOR_GEAR_RATIO, 1);
 
         for (Variable v : action.getVariables()) {
@@ -450,6 +602,18 @@ public class RedCrabMinibot {
 
         double velocity = Utilities.motorAngleToTicks(CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, CLAW_ARM_MOTOR_GEAR_RATIO, CLAW_ARM_MAX_VELOCITY_DEGREES);
 
+        double currentAngle = Utilities.motorTicksToAngle(CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, CLAW_ARM_MOTOR_GEAR_RATIO, clawArm.getCurrentPosition());
+        double targetAngle = Utilities.motorTicksToAngle(CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, CLAW_ARM_MOTOR_GEAR_RATIO, clawArm.getTargetPosition());
+        double angleDiff = Math.abs(Utilities.angleDiff(currentAngle, targetAngle));
+
+        // Turn off motor if it is stowed or all the way down
+        if (targetAngle <= CLAW_ARM_STOW_ANGLE + 5.0 && angleDiff <= 5.0) {
+            velocity = 0.0;
+        } else if (targetAngle >= CLAW_ARM_COLLECT_ANGLE - 5.0 && angleDiff <= 5.0)
+        {
+            velocity = 0.0;
+        }
+
         clawArm.setVelocity(velocity);
     }
 
@@ -468,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/TeamPropVisionProcessor.java

@@ -158,6 +158,15 @@ public class TeamPropVisionProcessor implements VisionProcessor {
         return saturationRight;
     }
 
+    public double getHighestSaturation() {
+        if (getSaturationLeft() > getSaturationCenter() && getSaturationLeft() > getSaturationRight())
+            return getSaturationLeft();
+        if (getSaturationCenter() > getSaturationLeft() && getSaturationCenter() > getSaturationRight())
+            return getSaturationCenter();
+
+        return getSaturationRight();
+    }
+
     private android.graphics.Rect makeDrawableRect(Rect rect, float scaleBmpPxToCanvasPx) {
         int left = Math.round(rect.x * scaleBmpPxToCanvasPx);
         int top = Math.round(rect.y * scaleBmpPxToCanvasPx);

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,18 +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() {
-        RedCrabMinibot robot = new RedCrabMinibot(true);
-        blackboardSet("clawArmPower", 0.0);
-
+        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,18 +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() {
-        RedCrabMinibot robot = new RedCrabMinibot(true);
-        blackboardSet("clawArmPower", 0.0);
-
+        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/RedCrabAutonomousEngine.java

@@ -1,13 +0,0 @@
-package dev.cyberarm.minibots.red_crab.engines;
-
-import dev.cyberarm.engine.V2.CyberarmEngine;
-import dev.cyberarm.engine.V2.Utilities;
-
-public abstract class RedCrabAutonomousEngine extends CyberarmEngine {
-    @Override
-    public void loop() {
-        Utilities.hubsClearBulkReadCache(hardwareMap);
-
-        super.loop();
-    }
-}

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

@@ -4,18 +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() {
-        RedCrabMinibot robot = new RedCrabMinibot(true);
-        blackboardSet("clawArmPower", 0.0);
-
+        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,18 +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() {
-        RedCrabMinibot robot = new RedCrabMinibot(true);
-        blackboardSet("clawArmPower", 0.0);
-
+        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

@@ -0,0 +1,25 @@
+package dev.cyberarm.minibots.red_crab.engines;
+
+import dev.cyberarm.engine.V2.CyberarmEngine;
+import dev.cyberarm.engine.V2.Utilities;
+import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
+
+public abstract class RedCrabEngine extends CyberarmEngine {
+    protected RedCrabMinibot robot;
+
+    @Override
+    public void loop() {
+        Utilities.hubsClearBulkReadCache(hardwareMap);
+
+        super.loop();
+
+        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,23 +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);
-
-        super.loop();
-    }
 }

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

@@ -31,7 +31,7 @@ public class ClawArmMove extends CyberarmState {
         int tolerance = robot.clawArm.getTargetPositionTolerance();
         int position = robot.clawArm.getCurrentPosition();
 
-        if (Utilities.isBetween(position, position - tolerance, position + tolerance) || runTime() >= timeoutMS) {
+        if (Utilities.isBetween(robot.clawArm.getTargetPosition(), position - tolerance, position + tolerance) || runTime() >= timeoutMS) {
             this.finished();
         }
     }

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/Move.java

@@ -1,5 +1,7 @@
 package dev.cyberarm.minibots.red_crab.states;
 
+import android.util.Log;
+
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.util.Range;
@@ -27,13 +29,21 @@ public class Move extends CyberarmState {
         this.lerpMM_DOWN = robot.config.variable(groupName, actionName, "lerpMM_DOWN").value();
         this.toleranceMM = robot.config.variable(groupName, actionName, "toleranceMM").value();
 
-        this.maxVelocityMM = robot.config.variable(groupName, actionName, "maxVelocityMM").value();
-        this.minVelocityMM = robot.config.variable(groupName, actionName, "minVelocityMM").value();
+        double maxVelocityMM = robot.config.variable(groupName, actionName, "maxVelocityMM").value();
+        double minVelocityMM = robot.config.variable(groupName, actionName, "minVelocityMM").value();
 
         this.strafe = robot.config.variable(groupName, actionName, "strafe").value();
 
         this.timeoutMS = robot.config.variable(groupName, actionName, "timeoutMS").value();
 
+        if (distanceMM < 0) {
+            this.maxVelocityMM = Math.abs(maxVelocityMM) * -1;
+            this.minVelocityMM = Math.abs(minVelocityMM) * -1;
+        } else {
+            this.maxVelocityMM = Math.abs(maxVelocityMM);
+            this.minVelocityMM = Math.abs(minVelocityMM);
+        }
+
         // Validate distance and lerp distances
         if (lerpMM_UP == 0 && lerpMM_DOWN == 0) { return; }
 
@@ -94,6 +104,9 @@ public class Move extends CyberarmState {
             robot.backLeft.setTargetPosition(distanceTicks);
             robot.backRight.setTargetPosition(distanceTicks);
         }
+
+        // Explicitly reset cached hub data, fixes Move state erroneous instantly finishing- sometimes.
+        Utilities.hubsClearBulkReadCache(engine.hardwareMap);
     }
 
     @Override
@@ -122,14 +135,17 @@ public class Move extends CyberarmState {
         double velocity = lerpVelocity(travelledDistance);
 
         double angleDiff = Utilities.angleDiff(initialHeadingDegrees, Utilities.facing(robot.imu));
+        if (maxVelocityMM < 0)
+            angleDiff = -1 * angleDiff;
 
         double leftVelocity = velocity;
         double rightVelocity = velocity;
         // use +10% of power at 7 degrees of error to correct angle
         double correctiveVelocity = Utilities.lerp(0.0, 1.0, angleDiff / 7.0) * (velocity * 0.1);
-        if (angleDiff > -0.5) {
+
+        if (angleDiff < -0.5) {
             leftVelocity += correctiveVelocity;
-        } else if (angleDiff < 0.5) {
+        } else if (angleDiff > 0.5) {
             rightVelocity += correctiveVelocity;
         }
 
@@ -155,7 +171,6 @@ public class Move extends CyberarmState {
         double velocity = lerpVelocity(travelledDistance);
 
         double angleDiff = Utilities.angleDiff(initialHeadingDegrees, Utilities.facing(robot.imu));
-
         double frontVelocity = velocity;
         double backVelocity = velocity;
         // use +40% of power at 7 degrees of error to correct angle

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/PathEnactor.java

@@ -1,17 +1,31 @@
 package dev.cyberarm.minibots.red_crab.states;
 
+import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfigurationError;
+
 import dev.cyberarm.engine.V2.CyberarmState;
+import dev.cyberarm.engine.V2.Utilities;
 import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
 
 public class PathEnactor extends CyberarmState {
     private final RedCrabMinibot robot;
     private String pathGroupName;
+    private int forcePath;
     public PathEnactor(RedCrabMinibot robot, String groupName, String actionName) {
         this.robot = robot;
+
+        try {
+            this.forcePath = robot.config.variable(groupName, actionName, "forcePath").value();
+        } catch (TimeCraftersConfigurationError e) {
+            this.forcePath = -1;
+        }
     }
 
     @Override
     public void start() {
+        // FORCE PATH FOR DEBUGGING
+        if (Utilities.isBetween(forcePath, 0, 2))
+            engine.blackboardSet("autonomousPath", forcePath);
+
         String path;
         switch (engine.blackboardGetInt("autonomousPath")) {
             case 1:
@@ -25,7 +39,7 @@ public class PathEnactor extends CyberarmState {
                 break;
         }
 
-        this.pathGroupName = String.format("AutonomousPixelPath_%s", path);
+        this.pathGroupName = String.format("Autonomous_SpikePath_%s", path);
     }
 
     @Override

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

@@ -18,7 +18,6 @@ public class PathSelector extends CyberarmState {
     private final int timeoutMS;
     private final int path;
     private final double minConfidence;
-    private List<Recognition> recognitions = new ArrayList<>();
 
     public PathSelector(RedCrabMinibot robot, String groupName, String actionName) {
         this.robot = robot;
@@ -34,40 +33,27 @@ public class PathSelector extends CyberarmState {
         robot.visionPortal = VisionPortal.easyCreateWithDefaults(
                 engine.hardwareMap.get(WebcamName.class, robot.webcamName), robot.teamProp);
 
-//        robot.tfPixel.setClippingMargins(0, 0, 0, 0);
-//        robot.tfPixel.setMinResultConfidence((float) minConfidence);
-
         engine.blackboardSet("autonomousPath", path);
     }
 
     @Override
     public void exec() {
-        if (engine.blackboardGetInt("autonomousPath") != 0) {
-            this.finished();
-
-            return;
-        }
-
-//        recognitions = robot.tfPixel.getRecognitions();
-//        for (Recognition recognition : recognitions) {
-//            double x = (recognition.getLeft() + recognition.getRight()) / 2;
-//            double y = (recognition.getTop()  + recognition.getBottom()) / 2;
-//
-//            if (recognition.getLabel().equals("pixel")) {
-//                engine.blackboardSet("autonomousPath", path);
-//            }
-//        }
-
-        switch (robot.teamProp.getLocation()) {
-            case LEFT:
-                engine.blackboardSet("autonomousPath", 0);
-                break;
-            case CENTER:
-                engine.blackboardSet("autonomousPath", 1);
-                break;
-            case RIGHT:
-                engine.blackboardSet("autonomousPath", 2);
-                break;
+        // If we've got enough Saturation for our min confidence then do the needful.
+        if (robot.teamProp.getHighestSaturation() >= minConfidence) {
+            switch (robot.teamProp.getLocation()) {
+                case LEFT:
+                    engine.blackboardSet("autonomousPath", 0);
+                    break;
+                case CENTER:
+                    engine.blackboardSet("autonomousPath", 1);
+                    break;
+                case RIGHT:
+                    engine.blackboardSet("autonomousPath", 2);
+                    break;
+            }
+        } else
+        {
+            engine.blackboardSet("autonomousPath", path); // min confidence not meant, default to center.
         }
 
         if (runTime() >= timeoutMS) {
@@ -85,19 +71,9 @@ public class PathSelector extends CyberarmState {
     public void telemetry() {
         engine.telemetry.addLine();
 
-//        engine.telemetry.addData("Saturation", robot.spikeMark.getSaturation());
-
-//        engine.telemetry.addData("# Objects Detected", recognitions.size());
-//
-//        for(Recognition recognition : recognitions) {
-//            double x = (recognition.getLeft() + recognition.getRight()) / 2;
-//            double y = (recognition.getTop()  + recognition.getBottom()) / 2;
-//
-//            engine.telemetry.addLine();
-//
-//            engine.telemetry.addData("Image", "%s (%.0f %% Conf.)", recognition.getLabel(), recognition.getConfidence() * 100);
-//            engine.telemetry.addData("- Position", "%.0f / %.0f", x, y);
-//            engine.telemetry.addData("- Size", "%.0f x %.0f", recognition.getWidth(), recognition.getHeight());
-//        }
+        engine.telemetry.addData("Location", robot.teamProp.getLocation());
+        engine.telemetry.addData("Saturation Left", robot.teamProp.getSaturationLeft());
+        engine.telemetry.addData("Saturation Center", robot.teamProp.getSaturationCenter());
+        engine.telemetry.addData("Saturation Right", robot.teamProp.getSaturationRight());
     }
 }

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
 
@@ -209,17 +222,6 @@ public class Pilot extends CyberarmState {
 
                 robot.clawWrist.setPosition(RedCrabMinibot.CLAW_WRIST_COLLECT_POSITION);
                 break;
-
-        }
-
-        if (clawArmPosition == RedCrabMinibot.ClawArm_COLLECT &&
-                robot.clawArm.getCurrentPosition() >= Utilities.motorAngleToTicks(
-                RedCrabMinibot.CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION,
-                RedCrabMinibot.CLAW_ARM_MOTOR_GEAR_RATIO,
-                RedCrabMinibot.CLAW_ARM_COLLECT_ANGLE) - 5.0) {
-            robot.clawArm.setPower(0);
-        } else {
-            robot.clawArm.setPower(RedCrabMinibot.CLAW_ARM_MAX_SPEED);
         }
     }
 
@@ -229,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() {
@@ -260,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/dev/cyberarm/testing/CustomEncoderTestEngine.java

@@ -0,0 +1,40 @@
+package dev.cyberarm.testing;
+
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import dev.cyberarm.drivers.EncoderCustomKB2040;
+import dev.cyberarm.engine.V2.CyberarmEngine;
+import dev.cyberarm.engine.V2.CyberarmState;
+
+//@TeleOp(name = "I2C Driver Test", group = "TESTING")
+public class CustomEncoderTestEngine extends CyberarmEngine {
+    @Override
+    public void setup() {
+        addState(new CyberarmState() {
+            private EncoderCustomKB2040 encoder;
+            private double triggerMS = 0;
+            private int position = -1;
+
+            public void init() {
+                encoder = engine.hardwareMap.get(EncoderCustomKB2040.class, "encoder");
+            }
+
+            @Override
+            public void exec() {
+                if (runTime() - triggerMS >= 0) {
+                    triggerMS = runTime();
+
+                    position = encoder.getCurrentPosition();
+//                    encoder.reset();
+                }
+            }
+
+            @Override
+            public void telemetry() {
+                engine.telemetry.addData("POS", position);
+                engine.telemetry.addData("RunTime", runTime());
+                engine.telemetry.addData("Trigger", triggerMS);
+            }
+        });
+    }
+}

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

@@ -11,7 +11,7 @@ import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
 
 import dev.cyberarm.engine.V2.CyberarmEngine;
 
-@Autonomous(name = "BURNSVILLE blue backdrop")
+@Autonomous(name = "old BURNSVILLE blue audience")
 
 public class Competition2BlueBackStage extends CyberarmEngine {
 
@@ -22,7 +22,9 @@ public class Competition2BlueBackStage extends CyberarmEngine {
     public void init() {
         super.init();
         robot.clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.clawArm.setMode(DcMotor.RunMode.RUN_WITHOUT_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);
@@ -30,34 +32,77 @@ public class Competition2BlueBackStage extends CyberarmEngine {
 
     @Override
     public void setup() {
-        this.robot = new CompetitionRobotV1("Burnsville backdrop blue");
+        this.robot = new CompetitionRobotV1("burnsville audience blue");
         this.robot.setup();
-        addState(new DriveToCoordinatesState(robot,"burnsville backdrop blue", "0-00-0"));
-        addState(new ClawArmState(robot,"burnsville backdrop blue", "0-01-0"));
-        addState(new CameraVisionState(robot));
-        addState(new ClawArmState(robot,"burnsville backdrop blue", "0-02-0"));
-
-        addState(new DriveToCoordinatesState(robot,"burnsville backdrop blue", "3-03-0"));
-
-        addState(new ClawFingerState(robot,"burnsville backdrop blue", "0-04-0"));
-
-        addState(new DriveToCoordinatesState(robot,"burnsville backdrop blue", "3-05-0"));
-        addState(new DriveToCoordinatesState(robot,"burnsville backdrop blue", "3-06-0"));
-
-
-
-
-
+//        addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "0-00-0"));
 
+        addState(new ClawArmState(robot,"burnsville audience blue", "0-01-0"));
 
+//        addState(new CameraVisionState(robot));
 
+        addState(new ClawArmState(robot,"burnsville audience blue", "0-01-1"));
+
+        // drive to the left, center, or right spike mark
+//        addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-02-0"));
+//        addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-02-0"));
+        addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-02-0"));
+
+        // place pixel
+        addState(new ClawFingerState(robot,"burnsville audience blue", "0-02-1"));
+
+        // drive to search pos
+        addState(new ClawArmState(robot,"burnsville audience blue", "0-02-2"));
+
+        // close right finger
+        addState(new ClawFingerState(robot,"burnsville audience blue", "0-02-3"));
+
+        // drive back and away from the spike mark (x,y) (1050, 1000) H = 0
+//        addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-03-0"));
+//        addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-03-0"));
+        addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-03-0"));
 
 
+        // drive to the middle truss (right version) (1130,980) H = -90
+//        addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-04-0"));
+//        addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-04-0"));
+        addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-04-0"));
+
+        // drive to hover mode to clear the middle truss
+        addState(new ClawArmState(robot,"burnsville audience blue", "0-04-1"));
 
 
+        // drive under the middle truss across the field (right version) (1170,1080) H = -90
+//        addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-05-0"));
+//        addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-05-0"));
+        addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-05-0"));
+
+        // drive to deposit mode to place on back drop
+        addState(new ClawArmState(robot,"burnsville audience blue", "0-05-1"));
+
+        // drive to back drop
+//        addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-06-0"));
+//        addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-06-0"));
+        addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-06-0"));
+
+        // open claw to deposit gold
+        addState(new ClawFingerState(robot,"burnsville audience blue", "0-06-1"));
+
+        // drive back from backdrop
+//        addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-07-0"));
+//        addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-07-0"));
+        addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-07-0"));
+
+        // close left claw
+        addState(new ClawFingerState(robot,"burnsville audience blue", "0-07-1"));
 
 
+        // drive to parking spot
+//        addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-08-0"));
+//        addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-08-0"));
+        addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-08-0"));
 
+        // arm to collect pos
+        addState(new ClawArmState(robot,"burnsville audience blue", "0-08-1"));
 
     }
 

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

@@ -0,0 +1,67 @@
+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;
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawArmControlTask;
+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 = "Burnsville audience blue", preselectTeleOp = "Competition V1 TeleOp")
+@Disabled
+public class CompetitionBurnsvilleAudienceBlue 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("Burnsville audience blue");
+        addTask(new DriveToCoordinatesTask(robot));
+        addTask(new OdometryLocalizerTask(robot));
+//        addTask(new ClawArmControlTask(robot));
+
+        this.robot.setup();
+        addState(new ClawArmState(robot,"Burnsville audience blue", "0-01-0"));
+
+        addState(new CameraVisionState(robot));
+
+        addState(new ClawArmState(robot,"Burnsville audience blue", "0-01-1"));
+
+        // drive to the left, center, or right spike mark
+        addState(new DriveToCoordinatesState(robot,"Burnsville audience blue", "1-02-0"));
+        addState(new DriveToCoordinatesState(robot,"Burnsville audience blue", "1-02-1"));
+        addState(new DriveToCoordinatesState(robot,"Burnsville audience blue", "2-02-0"));
+        addState(new DriveToCoordinatesState(robot,"Burnsville audience blue", "3-02-0"));
+
+        // place pixel
+        addState(new ClawFingerState(robot,"Burnsville audience blue", "0-02-1"));
+
+        // drive to search pos
+        addState(new ClawArmState(robot,"Burnsville audience blue", "0-02-2"));
+
+        // close right finger
+        addState(new ClawFingerState(robot,"Burnsville audience blue", "0-02-3"));
+    }
+
+}

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

@@ -0,0 +1,68 @@
+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;
+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 = "Burnsville audience red", preselectTeleOp = "Competition V1 TeleOp")
+@Disabled
+
+public class CompetitionBurnsvilleAudienceRed 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("Burnsville audience red");
+        addTask(new DriveToCoordinatesTask(robot));
+        addTask(new OdometryLocalizerTask(robot));
+//        addTask(new ClawArmControlTask(robot));
+
+        this.robot.setup();
+        addState(new ClawArmState(robot,"Burnsville audience red", "0-01-0"));
+
+        addState(new CameraVisionState(robot));
+
+        addState(new ClawArmState(robot,"Burnsville audience red", "0-01-1"));
+
+        // drive to the left, center, or right spike mark
+        addState(new DriveToCoordinatesState(robot,"Burnsville audience red", "3-02-0"));
+        addState(new DriveToCoordinatesState(robot,"Burnsville audience red", "3-02-1"));
+        addState(new DriveToCoordinatesState(robot,"Burnsville audience red", "2-02-0"));
+        addState(new DriveToCoordinatesState(robot,"Burnsville audience red", "1-02-0"));
+
+        // place pixel
+        addState(new ClawFingerState(robot,"Burnsville audience red", "0-02-1"));
+
+        // drive to search pos
+        addState(new ClawArmState(robot,"Burnsville audience red", "0-02-2"));
+
+        // close right finger
+        addState(new ClawFingerState(robot,"Burnsville audience red", "0-02-3"));
+
+    }
+
+}

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

@@ -0,0 +1,89 @@
+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;
+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 org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
+
+import dev.cyberarm.engine.V2.CyberarmEngine;
+
+@Autonomous(name = "Burnsville BackDrop blue", preselectTeleOp = "Competition V1 TeleOp")
+@Disabled
+
+public class CompetitionBurnsvilleBackDropBlue 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("Burnsville BackDrop blue");
+        addTask(new DriveToCoordinatesTask(robot));
+        addTask(new OdometryLocalizerTask(robot));
+//        addTask(new ClawArmControlTask(robot));
+
+        this.robot.setup();
+//        addState(new ClawArmState(robot,"Burnsville BackDrop blue", "0-01-0"));
+
+//        addState(new CameraVisionState(robot));
+
+//        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 ClawArmState(robot,"Burnsville BackDrop blue", "0-02-4"));
+
+        // place pixel
+        addState(new ClawFingerState(robot,"Burnsville BackDrop blue", "0-02-1"));
+
+        // drive to search pos
+        addState(new ClawArmState(robot,"Burnsville BackDrop blue", "0-02-2"));
+
+        // close right finger
+        addState(new ClawFingerState(robot,"Burnsville BackDrop blue", "0-02-3"));
+
+        addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "0-03-0"));
+
+        addState(new ClawArmState(robot,"Burnsville BackDrop red", "0-03-1"));
+
+//        addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "1-04-0"));
+//        addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "2-04-0"));
+        addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "3-04-0"));
+
+        addState(new ClawArmState(robot,"Burnsville BackDrop red", "0-04-1"));
+
+        addState(new ClawFingerState(robot,"Burnsville BackDrop red", "0-04-2"));
+
+        addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "0-05-0"));
+
+
+        addState(new ClawArmState(robot,"Burnsville BackDrop red", "end task"));
+
+//
+    }
+
+}

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

@@ -0,0 +1,88 @@
+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;
+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 = "Burnsville BackDrop red", preselectTeleOp = "Competition V1 TeleOp")
+@Disabled
+
+public class CompetitionBurnsvilleBackDropRed extends CyberarmEngine {
+
+    CompetitionRobotV1 robot;
+
+
+    @Override
+    public void init() {
+        super.init();
+        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("Burnsville BackDrop red");
+        addTask(new DriveToCoordinatesTask(robot));
+        addTask(new OdometryLocalizerTask(robot));
+
+        this.robot.setup();
+        addState(new ClawArmState(robot,"Burnsville BackDrop red", "0-01-0"));
+
+        addState(new CameraVisionState(robot));
+
+        addState(new ClawArmState(robot,"Burnsville BackDrop red", "0-01-1"));
+
+        // drive to the left, center, or right spike mark
+        addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "1-02-0"));
+        addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "1-02-1"));
+        addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "2-02-0"));
+        addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "3-02-0"));
+
+        addState(new ClawArmState(robot,"Burnsville BackDrop red", "0-02-5"));
+
+
+        // place pixel
+        addState(new ClawFingerState(robot,"Burnsville BackDrop red", "0-02-1"));
+
+        // drive to search pos
+        addState(new ClawArmState(robot,"Burnsville BackDrop red", "0-02-2"));
+
+        // close right finger
+        addState(new ClawFingerState(robot,"Burnsville BackDrop red", "0-02-3"));
+
+        addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "0-03-0"));
+
+        addState(new ClawArmState(robot,"Burnsville BackDrop red", "0-03-1"));
+
+        addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "1-04-0"));
+        addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "2-04-0"));
+        addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "3-04-0"));
+
+        addState(new ClawArmState(robot,"Burnsville BackDrop red", "0-04-1"));
+
+        addState(new ClawFingerState(robot,"Burnsville BackDrop red", "0-04-2"));
+
+        addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "0-05-0"));
+
+
+        addState(new ClawArmState(robot,"Burnsville BackDrop red", "end task"));
+
+
+
+    }
+
+}

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,8 +54,7 @@ public class CameraVisionState extends CyberarmState {
 
     @Override
     public void exec() {
-        robot.clawArmControl();
-        if (System.currentTimeMillis() - initTime > 3000){
+        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/ClawArmControlTask.java

@@ -0,0 +1,21 @@
+package org.timecrafters.CenterStage.Autonomous.CompetitionStates;
+
+import com.acmerobotics.dashboard.config.Config;
+
+import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
+
+import dev.cyberarm.engine.V2.CyberarmState;
+import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
+
+@Config
+public class ClawArmControlTask extends CyberarmState {
+
+    CompetitionRobotV1 robot;
+
+    public ClawArmControlTask(CompetitionRobotV1 robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void exec() {robot.clawArmControl();}
+}

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

@@ -13,13 +13,23 @@ public class ClawArmState extends CyberarmState {
 
     CompetitionRobotV1 robot;
     public String armPos;
+    public long wait;
+    public long initTime;
     public ClawArmState(CompetitionRobotV1 robot, String groupName, String actionName) {
         this.robot = robot;
         this.armPos = robot.configuration.variable(groupName, actionName, "armPos").value();
+        this.wait = robot.configuration.variable(groupName, actionName, "wait").value();
+    }
+
+    @Override
+    public void start() {
+        super.start();
+        initTime = System.currentTimeMillis();
     }
 
     @Override
     public void exec() {
+        robot.armTime = System.currentTimeMillis() - initTime;
         // odometry driving ALWAYS
         robot.DriveToCoordinates();
         robot.OdometryLocalizer();
@@ -28,7 +38,7 @@ public class ClawArmState extends CyberarmState {
         robot.armPos = armPos;
         robot.clawArmControl();
 
-        if (robot.clawArm.getCurrentPosition() > robot.target - 20 || robot.clawArm.getCurrentPosition() < robot.target + 20) {
+        if ((robot.clawArm.getCurrentPosition() > robot.target - 20 || robot.clawArm.getCurrentPosition() < robot.target + 20) && wait < System.currentTimeMillis() - initTime) {
             setHasFinished(true);
         }
 

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

@@ -1,11 +1,11 @@
 package org.timecrafters.CenterStage.Autonomous.CompetitionStates;
 
+import android.util.Log;
+
 import com.acmerobotics.dashboard.config.Config;
-import com.arcrobotics.ftclib.controller.PIDController;
 
 import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
 import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
-import org.timecrafters.CenterStage.Common.PrototypeRobot;
 
 import dev.cyberarm.engine.V2.CyberarmState;
 @Config
@@ -15,21 +15,24 @@ 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 long initTime;
+    public double maxHPower;
+    private String actionName;
 
     public DriveToCoordinatesState(CompetitionRobotV1 robot, String groupName, String actionName) {
+        this.actionName = actionName;
+
         this.robot = robot;
         this.xTarget = robot.configuration.variable(groupName, actionName, "xTarget").value();
         this.yTarget = robot.configuration.variable(groupName, actionName, "yTarget").value();
         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();
@@ -38,47 +41,50 @@ public class DriveToCoordinatesState extends CyberarmState {
     @Override
     public void start() {
         super.start();
-        initTime = System.currentTimeMillis();
+        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);
+        }
+
     }
 
     @Override
     public void exec() {
-        if (!posSpecific) {
-            // enter loop
-        } else {
+
+        if (posSpecific) {
             if (objectPos != robot.objectPos) {
                 // enter the ending loop
-//                setHasFinished(true);
+                setHasFinished(true);
+            } else {
+
+                if (armDrive) {
+                    robot.clawArmControl();
+                }
+
+                if (Math.abs(robot.positionX - robot.xTarget) < 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);
+                }
+            }
+        } else {
+                if (armDrive) {
+                    robot.clawArmControl();
+                }
+
+                if (Math.abs(robot.positionX - robot.xTarget) < 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);
+                }
             }
         }
-        if (posAchieved) {
-//            setHasFinished(true);
-        }
-
-        if (armDrive) {
-            robot.clawArmControl();
-        }
-
-        robot.yMaxPower = maxYPower;
-        robot.xMaxPower = maxXPower;
-
-        robot.hTarget = hTarget;
-        robot.yTarget = yTarget;
-        robot.xTarget = xTarget;
-
-        robot.DriveToCoordinates();
-        robot.OdometryLocalizer();
-
-
-        if ((Math.abs(robot.backLeftPower) < 0.18 &&
-                Math.abs(robot.backRightPower) < 0.18 &&
-                Math.abs(robot.frontLeftPower) < 0.18 &&
-                Math.abs(robot.frontRightPower) < 0.18) || (System.currentTimeMillis() - initTime > 5000)) {
-            posAchieved = true;
-        }
-    }
-
-
 
     @Override
     public void telemetry() {

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

@@ -0,0 +1,24 @@
+package org.timecrafters.CenterStage.Autonomous.CompetitionStates;
+
+import android.util.Log;
+
+import com.acmerobotics.dashboard.config.Config;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
+
+import dev.cyberarm.engine.V2.CyberarmState;
+
+@Config
+public class DriveToCoordinatesTask extends CyberarmState {
+
+    CompetitionRobotV1 robot;
+    public DriveToCoordinatesTask(CompetitionRobotV1 robot) {this.robot = robot;}
+    @Override
+    public void exec() {
+        robot.XDrivePowerModifier();
+        robot.YDrivePowerModifier();
+        robot.HDrivePowerModifier();
+        robot.DriveToCoordinates();
+    }
+}

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

@@ -0,0 +1,18 @@
+package org.timecrafters.CenterStage.Autonomous.CompetitionStates;
+
+import com.acmerobotics.dashboard.config.Config;
+
+import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
+
+import dev.cyberarm.engine.V2.CyberarmState;
+
+@Config
+public class OdometryLocalizerTask extends CyberarmState {
+
+    CompetitionRobotV1 robot;
+    public OdometryLocalizerTask(CompetitionRobotV1 robot) {
+        this.robot = robot;
+    }
+    @Override
+    public void exec() {robot.OdometryLocalizer();}
+}

TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiBlueCrabDriveStatev1.java

@@ -0,0 +1,25 @@
+package org.timecrafters.CenterStage.Autonomous.SodiStates;
+
+import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
+
+import dev.cyberarm.engine.V2.CyberarmState;
+
+public class SodiBlueCrabDriveStatev1 extends CyberarmState {
+    final private CompetitionRobotV1 robot;
+
+    public SodiBlueCrabDriveStatev1() {
+        robot = new CompetitionRobotV1("Assignment");
+        robot.setup();
+    }
+    @Override
+    public void init() {
+
+        
+
+    }
+
+    @Override
+    public void exec() {
+
+    }
+}

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

@@ -5,16 +5,19 @@ import com.arcrobotics.ftclib.controller.PIDController;
 import com.qualcomm.hardware.rev.Rev2mDistanceSensor;
 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.DistanceSensor;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.hardware.IMU;
+import com.qualcomm.robotcore.hardware.PIDFCoefficients;
 import com.qualcomm.robotcore.hardware.Servo;
 import com.qualcomm.robotcore.hardware.TouchSensor;
 import com.qualcomm.robotcore.util.ElapsedTime;
 
 import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
 import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.Position;
 import org.openftc.easyopencv.OpenCvCamera;
 import org.openftc.easyopencv.OpenCvCameraFactory;
 import org.openftc.easyopencv.OpenCvCameraRotation;
@@ -38,17 +41,21 @@ public class CompetitionRobotV1 extends Robot {
     // ------------------------------------------------------------------------------------------------------------------ HardwareMap setup:
     public OpenCvWebcam webcam1 = null;
     public WebcamName webCamName;
-    public DcMotor frontLeft, frontRight, backLeft, backRight, lift, clawArm, chinUp;
+    public DcMotor frontLeft, frontRight, backLeft, backRight, lift, chinUp;
+    public DcMotorEx clawArm;
 
     public IMU imu;
-    public Servo shoulder, elbow, leftClaw, rightClaw, chinUpServo;
+    public Servo shoulder, elbow, leftClaw, rightClaw, chinUpServo, shootServo;
     public DistanceSensor customObject;
     public TouchSensor touchLeftArm, touchRightArm;
 
     // ----------------------------------------------------------------------------------------------------------------- odometry variables:
     public static double Hp = 0.8, Hi = 0, Hd = 0;
-    public static double Xp = -0.035, Xi = 0, Xd = 0;
-    public static double Yp = 0.035, Yi = 0, Yd = 0.0013;
+    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;
@@ -57,6 +64,8 @@ public class CompetitionRobotV1 extends Robot {
     public double xTarget = 1000;
     public double yTarget = 1000;
     public double hTarget = 0;
+    public double targetVelocityX = 0;
+    public double targetVelocityY = 0;
 
     public int currentRightPosition = 0;
     public int currentLeftPosition = 0;
@@ -64,22 +73,28 @@ 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)
 
     // heading math variables for pid with imu
     public double headingIntegralSum = 0;
     public double xIntegralSum = 0;
+    public double xVeloIntegralSum = 0;
     public double yIntegralSum = 0;
+    public double yVeloIntegralSum = 0;
     public final double cm_per_tick = (2 * Math.PI * R) / N;
     private double headingLastError = 0;
     private double xLastError = 0;
+    private double xVeloLastError = 0;
     private double yLastError = 0;
+    private double yVeloLastError = 0;
     ElapsedTime headingTimer = new ElapsedTime();
     ElapsedTime xTimer = new ElapsedTime();
     ElapsedTime yTimer = new ElapsedTime();
+    ElapsedTime xVeloTimer = new ElapsedTime();
+    ElapsedTime yVeloTimer = new ElapsedTime();
 
     public double frontLeftPower;
     public double backLeftPower;
@@ -88,23 +103,32 @@ 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 = 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 = 0.38;
-    public  double shoulderDeposit = 0.36;
-    public  double shoulderPassive = 0.8;
-    public  double elbowCollect = 0.02;
-    public  double elbowDeposit = 0;
+    public double p = 0.007, i = 0,  d = 0.0001, f = 0;
+    public double shoulderCollect = 0.86;
+    public double shoulderDeposit = 1;
+    public double shoulderPassive = 1;
+    public double elbowCollect = 0.02;
+    public double elbowDeposit = 0;
 
 
     private HardwareMap hardwareMap;
@@ -140,13 +164,13 @@ public class CompetitionRobotV1 extends Robot {
         backLeft = engine.hardwareMap.dcMotor.get("backLeft");
         chinUp = engine.hardwareMap.dcMotor.get("chinUp");
         lift = engine.hardwareMap.dcMotor.get("Lift");
-        clawArm = engine.hardwareMap.dcMotor.get("clawArm");
+        clawArm = (DcMotorEx) engine.hardwareMap.dcMotor.get("clawArm");
 
         frontRight.setDirection(DcMotorSimple.Direction.FORWARD);
         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);
 
@@ -160,9 +184,6 @@ public class CompetitionRobotV1 extends Robot {
         backLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
         frontRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
         chinUp.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-        clawArm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-
-
 
         frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
         backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
@@ -175,9 +196,10 @@ public class CompetitionRobotV1 extends Robot {
         // ----------------------------------------------------------------------------------------------------------------------------- IMU
         IMU.Parameters parameters = new IMU.Parameters(
                 new RevHubOrientationOnRobot(
-                        RevHubOrientationOnRobot.LogoFacingDirection.LEFT,
+                        RevHubOrientationOnRobot.LogoFacingDirection.RIGHT,
                         RevHubOrientationOnRobot.UsbFacingDirection.UP));
 
+
         imu.initialize(parameters);
 
         configuration = new TimeCraftersConfiguration("Blue Crab");
@@ -197,7 +219,7 @@ public class CompetitionRobotV1 extends Robot {
         leftClaw = hardwareMap.servo.get("leftClaw");
         rightClaw = hardwareMap.servo.get("rightClaw");
         chinUpServo = hardwareMap.servo.get("chinUpServo");
-
+        shootServo = hardwareMap.servo.get("shoot");
 
         elbow.setDirection(Servo.Direction.REVERSE);
 
@@ -205,8 +227,20 @@ public class CompetitionRobotV1 extends Robot {
 
     }
 
+
     // -------------------------------------------------------------------------------------------------------------------------- Functions:
 
+    public void velocityChecker(){
+        long startTime = System.currentTimeMillis();
+        double oldXpos = positionX;
+        double oldYpos = positionX;
+
+        xVelocity = (oldXpos - positionX) / deltaTime;
+        yVelocity = (oldYpos - positionY) / deltaTime;
+
+        deltaTime = startTime - System.currentTimeMillis();
+
+    }
 
     public void OdometryLocalizer() { // ------------------------------------------------------------------------------- Odometry Localizer:
         // update positions
@@ -215,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;
@@ -249,13 +286,13 @@ 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){
         double error = angleWrap(current - reference);
         headingIntegralSum += error * headingTimer.seconds();
-        double derivative = (headingLastError - error) / headingTimer.seconds();
+        double derivative = (error - headingLastError) / headingTimer.seconds();
 
         headingTimer.reset();
 
@@ -285,45 +322,56 @@ public class CompetitionRobotV1 extends Robot {
         return output;
     }
 
+    public void YDrivePowerModifier () {
+
+        rawPidY = XPIDControl(xTarget, positionX);
+
+        if (Math.abs(rawPidY) > yMaxPower) {
+            if (rawPidY < 0) {
+                pidY = -yMaxPower;
+            } else {
+                pidY = yMaxPower;
+            }
+        } else {
+            pidY = rawPidY;
+        }
+    }
+
+    public void XDrivePowerModifier () {
+
+        rawPidX = YPIDControl(yTarget, positionY);
+
+        if (Math.abs(rawPidX) > xMaxPower) {
+            if (rawPidX < 0) {
+                pidX = -xMaxPower;
+            } else {
+                pidX = xMaxPower;
+            }
+        } else {
+            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
 
-//        rawPidY = XPIDControl(xTarget, positionX);
-//        rawPidX = YPIDControl(yTarget, positionY);
-
-        if (Math.abs(yTarget - positionY) > 5) {
-            if (Math.abs(XPIDControl(xTarget, positionX)) >= yMaxPower) {
-                if (XPIDControl(xTarget, positionX) < 0) {
-                    pidY = yMaxPower * -1;
-                } else {
-                    pidY = yMaxPower;
-                }
-            } else {
-                pidY = rawPidX;
-            }
-        } else {
-            pidY = rawPidX;
-        }
-
         double heading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
-        double rx = HeadingPIDControl(Math.toRadians(hTarget), heading);
 
-        if (Math.abs(xTarget - positionX) > 5) {
-            if (Math.abs(YPIDControl(yTarget, positionY)) >= xMaxPower) {
-                if (YPIDControl(yTarget, positionY) < 0) {
-                    pidX = xMaxPower * -1;
-                } else {
-                    pidX = xMaxPower;
-                }
-            } else {
-                pidX = rawPidY;
-            }
-        } else {
-            pidX = rawPidY;
-        }
-
-        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);
@@ -331,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);
@@ -357,7 +405,7 @@ public class CompetitionRobotV1 extends Robot {
         if (Objects.equals(armPos, "deposit")) {
             shoulder.setPosition(shoulderDeposit);
             elbow.setPosition(elbowDeposit);
-            target = 400;
+            target = 300;
 
 
         }
@@ -365,24 +413,21 @@ public class CompetitionRobotV1 extends Robot {
             if (lift.getCurrentPosition() >= 20) {
                 lift.setPower(-0.6);
             } else {
-                shoulder.setPosition(0.38);
-                target = 100;
+                shoulder.setPosition(shoulderCollect);
+                target = 120;
             }
 
         }
         if (armPos.equals("search")) {
-            shoulder.setPosition(0.15);
-            target = 570;
+            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.setPower(power);
+        clawArm.setTargetPosition(target);
+        clawArm.setPower(0.4);
 
     }
 }

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

@@ -30,7 +30,8 @@ public class MiniYTeleOPBot extends Robot {
         this.hardwareMap = CyberarmEngine.instance.hardwareMap;
         CyberarmEngine engine = CyberarmEngine.instance;
 
-        configuration = new TimeCraftersConfiguration("Minibot Yellow");
+//        configuration = new TimeCraftersConfiguration("Minibot Yellow");
+        configuration = new TimeCraftersConfiguration();
 
 
         imu = engine.hardwareMap.get(IMU.class, "imu");

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

@@ -19,8 +19,7 @@ public class SodiPizzaMinibotObject extends Robot {
 
     public HardwareMap hardwareMap;
     public DcMotor leftFront, rightFront, leftBack, rightBack;
-//    public Servo shoulder, gripper;
-    public Servo launcher;
+    public Servo shoulder, gripper, launcher;
     public IMU imu;
     public Rev2mDistanceSensor distSensor;
     private String string;
@@ -70,9 +69,14 @@ 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("arm");
-//        gripper = engine.hardwareMap.servo.get("gripper");
+        shoulder = engine.hardwareMap.servo.get("shoulder");
+        gripper = engine.hardwareMap.servo.get("gripper");
         launcher = engine.hardwareMap.servo.get("launcher");
 
         //Distance Sensor

TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/ArmPosTestEngine.java

@@ -9,7 +9,7 @@ import org.timecrafters.CenterStage.TeleOp.States.YellowMinibotTeleOP;
 
 import dev.cyberarm.engine.V2.CyberarmEngine;
 
-@TeleOp(name = "arm test prototype bot")
+@TeleOp(name = "arm test prototype bot", group = "PROTOTYPE")
 
     public class ArmPosTestEngine extends CyberarmEngine {
     private PrototypeRobot robot;

TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/CompetitionRobotV1Engine.java

@@ -7,7 +7,7 @@ import org.timecrafters.CenterStage.TeleOp.States.CompetitionTeleOpState;
 
 import dev.cyberarm.engine.V2.CyberarmEngine;
 
-@TeleOp(name = "Competition V1 TeleOp", group = "Competition V1")
+@TeleOp(name = "Competition V1 TeleOp", group = "0 Competition V1")
 public class CompetitionRobotV1Engine extends CyberarmEngine {
     private CompetitionRobotV1 robot;
     @Override

TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/DriveMotorPortTestEngine.java

@@ -10,7 +10,7 @@ import org.timecrafters.CenterStage.TeleOp.TestingState.MotorPortTestingState;
 
 import dev.cyberarm.engine.V2.CyberarmEngine;
 
-@TeleOp (name = "Motor Port Test")
+@TeleOp (name = "Motor Port Test", group = "PROTOTYPE")
 public class DriveMotorPortTestEngine extends CyberarmEngine {
 
     MotorPortTestRobot robot;

TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/MiniBTeleOPEngine.java

@@ -7,7 +7,7 @@ import org.timecrafters.CenterStage.TeleOp.States.BlackMiniTeleOP;
 
 import dev.cyberarm.engine.V2.CyberarmEngine;
 
-@TeleOp(name = "Black Minibot")
+@TeleOp(name = "Black Minibot", group = "MINIBOT_SODI")
 
 public class MiniBTeleOPEngine extends CyberarmEngine {
     private MiniBTeleOPBot robot;

TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/MiniYTeleOPEngine.java

@@ -8,7 +8,7 @@ import org.timecrafters.CenterStage.TeleOp.States.YellowMinibotTeleOP;
 
 import dev.cyberarm.engine.V2.CyberarmEngine;
 
-@TeleOp(name = "A Yellow Minibot")
+@TeleOp(name = "A Yellow Minibot", group = "MINIBOT_SODI")
 
     public class MiniYTeleOPEngine extends CyberarmEngine {
     private MiniYTeleOPBot robot;

TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/SodiPizzaTeleOPEngine.java

@@ -6,7 +6,7 @@ import org.timecrafters.CenterStage.TeleOp.States.SodiPizzaTeleOPState;
 
 import dev.cyberarm.engine.V2.CyberarmEngine;
 
-@TeleOp(name = "Sodi Pizza Box Bot TeleOP", group = "")
+@TeleOp(name = "Sodi Pizza Box Bot TeleOP", group = "MINIBOT_SODI")
 public class SodiPizzaTeleOPEngine extends CyberarmEngine {
     @Override
     public void setup() {

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

@@ -20,20 +20,28 @@ public class CompetitionTeleOpState extends CyberarmState {
     private CompetitionRobotV1 robot;
     // ------------------------------------------------------------------------------------------------------------robot claw arm variables:
     private PIDController pidController;
-    public  double p = 0.007, i = 0,  d = 0.0001, f = 0;
+    public  double p = 0.005, i = 0,  d = 0.0001, f = 0;
     public int target = 0;
 
+    // ---------------------------------------------------------------------------------------------------------------------shoot variables:
+    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 boost;
     public double armPower;
     private double currentHeading;
     private boolean headingLock = false;
 
-    public static double Kp = 1;
+    public static double Kp = 0.8;
     public static double Ki = 0;
     public static double Kd = 0;
     private double lastError = 0;
@@ -53,7 +61,7 @@ public class CompetitionTeleOpState extends CyberarmState {
     //---------------------------------------------------------------------------------------------------------------- Drivetrain Variables:
     private boolean lbsVar1;
     private double drivePower = 1;
-    public double rx;
+    public double rx = engine.gamepad1.right_stick_x / 2;
 
     // ------------------------------------------------------------------------------------------------------------------- Slider Variables:
     private int maxExtension = 2000;
@@ -63,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;
 
 
 
@@ -104,21 +113,21 @@ public class CompetitionTeleOpState extends CyberarmState {
         }
     }
 
-
     public void DriveTrainTeleOp () {
 
         boolean lbs1 = engine.gamepad1.left_stick_button;
         if (lbs1 && !lbsVar1) {
             if (drivePower == 1) {
-                drivePower = 0.5;
+                drivePower = 0.35;
             } else {
                 drivePower = 1;
             }
         }
         lbsVar1 = lbs1;
 
-        double x = engine.gamepad1.left_stick_x;
-        double y = -engine.gamepad1.left_stick_y;
+
+        double x = -(engine.gamepad1.left_stick_x);
+        double y = (engine.gamepad1.left_stick_y);
         double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
 
         // angle math to make things field oriented
@@ -195,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.x) {
-//            armPos = "passive";
-//            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.dpad_left) {
+        } else if (engine.gamepad2.x && !engine.gamepad2.start) {
+            armPos = "passive";
+            depositMode = true;
+        } 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);
@@ -228,7 +275,7 @@ public class CompetitionTeleOpState extends CyberarmState {
                 robot.shoulder.setPosition(robot.shoulderCollect);
                 robot.elbow.setPosition(robot.elbowCollect);
                 robot.chinUpServo.setPosition(chinUpServoDown);
-                target = 30;
+                target = 0;
 
             }
         }
@@ -249,12 +296,11 @@ public class CompetitionTeleOpState extends CyberarmState {
                 robot.elbow.setPosition(robot.elbowCollect);
                 target = 120;
             }
-
         }
+
         if (Objects.equals(armPos, "lift up")) {
-            robot.shoulder.setPosition(robot.shoulderDeposit);
-            robot.elbow.setPosition(robot.elbowDeposit);
-            target = 120;
+            robot.shoulder.setPosition(0.2);
+            target = 850;
             robot.chinUpServo.setPosition(chinUpServoUp);
         }
 
@@ -264,18 +310,17 @@ public class CompetitionTeleOpState extends CyberarmState {
                 robot.chinUpServo.setPosition(chinUpServoDown);
             } else {
                 robot.lift.setPower(0);
-                robot.chinUpServo.setPosition(chinUpServoDown);
-                robot.shoulder.setPosition(robot.shoulderCollect);
-                robot.elbow.setPosition(robot.elbowCollect);
+                robot.shoulder.setPosition(0.2);
                 target = 850;
             }
         }
 
         if (armPos.equals("reset")) {
             robot.shoulder.setPosition(robot.shoulderPassive);
+            target = -10000;
             if (robot.touchLeftArm.isPressed() || robot.touchRightArm.isPressed()) {
                 robot.clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-                robot.clawArm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+                robot.clawArm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                 armPos = "collect";
             }
         }
@@ -286,11 +331,30 @@ public class CompetitionTeleOpState extends CyberarmState {
     public void init() {
             super.init();
             pidController = new PIDController(p, i, d);
+            robot.imu.resetYaw();
+        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);
+        } else {
+            robot.shootServo.setPosition(holdPos);
+        }
 
 
         if (engine.gamepad2.dpad_up) {
@@ -331,16 +395,16 @@ public class CompetitionTeleOpState extends CyberarmState {
         DriveTrainTeleOp();
 
         // ---------------------------------------------------------------------------------------- Game Pad 2, arms, claw, drone, and lift:
-        pidController.setPID(p, i, d);
-        int armCurrentPos = robot.clawArm.getCurrentPosition();
-        double pid = pidController.calculate(armCurrentPos, target);
+//        pidController.setPID(p, i, d);
+//        int armCurrentPos = robot.clawArm.getCurrentPosition();
+//        double pid = pidController.calculate(armCurrentPos, target);
 
         if (armPos.equals("reset")){
-            armPower = -0.2;
+            robot.clawArm.setPower(armPower);
         } else {
-            armPower = pid;
+            armPower = 0.4; // pid
         }
-
+        robot.clawArm.setTargetPosition(target);
         robot.clawArm.setPower(armPower);
 
         // ------------------------------------------------------------------------------------------------------------------- Lift Control:
@@ -352,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

@@ -11,6 +11,7 @@ import com.qualcomm.robotcore.hardware.DcMotor;
 
 import org.checkerframework.checker.units.qual.A;
 import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
 import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
 import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
 import org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject;
@@ -21,14 +22,12 @@ import dev.cyberarm.engine.V2.GamepadChecker;
 public class SodiPizzaTeleOPState extends CyberarmState {
 
     final private SodiPizzaMinibotObject robot;
-    private long lastMoveTime, lastDistRead /* <- last time Distance was read*/;
+    private long lastMoveTime, lastDistRead /* <- last time Distance was read*/, startingTime;
     public double drivePower;
     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 float 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;
+    public double approxObjPos /* <- Approximate distance away the nearest obstruction is */, objData1, objData2, objData3;
+    private int objectPos, armPos;
     private boolean droneLaunched;
     private char buttonPressed;
     private GamepadChecker gp1checker, gp2checker;
@@ -40,10 +39,17 @@ public class SodiPizzaTeleOPState extends CyberarmState {
         robot.setup();
     }
 
-    public float getApproxObjPos() {
-        if (System.currentTimeMillis() - lastDistRead >= 500) {
+    public double getApproxObjPos() {
+        if (System.currentTimeMillis() - lastDistRead >= 500 && System.currentTimeMillis() - lastDistRead <= 750) {
             /*Pseudocode: take objData1, wait, take 2, wait, take 3*/
-        }
+            objData1 = robot.distSensor.getDistance(DistanceUnit.MM);
+        } else if (System.currentTimeMillis() - lastDistRead >= 750 && System.currentTimeMillis() - lastDistRead <= 1250) {
+            /*Pseudocode: take objData1, wait, take 2, wait, take 3*/
+            objData2 = robot.distSensor.getDistance(DistanceUnit.MM);
+        } else if (System.currentTimeMillis() - lastDistRead >= 1250 && System.currentTimeMillis() - lastDistRead <= 1750) {
+            /*Pseudocode: take objData1, wait, take 2, wait, take 3*/
+            objData3 = robot.distSensor.getDistance(DistanceUnit.MM);
+        } else
         approxObjPos = (objData1 + objData2 + objData3)/3;
         return approxObjPos;
     }
@@ -52,11 +58,20 @@ public class SodiPizzaTeleOPState extends CyberarmState {
     public void telemetry() {
         engine.telemetry.addData("Launcher Servo: ", robot.launcher.getPosition());
         engine.telemetry.addData("Drone Launched?", droneLaunched);
+
+        engine.telemetry.addLine();
+        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);
@@ -67,70 +82,74 @@ 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);
 
         lastMoveTime = System.currentTimeMillis();
         lastDistRead = System.currentTimeMillis();
+        startingTime = System.currentTimeMillis();
+
+        robot.distSensor.getDistance(DistanceUnit.MM);
     }
 
     @Override
     public void exec() {
-        
-        
-        
 
-        if (Math.abs(engine.gamepad1.left_stick_y) < minInput &&
-                Math.abs(engine.gamepad1.left_stick_x) < minInput &&
-                Math.abs(engine.gamepad1.right_stick_x) < minInput){
-
-            drivePower = 0;
-            robot.leftFront.setPower(drivePower);
-            robot.rightFront.setPower(drivePower);
-            robot.leftBack.setPower(drivePower);
-            robot.rightBack.setPower(drivePower);
+        if(System.currentTimeMillis() - startingTime <= 2000) {
+            getApproxObjPos();
         }
 
-        if (Math.abs(yTransitPercent) > 0.01) {
+        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;
 
-            percentDenom = 100;
-        } else {
-            percentDenom = 0;
+// 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 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;
+
+        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(xTransitPercent) > 0.01) {
-
-            percentDenom = percentDenom + 100;
+        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 (Math.abs(rotPercent) > 0.01) {
-
-            percentDenom = percentDenom + 100;
-        }
-        yTransitPercent = engine.gamepad1.left_stick_y * 100;
-        xTransitPercent = engine.gamepad1.left_stick_x * 100;
-        rotPercent = engine.gamepad1.right_stick_x * -100;
-
-        lfPower = ((yTransitPercent + -xTransitPercent + rotPercent) / percentDenom);
-        robot.leftFront.setPower(lfPower);
-
-        rfPower = ((yTransitPercent + xTransitPercent + -rotPercent) / percentDenom);
-        robot.rightFront.setPower(rfPower);
-
-        lbPower = ((yTransitPercent + xTransitPercent + rotPercent) / percentDenom);
-        robot.leftBack.setPower(lbPower);
-
-        rbPower = ((yTransitPercent + -xTransitPercent + -rotPercent) / percentDenom);
-        robot.rightBack.setPower(rbPower);
-
-        if (engine.gamepad2.left_stick_button) {
+        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();
@@ -142,147 +161,159 @@ 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();
             }
         }
 
-//        // This moves the arm to Collect position, which is at servo position 0.00.
-//        if (engine.gamepad2.a && !engine.gamepad2.start) {
-//            armPos = 1;
-//        }
-//
-//        if (armPos == 1) {
-//
-//            buttonPressed = 'a';
-//
-//            if (Math.abs(drivePower) > 0.25) {
-//                drivePower = 0.15;
-//                robot.leftFront.setPower(drivePower);
-//                robot.leftBack.setPower(drivePower);
-//                robot.rightFront.setPower(drivePower);
-//                robot.rightBack.setPower(drivePower);
-//            } else {
-//                //if servo's position is greater than Collect position with a run-to tolerance of 0.05,
-//                //decrement position at a rate of 0.05 per 150 milliseconds.
-//                if (robot.shoulder.getPosition() > ARM_PRECOLLECT && System.currentTimeMillis() - lastMoveTime >= 150) {
-//                    robot.shoulder.setPosition(robot.shoulder.getPosition() - 0.05);
-//                    lastMoveTime = System.currentTimeMillis();
-//                } else if (System.currentTimeMillis() - lastMoveTime >= 150) {
-//                    robot.shoulder.setPosition(ARM_COLLECT);
-//                    armPos = 0;
-//                }
-//
-//            }
-//
-//        }
-//        //End of code for armPos = 1
-//
-//        // This moves the arm to Precollect position, which is at servo position 0.05.
-//        if (engine.gamepad2.x) {
-//            armPos = 2;
-//        }
-//
-//        if (armPos == 2) {
-//
-//            buttonPressed = 'x';
-//
-//            if (Math.abs(drivePower) > 0.25) {
-//                drivePower = 0.15;
-//                robot.leftFront.setPower(drivePower);
-//                robot.leftBack.setPower(drivePower);
-//                robot.rightFront.setPower(drivePower);
-//                robot.rightBack.setPower(drivePower);
-//            } else {
-//                //if servo's position is greater than Precollect position with a run-to tolerance of 0.05,
-//                //decrement position at a rate of 0.05 per 150 milliseconds.
-//                if (robot.shoulder.getPosition() > ARM_PRECOLLECT + 0.05 && System.currentTimeMillis() - lastMoveTime >= 150) {
-//                    robot.shoulder.setPosition(robot.shoulder.getPosition() - 0.05);
-//                    lastMoveTime = System.currentTimeMillis();
-//                }//Incrementing from Collect position is unnecessary, because Collect is within the tolerance of run-to.
-//                 else if (System.currentTimeMillis() - lastMoveTime >= 150) {
-//                    robot.shoulder.setPosition(ARM_PRECOLLECT);
-//                    armPos = 0;
-//                }
-//
-//            }
-//        }
-//        //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) {
-//            armPos = 3;
-//        }
-//
-//        if (armPos == 3) {
-//
-//            buttonPressed = 'b';
-//
-//            if (Math.abs(drivePower) > 0.25) {
-//                drivePower = 0.15;
-//                robot.leftFront.setPower(drivePower);
-//                robot.leftBack.setPower(drivePower);
-//                robot.rightFront.setPower(drivePower);
-//                robot.rightBack.setPower(drivePower);
-//            } else {
-//                //if servo's position is less than Deliver position with a run-to tolerance of 0.05,
-//                //increment position at a rate of 0.05 per 150 milliseconds.
-//                if (robot.shoulder.getPosition() < ARM_DELIVER - 0.05 && System.currentTimeMillis() - lastMoveTime >= 150) {
-//                    robot.shoulder.setPosition(robot.shoulder.getPosition() + 0.05);
-//                    lastMoveTime = System.currentTimeMillis();
-//                }//if servo's position is greater than Deliver position with a run-to tolerance of 0.05,
-//                //decrement position at a rate of 0.05 per 150 milliseconds.
-//                else if (robot.shoulder.getPosition() > ARM_DELIVER + 0.05 && System.currentTimeMillis() - lastMoveTime >= 150) {
-//                    robot.shoulder.setPosition(robot.shoulder.getPosition() - 0.05);
-//                    lastMoveTime = System.currentTimeMillis();
-//                } else if (System.currentTimeMillis() - lastMoveTime >= 150) {
-//                    robot.shoulder.setPosition(ARM_DELIVER);
-//                    armPos = 0;
-//                }
-//
-//            }
-//        }
-//        //End of code for armPos = 3
-//
-//        // This moves the arm to Stow position, which is at servo position 0.72.
-//         if (engine.gamepad2.y) {
-//            armPos = 4;
-//        }
-//
-//        if (armPos == 4) {
-//
-//            buttonPressed = 'y';
-//
-//            if (Math.abs(drivePower) > 0.25) {
-//                drivePower = 0.15;
-//                robot.leftFront.setPower(drivePower);
-//                robot.leftBack.setPower(drivePower);
-//                robot.rightFront.setPower(drivePower);
-//                robot.rightBack.setPower(drivePower);
-//            } else {
-//                //if servo's position is less than Collect position with a run-to tolerance of 0.05,
-//                //increment position at a rate of 0.05 per 150 milliseconds.
-//                if (robot.shoulder.getPosition() < ARM_STOW - 0.05 && System.currentTimeMillis() - lastMoveTime >= 150) {
-//                    robot.shoulder.setPosition(robot.shoulder.getPosition() + 0.05);
-//                    lastMoveTime = System.currentTimeMillis();
-//                }
-//                //Decrementing is unnecessary, because servo is mechanically inhibited from further advancing.
-//                else if (System.currentTimeMillis() - lastMoveTime >= 150) {
-//                    robot.shoulder.setPosition(ARM_STOW);
-//                    armPos = 0;
-//                }
-//
-//            }
-//        }
-        //End of code for armPos = 4
+        if (engine.gamepad1.dpad_up) {
+            if (System.currentTimeMillis() - lastMoveTime >= 150) {
+                robot.shoulder.setPosition(robot.shoulder.getPosition() + 0.05);
+                lastMoveTime = System.currentTimeMillis();
+            }
+        } else if (engine.gamepad1.dpad_down) {
+            if (System.currentTimeMillis() - lastMoveTime >= 150) {
+                robot.shoulder.setPosition(robot.shoulder.getPosition() - 0.05);
+                lastMoveTime = System.currentTimeMillis();
+            }
+        }
 
-//        if (engine.gamepad2.dpad_left) {
-//            robot.gripper.setPosition(GRIPPER_OPEN);
-//        } else if (engine.gamepad2.dpad_right) {
-//            robot.gripper.setPosition(GRIPPER_CLOSED);
-//        }
+        // This moves the arm to Collect position, which is at servo position 0.00.
+        if (engine.gamepad1.a && !engine.gamepad1.start) {
+            armPos = 1;
+        }
+
+        if (armPos == 1) {
+
+            buttonPressed = 'a';
+
+            if (Math.abs(drivePower) > 0.15) {
+                drivePower = 0.15;
+                robot.leftFront.setPower(drivePower);
+                robot.leftBack.setPower(drivePower);
+                robot.rightFront.setPower(drivePower);
+                robot.rightBack.setPower(drivePower);
+            } else {
+                //if servo's position is greater than Collect position with a run-to tolerance of 0.05,
+                //decrement position at a rate of 0.05 per 150 milliseconds.
+                if (robot.shoulder.getPosition() > ARM_PRECOLLECT && System.currentTimeMillis() - lastMoveTime >= 150) {
+                    robot.shoulder.setPosition(robot.shoulder.getPosition() - 0.05);
+                    lastMoveTime = System.currentTimeMillis();
+                } else if (System.currentTimeMillis() - lastMoveTime >= 150) {
+                    robot.shoulder.setPosition(ARM_COLLECT);
+                    armPos = 0;
+                }
+
+            }
+
+        }
+        //End of code for armPos = 1
+
+        // This moves the arm to Precollect position, which is at servo position 0.05.
+        if (engine.gamepad1.x) {
+            armPos = 2;
+        }
+
+        if (armPos == 2) {
+
+            buttonPressed = 'x';
+
+            if (Math.abs(drivePower) > 0.15) {
+                drivePower = 0.15;
+                robot.leftFront.setPower(drivePower);
+                robot.leftBack.setPower(drivePower);
+                robot.rightFront.setPower(drivePower);
+                robot.rightBack.setPower(drivePower);
+            } else {
+                //if servo's position is greater than Precollect position with a run-to tolerance of 0.05,
+                //decrement position at a rate of 0.05 per 150 milliseconds.
+                if (robot.shoulder.getPosition() > ARM_PRECOLLECT + 0.05 && System.currentTimeMillis() - lastMoveTime >= 150) {
+                    robot.shoulder.setPosition(robot.shoulder.getPosition() - 0.05);
+                    lastMoveTime = System.currentTimeMillis();
+                }//Incrementing from Collect position is unnecessary, because Collect is within the tolerance of run-to.
+                 else if (System.currentTimeMillis() - lastMoveTime >= 150) {
+                    robot.shoulder.setPosition(ARM_PRECOLLECT);
+                    armPos = 0;
+                }
+
+            }
+        }
+        //End of code for armPos = 2
+
+        // This moves the arm to Deliver position, which is at servo position 0.28.
+        if (engine.gamepad1.b && !engine.gamepad1.start) {
+            armPos = 3;
+        }
+
+        if (armPos == 3) {
+
+            buttonPressed = 'b';
+
+            if (Math.abs(drivePower) > 0.15) {
+                drivePower = 0.15;
+                robot.leftFront.setPower(drivePower);
+                robot.leftBack.setPower(drivePower);
+                robot.rightFront.setPower(drivePower);
+                robot.rightBack.setPower(drivePower);
+            } else {
+                //if servo's position is less than Deliver position with a run-to tolerance of 0.05,
+                //increment position at a rate of 0.05 per 150 milliseconds.
+                if (robot.shoulder.getPosition() < ARM_DELIVER - 0.05 && System.currentTimeMillis() - lastMoveTime >= 150) {
+                    robot.shoulder.setPosition(robot.shoulder.getPosition() + 0.05);
+                    lastMoveTime = System.currentTimeMillis();
+                }//if servo's position is greater than Deliver position with a run-to tolerance of 0.05,
+                //decrement position at a rate of 0.05 per 150 milliseconds.
+                else if (robot.shoulder.getPosition() > ARM_DELIVER + 0.05 && System.currentTimeMillis() - lastMoveTime >= 150) {
+                    robot.shoulder.setPosition(robot.shoulder.getPosition() - 0.05);
+                    lastMoveTime = System.currentTimeMillis();
+                } else if (System.currentTimeMillis() - lastMoveTime >= 150) {
+                    robot.shoulder.setPosition(ARM_DELIVER);
+                    armPos = 0;
+                }
+
+            }
+        }
+        //End of code for armPos = 3
+
+        // This moves the arm to Stow position, which is at servo position 0.72.
+         if (engine.gamepad1.y) {
+            armPos = 4;
+        }
+
+        if (armPos == 4) {
+
+            buttonPressed = 'y';
+
+            if (Math.abs(drivePower) > 0.15) {
+                drivePower = 0.15;
+                robot.leftFront.setPower(drivePower);
+                robot.leftBack.setPower(drivePower);
+                robot.rightFront.setPower(drivePower);
+                robot.rightBack.setPower(drivePower);
+            } else {
+                //if servo's position is less than Collect position with a run-to tolerance of 0.05,
+                //increment position at a rate of 0.05 per 150 milliseconds.
+                if (robot.shoulder.getPosition() < ARM_STOW - 0.05 && System.currentTimeMillis() - lastMoveTime >= 150) {
+                    robot.shoulder.setPosition(robot.shoulder.getPosition() + 0.05);
+                    lastMoveTime = System.currentTimeMillis();
+                }
+                //Decrementing is unnecessary, because servo is mechanically inhibited from further advancing.
+                else if (System.currentTimeMillis() - lastMoveTime >= 150) {
+                    robot.shoulder.setPosition(ARM_STOW);
+                    armPos = 0;
+                }
+
+            }
+        }
+//        End of code for armPos = 4
+
+        if (engine.gamepad1.dpad_left) {
+            robot.gripper.setPosition(GRIPPER_OPEN);
+        } else if (engine.gamepad1.dpad_right) {
+            robot.gripper.setPosition(GRIPPER_CLOSED);
+        }
 
         gp1checker.update();
         gp2checker.update();