Merge pull request #48 from Hemlock5712/Tweaks

Small tweaks

.vscode/settings.json

@@ -38,5 +38,5 @@
   "[java]": {
     "editor.defaultFormatter": "richardwillis.vscode-spotless-gradle"
   },
-  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable"
+  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx4G -Xms100m -Xlog:disable"
 }

WPILib-License.md

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

src/main/java/frc/robot/Constants.java

@@ -1,14 +1,8 @@
-// Copyright FRC 5712
+// Copyright (c) 2025 FRC 5712
 //
-// This program is free software; you can redistribute it and/or
-// modify it under the terms of the GNU General Public License
-// version 3 as published by the Free Software Foundation or
-// available in the root directory of this project.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU General Public License for more details.
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
 
 package frc.robot;
 
@@ -29,7 +23,6 @@
 import edu.wpi.first.units.measure.MomentOfInertia;
 import edu.wpi.first.wpilibj.RobotBase;
 import frc.robot.generated.TunerConstants;
-import frc.robot.utils.PPUtil;
 
 /**
  * This class defines the runtime mode used by AdvantageKit. The mode is always "real" when running
@@ -95,14 +88,10 @@ public static enum Mode {
   }
 
   static {
-    RobotConfig config;
-    try {
-      config = RobotConfig.fromGUISettings();
-      // Check if the GUI settings match the constants
-      PPUtil.compareConfigs(config, PP_CONFIG);
-    } catch (Exception e) {
-      PPUtil.badGUI();
-      e.printStackTrace();
+    // Checks to make sure config matches GUI values. Code should not throw as not breaking
+    if (!PP_CONFIG.hasValidConfig()) {
+      String error = "Invalid robot configuration detected in PP_CONFIG";
+      System.err.println(error);
     }
   }
 }

src/main/java/frc/robot/LimelightHelpers.java

@@ -1,4 +1,4 @@
-// LimelightHelpers v1.10 (REQUIRES LLOS 2024.9.1 OR LATER)
+// LimelightHelpers v1.11 (REQUIRES LLOS 2025.0 OR LATER)
 
 package frc.robot;
 
@@ -562,6 +562,37 @@ public String toString() {
     }
   }
 
+  /** Encapsulates the state of an internal Limelight IMU. */
+  public static class IMUData {
+    public double robotYaw = 0.0;
+    public double Roll = 0.0;
+    public double Pitch = 0.0;
+    public double Yaw = 0.0;
+    public double gyroX = 0.0;
+    public double gyroY = 0.0;
+    public double gyroZ = 0.0;
+    public double accelX = 0.0;
+    public double accelY = 0.0;
+    public double accelZ = 0.0;
+
+    public IMUData() {}
+
+    public IMUData(double[] imuData) {
+      if (imuData != null && imuData.length >= 10) {
+        this.robotYaw = imuData[0];
+        this.Roll = imuData[1];
+        this.Pitch = imuData[2];
+        this.Yaw = imuData[3];
+        this.gyroX = imuData[4];
+        this.gyroY = imuData[5];
+        this.gyroZ = imuData[6];
+        this.accelX = imuData[7];
+        this.accelY = imuData[8];
+        this.accelZ = imuData[9];
+      }
+    }
+  }
+
   private static ObjectMapper mapper;
 
   /** Print JSON Parse time to the console in milliseconds */
@@ -1251,6 +1282,25 @@ public static Pose2d getBotPose2d(String limelightName) {
     return toPose2D(result);
   }
 
+  /**
+   * Gets the current IMU data from NetworkTables. IMU data is formatted as [robotYaw, Roll, Pitch,
+   * Yaw, gyroX, gyroY, gyroZ, accelX, accelY, accelZ]. Returns all zeros if data is invalid or
+   * unavailable.
+   *
+   * @param limelightName Name/identifier of the Limelight
+   * @return IMUData object containing all current IMU data
+   */
+  public static IMUData getIMUData(String limelightName) {
+    double[] imuData = getLimelightNTDoubleArray(limelightName, "imu");
+    if (imuData == null || imuData.length < 10) {
+      System.err.printf(
+          "Invalid IMU data from %s: %s%n",
+          limelightName, imuData == null ? "null" : "insufficient length " + imuData.length);
+      return new IMUData(); // Returns object with all zeros
+    }
+    return new IMUData(imuData);
+  }
+
   /////
   /////
 
@@ -1329,6 +1379,43 @@ public static void setCropWindow(
     setLimelightNTDoubleArray(limelightName, "crop", entries);
   }
 
