Add basic Gradle Library Implementation

lib/build.gradle

@@ -0,0 +1,107 @@
+import java.text.SimpleDateFormat
+
+plugins {
+    // Apply the java-library plugin for API and implementation separation.
+    id 'java-library'
+    id "edu.wpi.first.GradleRIO" version "2024.3.2"
+	id "com.peterabeles.gversion" version "1.10"
+	id "com.diffplug.spotless" version "6.24.0"
+	id 'org.ajoberstar.grgit' version "5.2.1"
+}
+
+repositories {
+    // Use Maven Central for resolving dependencies.
+    mavenCentral()
+    mavenLocal()
+}
+
+def MAIN_CLASS = "coppercore"
+
+deploy {
+    targets {
+        roborio(getTargetTypeClass("RoboRIO")) {
+            team = project.frc.getTeamOrDefault(401)
+            debug = project.frc.getDebugOrDefault(false)
+
+            artifacts {
+
+                frcJava(getArtifactTypeClass('FRCJavaArtifact')) {
+
+                }
+
+                frcStaticFileDeploy(getArtifactTypeClass("FileTreeArtifact")) {
+                    files = project.fileTree('lib/src/main/deploy')
+                    directory = "/home/lvuser/deploy"
+                }
+            }
+        }
+    }
+}
+
+def deployArtifact = deploy.targets.roborio.artifacts.frcJava
+
+def includeDesktopSupport = true
+
+dependencies {
+    // Use JUnit Jupiter for testing.
+    testImplementation libs.junit.jupiter
+
+    testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
+
+    // This dependency is exported to consumers, that is to say found on their compile classpath.
+    api libs.commons.math3
+
+    // This dependency is used internally, and not exposed to consumers on their own compile classpath.
+    implementation libs.guava
+
+    // WPILIB setup
+	implementation wpi.java.deps.wpilib()
+	implementation wpi.java.vendor.java()
+
+	roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio)
+	roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio)
+
+	roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio)
+	roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio)
+
+	nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop)
+	nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop)
+	simulationDebug wpi.sim.enableDebug()
+
+	nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop)
+	nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop)
+	simulationRelease wpi.sim.enableRelease()
+}
+
+// Apply a specific Java toolchain to ease working on different environments.
+java {
+    toolchain {
+        languageVersion = JavaLanguageVersion.of(17)
+    }
+    sourceCompatibility = JavaVersion.VERSION_17
+	targetCompatibility = JavaVersion.VERSION_17
+}
+
+wpi.sim.addGui().defaultEnabled = true
+wpi.sim.addDriverstation()
+
+tasks.named('test') {
+    // Use JUnit Platform for unit tests.
+    useJUnitPlatform()
+}
+
+// adds dependency files to large jar
+jar {
+	from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } }
+	from sourceSets.main.allSource
+	manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(MAIN_CLASS)
+	duplicatesStrategy = DuplicatesStrategy.INCLUDE
+}
+
+// deploy jar + tasks
+deployArtifact.jarTask = jar
+wpi.java.configureExecutableTasks(jar)
+
+tasks.withType(JavaCompile) {
+	options.compilerArgs.add '-XDstringConcat=inline'
+}

lib/src/main/java/coppercore/constants/FieldConstants.java

@@ -0,0 +1,67 @@
+package constants;
+
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+
+public final class FieldConstants {
+    public static final double lengthM = 16.451;
+    public static final double widthM = 8.211;
+
+    public static final double midfieldLowThresholdM = 5.87;
+    public static final double midfieldHighThresholdM = 10.72;
+
+    public static final Rotation2d ampHeading = new Rotation2d(-Math.PI / 2);
+
+    public static final Rotation2d blueUpHeading = Rotation2d.fromRadians(0.0);
+    public static final Rotation2d blueDownHeading = Rotation2d.fromRadians(Math.PI);
+    public static final Rotation2d blueLeftHeading = Rotation2d.fromRadians(Math.PI / 2.0);
+    public static final Rotation2d blueRightHeading = Rotation2d.fromRadians(-Math.PI / 2.0);
+
+    public static final Rotation2d redUpHeading = Rotation2d.fromRadians(Math.PI);
+    public static final Rotation2d redDownHeading = Rotation2d.fromRadians(0.0);
+    public static final Rotation2d redLeftHeading = Rotation2d.fromRadians(-Math.PI / 2.0);
+    public static final Rotation2d redRightHeading = Rotation2d.fromRadians(Math.PI / 2.0);
+
+    public static final Rotation2d redSourceHeading =
+            new Rotation2d(Math.PI * 4 / 3); // 60 degrees
+    public static final Rotation2d blueSourceHeading =
+            new Rotation2d(Math.PI * 5 / 3); // 120 degrees
+
+    public static final Translation2d fieldToRedSpeaker =
+            new Translation2d(Units.inchesToMeters(652.73), Units.inchesToMeters(218.42));
+
+    public static final Translation2d fieldToBlueSpeaker =
+            new Translation2d(Units.inchesToMeters(-1.5), Units.inchesToMeters(218.42));
+
+    public static final Pose2d robotAgainstBlueSpeaker =
+            new Pose2d(1.39, 5.56, Rotation2d.fromDegrees(180));
+
+    public static final Pose2d robotAgainstRedSpeaker =
+            new Pose2d(15.19, 5.56, Rotation2d.fromDegrees(0));
+
+    public static final Pose2d robotAgainstBlueSpeakerRight =
+            new Pose2d(0.7, 4.38, Rotation2d.fromDegrees(120));
+
+    public static final Pose2d robotAgainstRedSpeakerRight =
+            new Pose2d(15.83, 6.73, Rotation2d.fromDegrees(-60));
+
+    public static final Pose2d robotAgainstBlueSpeakerLeft =
+            new Pose2d(0.7, 6.73, Rotation2d.fromDegrees(-120));
+
+    public static final Pose2d robotAgainstRedSpeakerLeft =
+            new Pose2d(15.83, 4.38, Rotation2d.fromDegrees(60));
+
+    public static final Pose2d robotAgainstBluePodium =
+            new Pose2d(2.57, 4.09, Rotation2d.fromDegrees(180));
+
+    public static final Pose2d robotAgainstRedPodium =
+            new Pose2d(13.93, 4.09, Rotation2d.fromDegrees(0));
+
+    public static final Pose2d robotAgainstBlueAmpZone =
+            new Pose2d(2.85, 7.68, Rotation2d.fromDegrees(-90));
+
+    public static final Pose2d robotAgainstRedAmpZone =
+            new Pose2d(13.74, 7.68, Rotation2d.fromDegrees(-90));
+}

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

@@ -0,0 +1,124 @@
+package coppercore.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 constants.FieldConstants;
+
+public class AllianceUtil {
+
+    public static Translation2d getFieldToSpeaker() {
+        if (!DriverStation.getAlliance().isEmpty()) {
+            switch (DriverStation.getAlliance().get()) {
+                case Blue:
+                    return FieldConstants.fieldToBlueSpeaker;
+                case Red:
+                    return FieldConstants.fieldToRedSpeaker;
+            }
+        }
+        return FieldConstants.fieldToRedSpeaker;
+    }
+
+    public static Rotation2d getAmpHeading() {
+        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:
+                    return FieldConstants.blueSourceHeading;
+                case Red:
+                    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/coppercore/utils/Deadband.java

@@ -0,0 +1,41 @@
+package coppercore.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;
+    }
+}