2025 cleanup

.gitignore

@@ -168,13 +168,17 @@ out/
 # Fleet
 .fleet
 
-
 # Simulation GUI and other tools window save file
-*-window.json
-
 networktables.json
-*.wpilog
 simgui.json
-simgui-ds.json
+*-window.json
+
+# Simulation data log directory
+logs/
+
+# Folder that has CTRE Phoenix Sim device config storage
+ctre_sim/
 
-ctre_sim
+# clangd
+/.cache
+compile_commands.json

.wpilib/wpilib_preferences.json

@@ -1,6 +1,6 @@
 {
     "enableCppIntellisense": false,
     "currentLanguage": "java",
-    "projectYear": "2024beta",
+    "projectYear": "2025beta",
     "teamNumber": 5990
 }

WPILib-License.md

@@ -1,4 +1,4 @@
-Copyright (c) 2009-2023 FIRST and other WPILib contributors
+Copyright (c) 2009-2024 FIRST and other WPILib contributors
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2024.3.2"
+    id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-1"
 }
 
 java {
@@ -88,7 +88,7 @@ dependencies {
     simulationRelease wpi.sim.enableRelease()
 
     implementation 'com.google.code.gson:gson:2.10.1'
-    implementation 'com.github.Programming-TRIGON:TRIGONLib:2024.2.2'
+    implementation 'com.github.Programming-TRIGON:TRIGONLib:2024.2.10'
 
     def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text)
     annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version"
@@ -117,4 +117,4 @@ wpi.java.configureTestTasks(test)
 // Configure string concat to always inline compile
 tasks.withType(JavaCompile) {
     options.compilerArgs.add '-XDstringConcat=inline'
-}
+}

gradle/wrapper/gradle-wrapper.properties

@@ -1,6 +1,6 @@
 distributionBase=GRADLE_USER_HOME
 distributionPath=permwrapper/dists
-distributionUrl=https\://services.gradle.org/distributions/gradle-8.4-bin.zip
+distributionUrl=https\://services.gradle.org/distributions/gradle-8.10.2-bin.zip
 networkTimeout=10000
 validateDistributionUrl=true
 zipStoreBase=GRADLE_USER_HOME

gradlew

@@ -15,6 +15,8 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 #
+# SPDX-License-Identifier: Apache-2.0
+#
 
 ##############################################################################
 #
@@ -55,7 +57,7 @@
 #       Darwin, MinGW, and NonStop.
 #
 #   (3) This script is generated from the Groovy template
-#       https://github.com/gradle/gradle/blob/HEAD/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt
+#       https://github.com/gradle/gradle/blob/HEAD/platforms/jvm/plugins-application/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt
 #       within the Gradle project.
 #
 #       You can find Gradle at https://github.com/gradle/gradle/.