+  public enum IMUMode {
+    EXTERNAL(0), // Use external IMU yaw only
+    EXTERNAL_FUSED(1), // Use external IMU yaw and configure internal IMU
+    INTERNAL(2); // Use internal IMU only
+
+    private final int value;
+
+    IMUMode(int value) {
+      this.value = value;
+    }
+
+    public int getValue() {
+      return value;
+    }
+  }
+
+  /**
+   * Configures the IMU mode for MegaTag2 Localization <br>
+   * <br>
+   * EXTERNAL - Use external IMU yaw submitted via SetRobotOrientation() for MT2 localization. The
+   * internal IMU is ignored entirely. <br>
+   * <br>
+   * EXTERNAL_FUSED - Use external IMU yaw submitted via SetRobotOrientation(), and configure the
+   * LL4 internal IMU’s fused yaw to match the submitted yaw value. <br>
+   * <br>
+   * INTERNAL - Use internal IMU for MT2 localization. External imu data is ignored entirely
+   *
+   * @param limelightName Name/identifier of the Limelight
+   * @param mode IMU mode.
+   */
+  public static void SetIMUMode(String limelightName, IMUMode mode) {
+    if (mode == null) {
+      throw new IllegalArgumentException("IMU mode cannot be null");
+    }
+    setLimelightNTDouble(limelightName, "imumode_set", mode.getValue());
+  }
+
   /** Sets 3D offset point for easy 3D targeting. */
   public static void setFiducial3DOffset(
       String limelightName, double offsetX, double offsetY, double offsetZ) {

src/main/java/frc/robot/commands/DriveCommands.java

@@ -74,12 +74,12 @@ public static Command wheelRadiusCharacterization(Drive drive) {
                     () -> {
                       Angle[] positions = drive.getDrivePositions();
                       double wheelDelta = 0.0;
-                      for (int i = 0; i < 4; i++) {
+                      for (int i = 0; i < Constants.PP_CONFIG.numModules; i++) {
                         wheelDelta +=
                             Math.abs(
                                     positions[i].minus(state.positions[i]).baseUnitMagnitude()
                                         / Constants.SWERVE_MODULE_CONSTANTS.DriveMotorGearRatio)
-                                / 4.0;
+                                / Constants.PP_CONFIG.numModules;
                       }
                       double wheelRadius =
                           (state.gyroDelta * Constants.DRIVE_BASE_RADIUS) / wheelDelta;
@@ -105,7 +105,7 @@ public static Command wheelRadiusCharacterization(Drive drive) {
   }
 
   private static class WheelRadiusCharacterizationState {
-    Angle[] positions = new Angle[4];
+    Angle[] positions = new Angle[Constants.PP_CONFIG.numModules];
     Rotation2d lastAngle = Rotation2d.kZero;
     double gyroDelta = 0.0;
   }

src/main/java/frc/robot/subsystems/arm/Arm.java

@@ -1,13 +1,8 @@
-// Copyright FRC 5712
-// This program is free software; you can redistribute it and/or
-// modify it under the terms of the GNU General Public License
-// version 3 as published by the Free Software Foundation or
-// available in the root directory of this project.
+// Copyright (c) 2025 FRC 5712
 //
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU General Public License for more details.
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
 
 package frc.robot.subsystems.arm;
 
@@ -127,9 +122,13 @@ public ArmPosition getMode() {
    * @param position The desired ArmPosition
    */
   private void setArmPosition(ArmPosition position) {
-    currentCommand.cancel();
-    currentMode = position;
-    currentCommand.schedule();
+    if (currentMode != position) {
+      if (currentCommand != null) {
+        currentCommand.cancel();
+      }
+      currentMode = position;
+      currentCommand.schedule();
+    }
   }
 
   // Command that runs the appropriate routine based on the current position

src/main/java/frc/robot/subsystems/arm/ArmIO.java

@@ -1,13 +1,8 @@
-// Copyright FRC 5712
-// This program is free software; you can redistribute it and/or
-// modify it under the terms of the GNU General Public License
-// version 3 as published by the Free Software Foundation or
-// available in the root directory of this project.
+// Copyright (c) 2025 FRC 5712
 //
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU General Public License for more details.
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
 
 package frc.robot.subsystems.arm;
 

src/main/java/frc/robot/subsystems/arm/ArmIOCTRE.java

@@ -1,13 +1,8 @@
-// Copyright FRC 5712
-// This program is free software; you can redistribute it and/or
-// modify it under the terms of the GNU General Public License
-// version 3 as published by the Free Software Foundation or
-// available in the root directory of this project.
+// Copyright (c) 2025 FRC 5712
 //
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU General Public License for more details.
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
 
 package frc.robot.subsystems.arm;
 

src/main/java/frc/robot/subsystems/arm/ArmIOSIM.java

@@ -1,13 +1,8 @@
-// Copyright FRC 5712
-// This program is free software; you can redistribute it and/or
-// modify it under the terms of the GNU General Public License
-// version 3 as published by the Free Software Foundation or
-// available in the root directory of this project.
+// Copyright (c) 2025 FRC 5712
 //
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU General Public License for more details.
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
 
 package frc.robot.subsystems.arm;
 

src/main/java/frc/robot/subsystems/drive/Drive.java

@@ -1,3 +1,9 @@
+// Copyright (c) 2025 FRC 5712
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
 package frc.robot.subsystems.drive;
 
 import static edu.wpi.first.units.Units.*;
@@ -29,6 +35,7 @@
 import frc.robot.Constants;
 import frc.robot.Constants.Mode;
 import frc.robot.subsystems.vision.VisionUtil.VisionMeasurement;
+import frc.robot.utils.ArrayBuilder;
 import java.util.List;
 import java.util.function.Supplier;
 import org.littletonrobotics.junction.AutoLogOutput;
@@ -41,32 +48,23 @@
 public class Drive extends SubsystemBase {
   private final DriveIO io;
   private final DriveIOInputsAutoLogged inputs;
-  private final ModuleIOInputsAutoLogged[] modules =
-      new ModuleIOInputsAutoLogged[] {
-        new ModuleIOInputsAutoLogged(),
-        new ModuleIOInputsAutoLogged(),
-        new ModuleIOInputsAutoLogged(),
-        new ModuleIOInputsAutoLogged()
-      };
+  private final ModuleIOInputsAutoLogged[] modules = ArrayBuilder.buildModuleAutoLogeed();
 
   private final SwerveDriveKinematics kinematics =
       new SwerveDriveKinematics(Constants.SWERVE_MODULE_OFFSETS);
   private SwerveDrivePoseEstimator poseEstimator = null;
   private Trigger estimatorTrigger =
       new Trigger(() -> poseEstimator != null).and(() -> Constants.currentMode == Mode.REPLAY);
-  private SwerveModulePosition[] currentPositions =
-      new SwerveModulePosition[] {
-        new SwerveModulePosition(),
-        new SwerveModulePosition(),
-        new SwerveModulePosition(),
-        new SwerveModulePosition()
-      };
+  private SwerveModulePosition[] currentPositions = ArrayBuilder.buildSwerveModulePosition();
 
-  private Alert[] driveDisconnectedAlert = new Alert[4];
-  private Alert[] turnDisconnectedAlert = new Alert[4];
-  private Alert[] turnEncoderDisconnectedAlert = new Alert[4];
+  private Alert[] driveDisconnectedAlert =
+      ArrayBuilder.buildAlert("Disconnected drive motor on module");
+  private Alert[] turnDisconnectedAlert =
+      ArrayBuilder.buildAlert("Disconnected turn motor on module");
+  private Alert[] turnEncoderDisconnectedAlert =
+      ArrayBuilder.buildAlert("Disconnected turn encoder on module");
 
-  private Alert gyroDisconnectedAlert;
+  private Alert gyroDisconnectedAlert = new Alert("Gyro Disconnected", AlertType.kError);
 
   /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */
   private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero;
@@ -147,26 +145,9 @@ public Drive(DriveIO io) {
     this.io = io;
     inputs = new DriveIOInputsAutoLogged();
 
-    configureAlerts();
     configureAutoBuilder();
   }
 
-  private void configureAlerts() {
-    gyroDisconnectedAlert = new Alert("Gyro Disconnected", AlertType.kError);
-
-    for (int i = 0; i < modules.length; i++) {
-      driveDisconnectedAlert[i] =
-          new Alert(
-              "Disconnected drive motor on module " + Integer.toString(i) + ".", AlertType.kError);
-      turnDisconnectedAlert[i] =
-          new Alert(
-              "Disconnected turn motor on module " + Integer.toString(i) + ".", AlertType.kError);
-      turnEncoderDisconnectedAlert[i] =
-          new Alert(
-              "Disconnected turn encoder on module " + Integer.toString(i) + ".", AlertType.kError);
-    }
-  }
-
   private void configureAutoBuilder() {
     AutoBuilder.configure(
         this::getPose, // Supplier of current robot pose
@@ -296,8 +277,8 @@ public Rotation2d getOperatorForwardDirection() {
   }
 
   public Angle[] getDrivePositions() {
-    Angle[] values = new Angle[4];
-    for (int i = 0; i < 4; i++) {
+    Angle[] values = new Angle[Constants.PP_CONFIG.numModules];
+    for (int i = 0; i < values.length; i++) {
       values[i] = modules[i].drivePosition;
     }
     return values;