add utils folder from 2024-robot and add WPILib dependencies + plugin

lib/build.gradle.kts

@@ -7,7 +7,9 @@
 
 plugins {
     // Apply the java-library plugin for API and implementation separation.
-    `java-library`
+    id `java-library`
+    // WPILib plugin
+    id `edu.wpi.first.GradleRIO` version `2024.3.2`
 }
 
 repositories {
@@ -26,6 +28,10 @@ dependencies {
 
     // This dependency is used internally, and not exposed to consumers on their own compile classpath.
     implementation(libs.guava)
+
+    // WPILib dependencies
+    implementation wpi.java.deps.wpilib()
+	implementation wpi.java.vendor.java()
 }
 
 // Apply a specific Java toolchain to ease working on different environments.

lib/src/main/java/org/utils/AllianceUtil.java

@@ -0,0 +1,130 @@
+package frc.robot.utils;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.DriverStation;
+import frc.robot.Constants.FieldConstants;
+import org.littletonrobotics.junction.Logger;
+
+public class AllianceUtil {
+
+    public static Translation2d getFieldToSpeaker() {
+        if (!DriverStation.getAlliance().isEmpty()) {
+            switch (DriverStation.getAlliance().get()) {
+                case Blue:
+                    Logger.recordOutput("Field/speaker", FieldConstants.fieldToBlueSpeaker);
+                    return FieldConstants.fieldToBlueSpeaker;
+                case Red:
+                    Logger.recordOutput("Field/speaker", FieldConstants.fieldToRedSpeaker);
+                    return FieldConstants.fieldToRedSpeaker;
+            }
+        }
+        return FieldConstants.fieldToRedSpeaker;
+    }
+
+    public static Rotation2d getAmpHeading() {
+        Logger.recordOutput("Field/amp", FieldConstants.ampHeading);
+        return FieldConstants.ampHeading;
+    }
+
+    public static Pose2d getPoseAgainstSpeaker() {
+        if (!DriverStation.getAlliance().isEmpty()) {
+            switch (DriverStation.getAlliance().get()) {
+                case Blue:
+                    return FieldConstants.robotAgainstBlueSpeaker;
+                case Red:
+                    return FieldConstants.robotAgainstRedSpeaker;
+            }
+        }
+        return FieldConstants.robotAgainstRedSpeaker;
+    }
+
+    public static Pose2d getPoseAgainstSpeakerLeft() {
+        if (!DriverStation.getAlliance().isEmpty()) {
+            switch (DriverStation.getAlliance().get()) {
+                case Blue:
+                    return FieldConstants.robotAgainstBlueSpeakerLeft;
+                case Red:
+                    return FieldConstants.robotAgainstRedSpeakerLeft;
+            }
+        }
+        return FieldConstants.robotAgainstRedSpeakerLeft;
+    }
+
+    public static Pose2d getPoseAgainstSpeakerRight() {
+        if (!DriverStation.getAlliance().isEmpty()) {
+            switch (DriverStation.getAlliance().get()) {
+                case Blue:
+                    return FieldConstants.robotAgainstBlueSpeakerRight;
+                case Red:
+                    return FieldConstants.robotAgainstRedSpeakerRight;
+            }
+        }
+        return FieldConstants.robotAgainstRedSpeakerRight;
+    }
+
+    public static Pose2d getPoseAgainstPodium() {
+        if (!DriverStation.getAlliance().isEmpty()) {
+            switch (DriverStation.getAlliance().get()) {
+                case Blue:
+                    return FieldConstants.robotAgainstBluePodium;
+                case Red:
+                    return FieldConstants.robotAgainstRedPodium;
+            }
+        }
+        return FieldConstants.robotAgainstRedPodium;
+    }
+
+    public static Pose2d getPoseAgainstAmpZone() {
+        if (!DriverStation.getAlliance().isEmpty()) {
+            switch (DriverStation.getAlliance().get()) {
+                case Blue:
+                    return FieldConstants.robotAgainstRedAmpZone;
+                case Red:
+                    return FieldConstants.robotAgainstBlueAmpZone;
+            }
+        }
+        return FieldConstants.robotAgainstRedAmpZone;
+    }
+
+    public static Rotation2d getSourceHeading() {
+        if (!DriverStation.getAlliance().isEmpty()) {
+            switch (DriverStation.getAlliance().get()) {
+                case Blue:
+                    Logger.recordOutput("Field/source", FieldConstants.blueSourceHeading);
+                    return FieldConstants.blueSourceHeading;
+                case Red:
+                    Logger.recordOutput("Field/source", FieldConstants.redSourceHeading);
+                    return FieldConstants.redSourceHeading;
+            }
+        }
+        return FieldConstants.redSourceHeading;
+    }
+
+    /** Returns whether the speaker is significantly to the robot's left */
+    public static boolean isLeftOfSpeaker(double robotY, double tolerance) {
+        if (!DriverStation.getAlliance().isEmpty()) {
+            switch (DriverStation.getAlliance().get()) {
+                case Blue:
+                    return robotY > FieldConstants.fieldToBlueSpeaker.getY() + tolerance;
+                case Red:
+                    return robotY < FieldConstants.fieldToRedSpeaker.getY() - tolerance;
+            }
+        }
+        return robotY < FieldConstants.fieldToRedSpeaker.getY() - tolerance;
+    }
+
+    /** Returns whether the speaker is significantly to the robot's right */
+    public static boolean isRightOfSpeaker(double robotY, double tolerance) {
+        if (!DriverStation.getAlliance().isEmpty()) {
+            switch (DriverStation.getAlliance().get()) {
+                case Blue:
+                    return robotY < FieldConstants.fieldToBlueSpeaker.getY() - tolerance;
+                case Red:
+                    return robotY > FieldConstants.fieldToRedSpeaker.getY() + tolerance;
+            }
+        }
+        return robotY > FieldConstants.fieldToRedSpeaker.getY() + tolerance;
+    }
+}

lib/src/main/java/org/utils/Deadband.java

@@ -0,0 +1,41 @@
+package frc.robot.utils;
+
+public class Deadband {
+    // 1D Deadband in linear format
+    public static double oneAxisDeadband(double input, double deadband) {
+        if (Math.abs(input) < deadband) {
+            return 0;
+        } else {
+            return input;
+        }
+    }
+
+    // 2D Deadband in a circular format
+    public static double[] twoAxisDeadband(double inputX, double inputY, double deadband) {
+        double[] output = new double[2];
+        if (Math.sqrt(Math.pow(inputX, 2) + Math.pow(inputY, 2)) < deadband) {
+            output[0] = 0;
+            output[1] = 0;
+        } else {
+            output[0] = inputX;
+            output[1] = inputY;
+        }
+        return output;
+    }
+
+    // 3D Deadband in a spherical format
+    public static double[] threeAxisDeadband(
+            double inputX, double inputY, double inputZ, double deadband) {
+        double[] output = new double[3];
+        if (Math.sqrt(Math.pow(inputX, 2) + Math.pow(inputY, 2) + Math.pow(inputZ, 2)) < deadband) {
+            output[0] = 0;
+            output[1] = 0;
+            output[2] = 0;
+        } else {
+            output[0] = inputX;
+            output[1] = inputY;
+            output[2] = inputZ;
+        }
+        return output;
+    }
+}

lib/src/main/java/org/utils/FieldFinder.java

@@ -0,0 +1,169 @@
+package frc.robot.utils;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Pose2d;
+import java.awt.geom.Line2D;
+
+public class FieldFinder {
+    private class FieldLocations2024 {
+        public static final double STAGE_RED_NEAR_SIDE_X = 13.6; // TODO: Make these less arbitrary
+        public static final double STAGE_RED_NEAR_SIDE_Y = 4.0;
+        public static final double STAGE_RED_LEFT_SIDE_X = 10.8;
+        public static final double STAGE_RED_LEFT_SIDE_Y = 2.2;
+        public static final double STAGE_RED_RIGHT_SIDE_X = 10.8;
+        public static final double STAGE_RED_RIGHT_SIDE_Y = 5.8;
+
+        public static final double STAGE_BLUE_NEAR_SIDE_X = 3.0;
+        public static final double STAGE_BLUE_NEAR_SIDE_Y = 4.0;
+        public static final double STAGE_BLUE_LEFT_SIDE_X = 5.6;
+        public static final double STAGE_BLUE_LEFT_SIDE_Y = 5.8;
+        public static final double STAGE_BLUE_RIGHT_SIDE_X = 5.6;
+        public static final double STAGE_BLUE_RIGHT_SIDE_Y = 2.2;
+
+        public static final double WING_RED_END_X = 12; // Actual 10.8
+
+        public static final double WING_BLUE_END_X = 4.6; // Actual 5.8
+    }
+
+    public enum FieldLocations {
+        RED_STAGE,
+        BLUE_STAGE,
+        RED_WING,
+        BLUE_WING,
+        MIDDLE
+    }
+
+    /**
+     * This method is used to determine where the robot is on the field
+     *
+     * @param x The x position of the robot
+     * @param y The y position of the robot
+     * @return The location on the field
+     */
+    public static FieldLocations whereAmI(double x, double y) {
+
+        if (inTheTriangle(
+                x,
+                y,
+                FieldLocations2024.STAGE_RED_NEAR_SIDE_X,
+                FieldLocations2024.STAGE_RED_NEAR_SIDE_Y,
+                FieldLocations2024.STAGE_RED_LEFT_SIDE_X,
+                FieldLocations2024.STAGE_RED_LEFT_SIDE_Y,
+                FieldLocations2024.STAGE_RED_RIGHT_SIDE_X,
+                FieldLocations2024.STAGE_RED_RIGHT_SIDE_Y)) {
+            return FieldLocations.RED_STAGE;
+        } else if (inTheTriangle(
+                x,
+                y,
+                FieldLocations2024.STAGE_BLUE_NEAR_SIDE_X,
+                FieldLocations2024.STAGE_BLUE_NEAR_SIDE_Y,
+                FieldLocations2024.STAGE_BLUE_LEFT_SIDE_X,
+                FieldLocations2024.STAGE_BLUE_LEFT_SIDE_Y,
+                FieldLocations2024.STAGE_BLUE_RIGHT_SIDE_X,
+                FieldLocations2024.STAGE_BLUE_RIGHT_SIDE_Y)) {
+            return FieldLocations.BLUE_STAGE;
+        } else if (x > FieldLocations2024.WING_RED_END_X) {
+            return FieldLocations.RED_WING;
+        } else if (x < FieldLocations2024.WING_BLUE_END_X) {
+            return FieldLocations.BLUE_WING;
+        } else {
+            return FieldLocations.MIDDLE;
+        }
+    }
+
+    /**
+     * This method is used to determine where the robot is on the field
+     *
+     * @param pose The x pose of the robot
+     * @return The location on the field
+     */
+    public static FieldLocations whereAmI(Pose2d pose) {
+        return whereAmI(pose.getX(), pose.getY());
+    }
+
+    /**
+     * This method is used to determine if the robot will hit a location on the field
+     *
+     * @param x The x position of the robot
+     * @param y The y position of the robot
+     * @param dx The change in, or delta, x of the robot (not to be confused with x velocity)
+     * @param dy The change in, or delta, y of the robot (not to be confused with y velocity)
+     * @param location The location on the field to check
+     * @return {@code true} if the robot will hit the location
+     */
+    public static boolean willIHitThis(
+            double x, double y, double dx, double dy, FieldLocations location) {
+        switch (location) {
+            case RED_STAGE:
+                return willIHitTriangle(
+                        x,
+                        y,
+                        dx,
+                        dy,
+                        FieldLocations2024.STAGE_RED_NEAR_SIDE_X,
+                        FieldLocations2024.STAGE_RED_NEAR_SIDE_Y,
+                        FieldLocations2024.STAGE_RED_LEFT_SIDE_X,
+                        FieldLocations2024.STAGE_RED_LEFT_SIDE_Y,
+                        FieldLocations2024.STAGE_RED_RIGHT_SIDE_X,
+                        FieldLocations2024.STAGE_RED_RIGHT_SIDE_Y);
+            case BLUE_STAGE:
+                return willIHitTriangle(
+                        x,
+                        y,
+                        dx,
+                        dy,
+                        FieldLocations2024.STAGE_BLUE_NEAR_SIDE_X,
+                        FieldLocations2024.STAGE_BLUE_NEAR_SIDE_Y,
+                        FieldLocations2024.STAGE_BLUE_LEFT_SIDE_X,
+                        FieldLocations2024.STAGE_BLUE_LEFT_SIDE_Y,
+                        FieldLocations2024.STAGE_BLUE_RIGHT_SIDE_X,
+                        FieldLocations2024.STAGE_BLUE_RIGHT_SIDE_Y);
+            case RED_WING:
+                return x > FieldLocations2024.WING_RED_END_X
+                        || x + dx > FieldLocations2024.WING_RED_END_X;
+            case BLUE_WING:
+                return x < FieldLocations2024.WING_BLUE_END_X
+                        || x + dx < FieldLocations2024.WING_BLUE_END_X;
+            case MIDDLE:
+                return (x < FieldLocations2024.WING_RED_END_X
+                                || x + dx < FieldLocations2024.WING_RED_END_X)
+                        && (x > FieldLocations2024.WING_BLUE_END_X
+                                || x + dx > FieldLocations2024.WING_BLUE_END_X);
+            default:
+                return false;
+        }
+    }
+
+    private static boolean willIHitTriangle(
+            double x,
+            double y,
+            double dx,
+            double dy,
+            double x1,
+            double y1,
+            double x2,
+            double y2,
+            double x3,
+            double y3) {
+        return inTheTriangle(x, y, x1, y1, x2, y2, x3, y3)
+                || Line2D.linesIntersect(x, y, x + dx, y + dy, x1, y1, x2, y2)
+                || Line2D.linesIntersect(x, y, x + dx, y + dy, x2, y2, x3, y3)
+                || Line2D.linesIntersect(x, y, x + dx, y + dy, x3, y3, x1, y1);
+    }
+
+    private static boolean inTheTriangle(
+            double x, double y, double x1, double y1, double x2, double y2, double x3, double y3) {
+        double area = areaOfTriangle(x1, y1, x2, y2, x3, y3);
+
+        double a1 = areaOfTriangle(x, y, x2, y2, x3, y3);
+        double a2 = areaOfTriangle(x1, y1, x, y, x3, y3);
+        double a3 = areaOfTriangle(x1, y1, x2, y2, x, y);
+
+        return MathUtil.isNear(area, a1 + a2 + a3, 0.05);
+    }
+
+    private static double areaOfTriangle(
+            double x1, double y1, double x2, double y2, double x3, double y3) {
+        return 0.5 * Math.abs(x1 * (y2 - y3) + x2 * (y3 - y1) + x3 * (y1 - y2));
+    }
+}