@@ -84,7 +86,8 @@ done
 # shellcheck disable=SC2034
 APP_BASE_NAME=${0##*/}
 # Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036)
-APP_HOME=$( cd "${APP_HOME:-./}" > /dev/null && pwd -P ) || exit
+APP_HOME=$( cd -P "${APP_HOME:-./}" > /dev/null && printf '%s
+' "$PWD" ) || exit
 
 # Use the maximum available, or set MAX_FD != -1 to use that value.
 MAX_FD=maximum

gradlew.bat

@@ -13,6 +13,8 @@
 @rem See the License for the specific language governing permissions and
 @rem limitations under the License.
 @rem
+@rem SPDX-License-Identifier: Apache-2.0
+@rem
 
 @if "%DEBUG%"=="" @echo off
 @rem ##########################################################################
@@ -43,11 +45,11 @@ set JAVA_EXE=java.exe
 %JAVA_EXE% -version >NUL 2>&1
 if %ERRORLEVEL% equ 0 goto execute
 
-echo.
-echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
-echo.
-echo Please set the JAVA_HOME variable in your environment to match the
-echo location of your Java installation.
+echo. 1>&2
+echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 1>&2
+echo. 1>&2
+echo Please set the JAVA_HOME variable in your environment to match the 1>&2
+echo location of your Java installation. 1>&2
 
 goto fail
 
@@ -57,11 +59,11 @@ set JAVA_EXE=%JAVA_HOME%/bin/java.exe
 
 if exist "%JAVA_EXE%" goto execute
 
-echo.
-echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME%
-echo.
-echo Please set the JAVA_HOME variable in your environment to match the
-echo location of your Java installation.
+echo. 1>&2
+echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 1>&2
+echo. 1>&2
+echo Please set the JAVA_HOME variable in your environment to match the 1>&2
+echo location of your Java installation. 1>&2
 
 goto fail
 

settings.gradle

@@ -4,7 +4,7 @@ pluginManagement {
     repositories {
         mavenLocal()
         gradlePluginPortal()
-        String frcYear = '2024'
+        String frcYear = '2025'
         File frcHome
         if (OperatingSystem.current().isWindows()) {
             String publicFolder = System.getenv('PUBLIC')

src/main/java/frc/trigon/robot/RobotContainer.java

@@ -7,8 +7,8 @@
 
 import com.pathplanner.lib.auto.AutoBuilder;
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.trigon.robot.commands.Commands;
-import frc.trigon.robot.constants.CommandConstants;
+import frc.trigon.robot.commands.CommandConstants;
+import frc.trigon.robot.commands.factories.GeneralCommands;
 import frc.trigon.robot.constants.OperatorConstants;
 import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator;
 import frc.trigon.robot.subsystems.swerve.Swerve;
@@ -17,7 +17,7 @@
 public class RobotContainer {
     public static final Swerve SWERVE = new Swerve();
     public static final PoseEstimator POSE_ESTIMATOR = new PoseEstimator();
-    private LoggedDashboardChooser<Command> autoChooser;
+    static LoggedDashboardChooser<Command> autoChooser;
 
     public RobotContainer() {
         configureBindings();
@@ -42,9 +42,10 @@ private void bindDefaultCommands() {
 
     private void bindControllerCommands() {
         OperatorConstants.RESET_HEADING_TRIGGER.onTrue(CommandConstants.RESET_HEADING_COMMAND);
+        OperatorConstants.SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_TRIGGER.onTrue(CommandConstants.SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_COMMAND);
         OperatorConstants.DRIVE_FROM_DPAD_TRIGGER.whileTrue(CommandConstants.SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND);
-        OperatorConstants.TOGGLE_FIELD_AND_SELF_RELATIVE_DRIVE_TRIGGER.onTrue(Commands.getToggleFieldAndSelfRelativeDriveCommand());
-        OperatorConstants.TOGGLE_BRAKE_TRIGGER.onTrue(Commands.getToggleBrakeCommand());
+        OperatorConstants.TOGGLE_FIELD_AND_SELF_RELATIVE_DRIVE_TRIGGER.onTrue(GeneralCommands.getToggleFieldAndSelfRelativeDriveCommand());
+        OperatorConstants.TOGGLE_BRAKE_TRIGGER.onTrue(GeneralCommands.getToggleBrakeCommand());
     }
 
     private void buildAutoChooser() {

src/main/java/frc/trigon/robot/constants/CommandConstants.java → src/main/java/frc/trigon/robot/commands/CommandConstants.java

@@ -1,10 +1,11 @@
-package frc.trigon.robot.constants;
+package frc.trigon.robot.commands;
 
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import frc.trigon.robot.RobotContainer;
+import frc.trigon.robot.constants.OperatorConstants;
 import frc.trigon.robot.subsystems.ledstrip.LEDStripCommands;
 import frc.trigon.robot.subsystems.ledstrip.LEDStripConstants;
 import frc.trigon.robot.subsystems.swerve.SwerveCommands;
@@ -21,23 +22,32 @@ public class CommandConstants {
 
     public static final Command
             FIELD_RELATIVE_DRIVE_COMMAND = SwerveCommands.getOpenLoopFieldRelativeDriveCommand(
-            () -> DRIVER_CONTROLLER.getLeftY() / OperatorConstants.STICKS_SPEED_DIVIDER / calculateShiftModeValue(MINIMUM_TRANSLATION_SHIFT_POWER),
-            () -> DRIVER_CONTROLLER.getLeftX() / OperatorConstants.STICKS_SPEED_DIVIDER / calculateShiftModeValue(MINIMUM_TRANSLATION_SHIFT_POWER),
-            () -> DRIVER_CONTROLLER.getRightX() / OperatorConstants.STICKS_SPEED_DIVIDER / calculateShiftModeValue(MINIMUM_ROTATION_SHIFT_POWER)
+            () -> calculateDriveStickAxisValue(DRIVER_CONTROLLER.getLeftX()),
+            () -> calculateDriveStickAxisValue(DRIVER_CONTROLLER.getLeftY()),
+            () -> calculateRotationStickAxisValue(DRIVER_CONTROLLER.getRightX())
     ),
             SELF_RELATIVE_DRIVE_COMMAND = SwerveCommands.getOpenLoopSelfRelativeDriveCommand(
-                    () -> DRIVER_CONTROLLER.getLeftY() / OperatorConstants.STICKS_SPEED_DIVIDER / calculateShiftModeValue(MINIMUM_TRANSLATION_SHIFT_POWER),
-                    () -> DRIVER_CONTROLLER.getLeftX() / OperatorConstants.STICKS_SPEED_DIVIDER / calculateShiftModeValue(MINIMUM_TRANSLATION_SHIFT_POWER),
-                    () -> DRIVER_CONTROLLER.getRightX() / OperatorConstants.STICKS_SPEED_DIVIDER / calculateShiftModeValue(MINIMUM_ROTATION_SHIFT_POWER)
+                    () -> calculateDriveStickAxisValue(DRIVER_CONTROLLER.getLeftX()),
+                    () -> calculateDriveStickAxisValue(DRIVER_CONTROLLER.getLeftY()),
+                    () -> calculateRotationStickAxisValue(DRIVER_CONTROLLER.getRightX())
             ),
             RESET_HEADING_COMMAND = new InstantCommand(() -> RobotContainer.POSE_ESTIMATOR.resetPose(changeRotation(new MirrorablePose2d(RobotContainer.POSE_ESTIMATOR.getCurrentPose(), false), new Rotation2d()).get())),
+            SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_COMMAND = new InstantCommand(RobotContainer.POSE_ESTIMATOR::setGyroHeadingToBestSolvePNPHeading).ignoringDisable(true),
             SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND = SwerveCommands.getOpenLoopSelfRelativeDriveCommand(
                     () -> getXPowerFromPov(DRIVER_CONTROLLER.getPov()) / OperatorConstants.POV_DIVIDER / calculateShiftModeValue(MINIMUM_TRANSLATION_SHIFT_POWER),
                     () -> getYPowerFromPov(DRIVER_CONTROLLER.getPov()) / OperatorConstants.POV_DIVIDER / calculateShiftModeValue(MINIMUM_TRANSLATION_SHIFT_POWER),
                     () -> 0
             ),
             STATIC_WHITE_LED_COLOR_COMMAND = LEDStripCommands.getStaticColorCommand(Color.white, LEDStripConstants.LED_STRIPS);
 
+    public static double calculateDriveStickAxisValue(double axisValue) {
+        return axisValue / OperatorConstants.STICKS_SPEED_DIVIDER / calculateShiftModeValue(MINIMUM_TRANSLATION_SHIFT_POWER);
+    }
+
+    public static double calculateRotationStickAxisValue(double axisValue) {
+        return axisValue / OperatorConstants.STICKS_SPEED_DIVIDER / calculateShiftModeValue(MINIMUM_ROTATION_SHIFT_POWER);
+    }
+
     /**
      * The shift mode is a mode of the robot that slows down the robot relative to how much the right trigger axis is pressed.
      * This method will take the given power, and slow it down relative to how much the right trigger is pressed.

src/main/java/frc/trigon/robot/commands/LEDAutoSetupCommand.java

@@ -1,16 +1,19 @@
 package frc.trigon.robot.commands;
 
 import com.pathplanner.lib.commands.PathPlannerAuto;
+import com.pathplanner.lib.path.PathPlannerPath;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.trigon.robot.RobotContainer;
 import frc.trigon.robot.subsystems.ledstrip.LEDStripCommands;
 import frc.trigon.robot.subsystems.ledstrip.LEDStripConstants;
+import org.json.simple.parser.ParseException;
 import org.trigon.utilities.mirrorable.MirrorablePose2d;
 
 import java.awt.*;
+import java.io.IOException;
 import java.util.function.Supplier;
 
 /**
@@ -58,7 +61,13 @@ public boolean runsWhenDisabled() {
 
     private Command getUpdateAutoStartPoseCommand() {
         return new InstantCommand(
-                () -> this.autoStartPose = new MirrorablePose2d(PathPlannerAuto.getStaringPoseFromAutoFile(autoName.get()), true).get()
+                () -> {
+                    try {
+                        this.autoStartPose = new MirrorablePose2d(PathPlannerPath.fromPathFile(PathPlannerAuto.currentPathName).getStartingHolonomicPose().get(), true).get();
+                    } catch (IOException | ParseException e) {
+                        throw new RuntimeException(e);
+                    }
+                }
         ).ignoringDisable(true);
     }
 

src/main/java/frc/trigon/robot/commands/Commands.java → src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java

@@ -1,13 +1,13 @@
-package frc.trigon.robot.commands;
+package frc.trigon.robot.commands.factories;
 
 import edu.wpi.first.wpilibj2.command.*;
 import frc.trigon.robot.RobotContainer;
-import frc.trigon.robot.constants.CommandConstants;
+import frc.trigon.robot.commands.CommandConstants;
 import frc.trigon.robot.subsystems.MotorSubsystem;
 
 import java.util.function.BooleanSupplier;
 
-public class Commands {
+public class GeneralCommands {
     public static boolean IS_BRAKING = true;
 
     public static Command withoutRequirements(Command command) {
@@ -61,6 +61,10 @@ public static Command runWhen(Command command, BooleanSupplier condition) {
         return new WaitUntilCommand(condition).andThen(command);
     }
 
+    public static Command runWhen(Command command, BooleanSupplier condition, double debounceTimeSeconds) {
+        return new WaitUntilCommand(condition).andThen(new WaitCommand(debounceTimeSeconds).andThen(command.onlyIf(condition)));
+    }
+
     public static Command duplicate(Command command) {
         return new FunctionalCommand(
                 command::initialize,

src/main/java/frc/trigon/robot/constants/CameraConstants.java

@@ -1,5 +1,8 @@
 package frc.trigon.robot.constants;
 
 public class CameraConstants {
-    //todo: implement CameraConstants
+    public static final double
+            TRANSLATIONS_STD_EXPONENT = 0.02,
+            THETA_STD_EXPONENT = 0.02;
+    //TODO: implement CameraConstants
 }

src/main/java/frc/trigon/robot/constants/FieldConstants.java

@@ -1,7 +1,40 @@
 package frc.trigon.robot.constants;
 
+import edu.wpi.first.apriltag.AprilTag;
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+
+import java.io.IOException;
+import java.util.HashMap;
+
 public class FieldConstants {
+    private static final boolean SHOULD_USE_HOME_TAG_LAYOUT = false;
+    private static final Transform3d TAG_OFFSET = new Transform3d(0, 0, 0, new Rotation3d(0, 0, 0));
+    public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT;
+
+    static {
+        try {
+            APRIL_TAG_FIELD_LAYOUT = SHOULD_USE_HOME_TAG_LAYOUT ?
+                    AprilTagFieldLayout.loadFromResource("path/to/layout.json") :
+                    AprilTagFields.kDefaultField.loadAprilTagLayoutField();//TODO:Switch for year
+        } catch (IOException e) {
+            throw new RuntimeException(e);
+        }
+    }
+
+    public static final HashMap<Integer, Pose3d> TAG_ID_TO_POSE = fieldLayoutToTagIdToPoseMap();
+
     public static final double
             FIELD_LENGTH_METERS = 16.54175,
             FIELD_WIDTH_METERS = 8.02;
+
+    private static HashMap<Integer, Pose3d> fieldLayoutToTagIdToPoseMap() {
+        final HashMap<Integer, Pose3d> tagIdToPose = new HashMap<>();
+        for (AprilTag aprilTag : APRIL_TAG_FIELD_LAYOUT.getTags())
+            tagIdToPose.put(aprilTag.ID, aprilTag.pose.transformBy(TAG_OFFSET));
+        return tagIdToPose;
+    }
 }

src/main/java/frc/trigon/robot/constants/OperatorConstants.java

@@ -21,6 +21,7 @@ public class OperatorConstants {
 
     public static final Trigger
             RESET_HEADING_TRIGGER = DRIVER_CONTROLLER.y(),
+            SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_TRIGGER = OPERATOR_CONTROLLER.r().or(DRIVER_CONTROLLER.a()),
             TOGGLE_BRAKE_TRIGGER = OPERATOR_CONTROLLER.g().or(RobotController::getUserButton),
             TOGGLE_FIELD_AND_SELF_RELATIVE_DRIVE_TRIGGER = DRIVER_CONTROLLER.b(),
             DRIVE_FROM_DPAD_TRIGGER = new Trigger(() -> DRIVER_CONTROLLER.getPov() != -1);

src/main/java/frc/trigon/robot/constants/RobotConstants.java

@@ -10,7 +10,7 @@ public class RobotConstants {
             IS_REPLAY = false;
     private static final double PERIODIC_TIME_SECONDS = 0.02;
     public static final String CANIVORE_NAME = "CANivore";
-    public static final String LOGGING_PATH = IS_SIMULATION && Robot.IS_REAL ? FilesHandler.DEPLOY_PATH + "logs/" : "/media/sda1/akitlogs/";
+    public static final String LOGGING_PATH = IS_SIMULATION && !Robot.IS_REAL ? FilesHandler.DEPLOY_PATH + "logs/" : "/media/sda1/akitlogs/";
 
     public static void init() {
         RobotHardwareStats.setCurrentRobotStats(Robot.IS_REAL, IS_SIMULATION, IS_REPLAY);