From 8fd550b67fbd87cf5ea1d509ad6cb03b170964b3 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Fri, 30 Oct 2026 11:18:06 -0700 Subject: [PATCH 01/10] Upgrade Az-RBSI to WPILib 2027 alpha 8 Merge the WPILib-imported project into the existing Az-RBSI tree while retaining its project-specific build tasks, dependencies, sources, and editor configuration. Remove the imported copy after bringing its four new vendor dependencies and required configuration changes into this tree. Move GradleRIO to 2027.0.0-alpha-8 and switch Systemcore deployment to the importer's application/classpath model. Replace the ShadowJar deployment with the ordinary application jar, retaining source, vendordep, and Gradle backup contents in that jar. Keep generated BuildConstants, the event-branch deploy commit task, Spotless, Lombok, protobuf, explicit Jackson and AdvantageKit dependencies, the custom test source set, and JUnit 6.1.3. Configure JNI debug settings using the alpha-8 GradleRIO properties. Adopt the imported settings.gradle resolution of the 2027_alpha7 WPILib install, including the Linux XDG and macOS .wpilib paths. Update WPILib preferences to team 2486, preserve the original .gitignore, retain the direct Java launch entry, rename the WPILib debug launch to Systemcore, and add the imported Java debug CodeLens setting. Refresh AdvantageKit, Commands V2, PathPlanner, REVLib, and Phoenix 6 vendor dependencies; replace Phoenix6 26.50.0-alpha-1 with 26.70.0-alpha-2. Add the imported AprilTag, YAGSL Commands V2, and YAMS Commands V2 vendor files. Preserve the original-only Autopilot, URCL, and PhotonVision vendor files while aligning their WPILib year with 2027_alpha7. Keep the imported changes to the seven robot Java files selected during the merge. Adapt robot code to the 2027 WPILib APIs: start the robot with a supplier, move geometry zero constants and SysId/debouncer enum values to their new names, use Optional rotation angles and the current HID trigger accessors, map Systemcore CAN bus names to CANPort, and update CTRE generated drivetrain constants to use CANBus networks. Move RBSI Pair use to org.wpilib.util.Pair. Replace the local Alert implementation with WPILib alerts and map existing severity levels to the built-in levels; give per-instance drivetrain and module alerts unique IDs. Publish the field through Telemetry and migrate the autonomous and drive-style choices to LoggedNetworkChooser. Bridge the current PhotonVision alpha-2 binary against the removed AprilTag and Pair class names. The local AprilTagFieldLayout adapter wraps the new org.wpilib.fields.Field and reads the existing field-layout JSON schema. Document the temporary bridge and its removal steps for a future compatible PhotonVision release. Complete the REVLib 2027 Spark migration. Convert raw encoder rotations and RPM to the radians and radians/second expected by ModuleIO, convert drive velocity and steering setpoints back to controller units, and scale PID and feedforward gains to preserve their physical units. Apply identical scaling to high-rate odometry queues and decode CANcoder queue values as rotations. Remove configuration calls for conversion factors, hall sensor averaging, and custom position-wrapping range that REVLib removed. Document physical validation still needed for wheel direction, steering wraparound, odometry, velocity control, SysId, and possible gain retuning with the new firmware. Update tests for alpha-8 HAL, rotation, Optional, and CANPort APIs. Add focused Spark unit-conversion and steering wraparound tests. Verify with ./gradlew test using the installed WPILib JDK and git diff --check; both pass. Co-authored-by: Codex --- .vscode/launch.json | 2 +- .vscode/settings.json | 1 + .wpilib/wpilib_preferences.json | 4 +- build.gradle | 30 ++-- doc/ALPHA8-REV-MIGRATION.md | 17 +++ doc/PHOTONVISION-COMPAT.md | 7 + settings.gradle | 14 +- src/main/java/first/Main.java | 2 +- src/main/java/frc/robot/Constants.java | 29 ++-- src/main/java/frc/robot/FieldState.java | 7 +- src/main/java/frc/robot/RobotContainer.java | 84 +++++------ .../frc/robot/commands/AutopilotCommands.java | 2 +- .../frc/robot/commands/DriveCommands.java | 4 +- .../generated/COMPBOTTunerConstants.java | 4 +- .../generated/DEVBOT1TunerConstants.java | 4 +- .../generated/DEVBOT2TunerConstants.java | 4 +- .../accelerometer/Accelerometer.java | 2 +- .../frc/robot/subsystems/drive/Drive.java | 44 +++--- .../subsystems/drive/DriveSimPhysics.java | 5 +- .../frc/robot/subsystems/drive/Module.java | 22 ++- .../frc/robot/subsystems/drive/ModuleIO.java | 4 +- .../subsystems/drive/ModuleIOBlended.java | 27 ++-- .../robot/subsystems/drive/ModuleIOSim.java | 5 +- .../robot/subsystems/drive/ModuleIOSpark.java | 72 ++++----- .../drive/ModuleIOSparkCANcoder.java | 66 ++++----- .../subsystems/drive/ModuleIOTalonFX.java | 6 +- .../subsystems/drive/ModuleIOTalonFXS.java | 6 +- .../subsystems/drive/SparkSwerveUnits.java | 44 ++++++ .../subsystems/flywheel_example/Flywheel.java | 5 +- .../flywheel_example/FlywheelIOSim.java | 3 +- .../flywheel_example/FlywheelIOSpark.java | 11 +- .../java/frc/robot/subsystems/imu/Imu.java | 6 +- .../java/frc/robot/subsystems/imu/ImuIO.java | 4 +- .../robot/subsystems/imu/ImuIOPigeon2.java | 2 +- .../frc/robot/subsystems/imu/ImuIOSim.java | 2 +- .../frc/robot/subsystems/vision/VisionIO.java | 2 +- .../vision/VisionIOPhotonVision.java | 6 +- src/main/java/frc/robot/util/Alert.java | 138 ------------------ .../ConcurrentTimeInterpolatableBuffer.java | 2 +- src/main/java/frc/robot/util/GeomUtil.java | 10 +- .../java/frc/robot/util/LocalADStarAK.java | 2 +- .../java/frc/robot/util/OverrideSwitches.java | 3 +- .../java/frc/robot/util/RBSIController.java | 24 +-- .../java/frc/robot/util/RBSIPowerMonitor.java | 16 +- src/main/java/org/wpilib/math/util/Pair.java | 8 + .../org/wpilib/vision/apriltag/AprilTag.java | 14 ++ .../vision/apriltag/AprilTagFieldLayout.java | 86 +++++++++++ .../vision/apriltag/AprilTagFields.java | 14 ++ src/test/CurrentLimitTests.java | 2 +- src/test/FusedCANcoderTests.java | 2 +- src/test/LatencyCompensationTests.java | 2 +- src/test/frc/robot/RobotConstantsTest.java | 7 +- .../robot/commands/DriveCommandsTests.java | 8 +- .../subsystems/drive/DriveSubsystemTests.java | 30 ++-- .../drive/SparkSwerveUnitsTest.java | 44 ++++++ .../flywheel_example/FlywheelSysIdTests.java | 8 +- src/test/frc/robot/util/UtilTests.java | 2 +- vendordeps/AdvantageKit.json | 11 +- vendordeps/AprilTag.json | 91 ++++++++++++ vendordeps/Autopilot.json | 2 +- vendordeps/CommandsV2.json | 8 +- vendordeps/PathplannerLibSystemCoreAlpha.json | 8 +- ...a-1.json => Phoenix6-26.70.0-alpha-2.json} | 64 ++++---- vendordeps/REVLib.json | 20 +-- vendordeps/URCL.json | 2 +- vendordeps/photonlib.json | 2 +- vendordeps/yagsl_commands2.json | 40 +++++ vendordeps/yams_commands2.json | 56 +++++++ 68 files changed, 798 insertions(+), 487 deletions(-) create mode 100644 doc/ALPHA8-REV-MIGRATION.md create mode 100644 doc/PHOTONVISION-COMPAT.md create mode 100644 src/main/java/frc/robot/subsystems/drive/SparkSwerveUnits.java delete mode 100644 src/main/java/frc/robot/util/Alert.java create mode 100644 src/main/java/org/wpilib/math/util/Pair.java create mode 100644 src/main/java/org/wpilib/vision/apriltag/AprilTag.java create mode 100644 src/main/java/org/wpilib/vision/apriltag/AprilTagFieldLayout.java create mode 100644 src/main/java/org/wpilib/vision/apriltag/AprilTagFields.java create mode 100644 src/test/frc/robot/subsystems/drive/SparkSwerveUnitsTest.java create mode 100644 vendordeps/AprilTag.json rename vendordeps/{Phoenix6-26.50.0-alpha-1.json => Phoenix6-26.70.0-alpha-2.json} (89%) create mode 100644 vendordeps/yagsl_commands2.json create mode 100644 vendordeps/yams_commands2.json diff --git a/.vscode/launch.json b/.vscode/launch.json index e9b8148..1894b3a 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -16,7 +16,7 @@ }, { "type": "wpilib", - "name": "WPILib roboRIO Debug", + "name": "WPILib Systemcore Debug", "request": "launch", "desktop": false } diff --git a/.vscode/settings.json b/.vscode/settings.json index 034d131..e0391a3 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -74,5 +74,6 @@ "[java]": { "editor.defaultFormatter": "richardwillis.vscode-spotless-gradle" }, + "java.debug.settings.enableRunDebugCodeLens": false, "java.dependency.enableDependencyCheckup": false } diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index f413a08..d48bf76 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { "enableCppIntellisense": false, "currentLanguage": "java", - "projectYear": "2027_alpha5", - "teamNumber": 9986 + "projectYear": "2027_alpha7", + "teamNumber": 2486 } diff --git a/build.gradle b/build.gradle index f33e305..2ef0f84 100644 --- a/build.gradle +++ b/build.gradle @@ -1,7 +1,7 @@ plugins { id "java" - id "org.wpilib.GradleRIO" version "2027.0.0-alpha-6" - id "com.gradleup.shadow" version "9.3.0" + id "application" + id "org.wpilib.GradleRIO" version "2027.0.0-alpha-8" id "com.peterabeles.gversion" version "1.11.0" id "com.diffplug.spotless" version "8.10.4" id "io.freefair.lombok" version "9.8.2" @@ -43,6 +43,7 @@ deploy { // getTargetTypeClass is a shortcut to get the class type using a string wpilibJava(getArtifactTypeClass('WPILibJavaArtifact')) { + debugJni = false } // Static files artifact @@ -59,9 +60,8 @@ deploy { def deployArtifact = deploy.targets.systemcore.artifacts.wpilibJava -// Set to true to use debug for all targets including JNI, which will drastically impact -// performance. -wpi.java.debugJni = false +// Set to true to use debug for simulation including JNI. +wpi.java.runSimWithDebugJni = false // Set this to true to enable desktop support. def includeDesktopSupport = true @@ -107,23 +107,19 @@ test { wpi.sim.addGui().defaultEnabled = true wpi.sim.addDriverstation() -// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat/shaded jar') -// in order to make them all available at runtime and merging service files to make JSON work. -// Also adding the manifest so WPILib knows where to look for our Robot Class. -shadowJar { - mergeServiceFiles() +application.mainClass = ROBOT_MAIN_CLASS + +deployArtifact.configureApplication(application) +wpi.java.configureApplication(application) +wpi.java.configureTestTasks(test) + +// Keep source and build inputs with the application jar for deployed backups. +jar { from('src') { into 'backup/src' } from('vendordeps') { into 'backup/vendordeps' } from('build.gradle') { into 'backup' } - manifest org.wpilib.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) - duplicatesStrategy = DuplicatesStrategy.INCLUDE } -// Configure jar and deploy tasks -deployArtifact.jarTask = shadowJar -wpi.java.configureExecutableTasks(shadowJar) -wpi.java.configureTestTasks(test) - // Configure string concat to always inline compile tasks.withType(JavaCompile) { options.compilerArgs.add '-XDstringConcat=inline' diff --git a/doc/ALPHA8-REV-MIGRATION.md b/doc/ALPHA8-REV-MIGRATION.md new file mode 100644 index 0000000..86e2413 --- /dev/null +++ b/doc/ALPHA8-REV-MIGRATION.md @@ -0,0 +1,17 @@ +# REVLib 2027 migration + +REVLib 2027 removed SPARK encoder conversion factors and the configurable +position wrapping range. `ModuleIOSpark`, `ModuleIOSparkCANcoder`, and +`ModuleIOBlended` now use raw encoder rotations/RPM at the controller boundary. +`SparkSwerveUnits` converts readings back to radians and radians/second for +`ModuleIO`, converts closed-loop requests to rotations/RPM, and scales the +SPARK gains to keep the previous physical units. Drive and steering odometry +queues use the same conversions as periodic telemetry. The CANcoder queue in +`ModuleIOSparkCANcoder` is decoded as rotations, matching its periodic data. + +The build and unit tests pass. Before operating a physical Spark module, +verify wheel direction, steering shortest-path behavior across zero, odometry, +velocity control, and SysId. The 2027 firmware's filtering behavior also +changed, so steering and drive gains may need tuning after those checks. + +REV's release notes: https://github.com/wpilibsuite/SystemCoreTesting/blob/main/REV.md diff --git a/doc/PHOTONVISION-COMPAT.md b/doc/PHOTONVISION-COMPAT.md new file mode 100644 index 0000000..d1e1656 --- /dev/null +++ b/doc/PHOTONVISION-COMPAT.md @@ -0,0 +1,7 @@ +# Temporary PhotonVision alpha-2 compatibility bridge + +PhotonVision `v2027.0.0-alpha-2` still links to WPILib APIs removed in the 2027 alpha7/alpha8 libraries. The temporary types under `src/main/java/org/wpilib/vision/apriltag/` wrap the new `org.wpilib.fields.Field` for PhotonVision's old `AprilTagFieldLayout`, `AprilTag`, and `AprilTagFields` references. `src/main/java/org/wpilib/math/util/Pair.java` supplies PhotonVision's old binary name; RBSI source uses `org.wpilib.util.Pair` directly. + +The layout bridge reads the seven existing deploy JSON files, which use the old `tags`/`field` schema, and constructs a new `Field` with the same tag poses and dimensions. It is limited to the methods referenced by this project and PhotonVision alpha-2. + +**Remove after a compatible PhotonVision release is installed:** delete the four compatibility types, migrate `FieldConstants` and callers to `org.wpilib.fields.Field`, and convert or replace the deploy layout JSON files if the new `Field.loadFromFile` schema is used. Verify `VisionSystemSim`, `PhotonCameraSim`, and the full test suite before deleting this note. diff --git a/settings.gradle b/settings.gradle index e2626a8..a17bda0 100644 --- a/settings.gradle +++ b/settings.gradle @@ -2,18 +2,26 @@ import org.gradle.internal.os.OperatingSystem pluginManagement { repositories { - String wpilibYear = '2027_alpha5' + String wpilibYear = '2027_alpha7' File wpilibHome - if (OperatingSystem.current().isWindows()) { + def os = OperatingSystem.current() + if (os.isWindows()) { String publicFolder = System.getenv('PUBLIC') if (publicFolder == null) { publicFolder = "C:\\Users\\Public" } def homeRoot = new File(publicFolder, "wpilib") wpilibHome = new File(homeRoot, wpilibYear) + } else if (os.isLinux()) { + String xdgDataHome = System.getenv('XDG_DATA_HOME') + if (xdgDataHome == null || xdgDataHome.trim().isEmpty() || !new File(xdgDataHome).isAbsolute()) { + xdgDataHome = new File(System.getProperty("user.home"), ".local/share").getPath() + } + def homeRoot = new File(xdgDataHome, "wpilib") + wpilibHome = new File(homeRoot, wpilibYear) } else { def userFolder = System.getProperty("user.home") - def homeRoot = new File(userFolder, "wpilib") + def homeRoot = new File(userFolder, ".wpilib") wpilibHome = new File(homeRoot, wpilibYear) } def wpilibHomeMaven = new File(wpilibHome, 'maven') diff --git a/src/main/java/first/Main.java b/src/main/java/first/Main.java index 8fd1dab..1d57820 100644 --- a/src/main/java/first/Main.java +++ b/src/main/java/first/Main.java @@ -20,6 +20,6 @@ private Main() {} *

If you change your main robot class, change the parameter type. */ public static void main(String... args) { - RobotBase.startRobot(frc.robot.Robot.class); + RobotBase.startRobot(frc.robot.Robot::new); } } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b64019b..e1fe7c9 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -28,7 +28,6 @@ import frc.robot.FieldConstants.AprilTagLayoutType; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.SwerveConstants; -import frc.robot.util.Alert; import frc.robot.util.RBSIEnum.AutoType; import frc.robot.util.RBSIEnum.CTREPro; import frc.robot.util.RBSIEnum.DriveStyle; @@ -49,6 +48,8 @@ import org.wpilib.math.system.DCMotor; import org.wpilib.math.util.Units; import org.wpilib.units.measure.Mass; +import org.wpilib.util.Alert; +import org.wpilib.util.Alert.Level; /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean @@ -132,15 +133,15 @@ public static final class RobotConstants { public static final Rotation3d kRioOrientation = switch (getRobot()) { case COMPBOT -> new Rotation3d(0, 0, Units.degreesToRadians(-90)); - case DEVBOT1, DEVBOT2 -> Rotation3d.kZero; - default -> Rotation3d.kZero; + case DEVBOT1, DEVBOT2 -> Rotation3d.ZERO; + default -> Rotation3d.ZERO; }; // IMU can be one of Pigeon2 or NavX public static final Rotation3d kIMUOrientation = switch (getRobot()) { - case COMPBOT -> Rotation3d.kZero; - case DEVBOT1, DEVBOT2 -> Rotation3d.kZero; - default -> Rotation3d.kZero; + case COMPBOT -> Rotation3d.ZERO; + case DEVBOT1, DEVBOT2 -> Rotation3d.ZERO; + default -> Rotation3d.ZERO; }; } @@ -189,13 +190,13 @@ public static final class CANBuses { .toArray(String[]::new); /** REV's 2027 constructors use a numbered SystemCore port rather than a Phoenix bus name. */ - public static int revPort(String bus) { + public static org.wpilib.hardware.bus.CANPort revPort(String bus) { return switch (bus) { - case SYSTEMCORE_0 -> 0; - case SYSTEMCORE_1 -> 1; - case SYSTEMCORE_2 -> 2; - case SYSTEMCORE_3 -> 3; - case SYSTEMCORE_4 -> 4; + case SYSTEMCORE_0 -> org.wpilib.hardware.bus.CANPort.CAN_S0; + case SYSTEMCORE_1 -> org.wpilib.hardware.bus.CANPort.CAN_S1; + case SYSTEMCORE_2 -> org.wpilib.hardware.bus.CANPort.CAN_S2; + case SYSTEMCORE_3 -> org.wpilib.hardware.bus.CANPort.CAN_S3; + case SYSTEMCORE_4 -> org.wpilib.hardware.bus.CANPort.CAN_S4; default -> throw new IllegalArgumentException("REV device requires a SystemCore CAN bus: " + bus); }; @@ -615,7 +616,9 @@ public static final class DeployConstants { public static RobotType getRobot() { if (!disableHAL && RobotBase.isReal() && robotType == RobotType.SIMBOT) { new Alert( - "Invalid robot selected, using competition robot as default.", Alert.AlertType.ERROR) + "invalidRobotSelected", + "Invalid robot selected, using competition robot as default.", + Level.HIGH) .set(true); robotType = RobotType.COMPBOT; } diff --git a/src/main/java/frc/robot/FieldState.java b/src/main/java/frc/robot/FieldState.java index f4ce0e9..51f2a74 100644 --- a/src/main/java/frc/robot/FieldState.java +++ b/src/main/java/frc/robot/FieldState.java @@ -17,10 +17,11 @@ package frc.robot; -import frc.robot.util.Alert; import org.wpilib.driverstation.Alliance; import org.wpilib.driverstation.MatchState; import org.wpilib.driverstation.RobotState; +import org.wpilib.util.Alert; +import org.wpilib.util.Alert.Level; public final class FieldState { private static final double TRANSITION_END_SECONDS = 130.0; @@ -29,7 +30,9 @@ public final class FieldState { private static final Alert missingHubDataAlert = new Alert( - "No HUB data from FMS! HUB is listed as ACTIVE! Driver BEWARE!", Alert.AlertType.WARNING); + "missingHubData", + "No HUB data from FMS! HUB is listed as ACTIVE! Driver BEWARE!", + Level.MEDIUM); private FieldState() {} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8a4a71c..0bab0d1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -43,8 +43,6 @@ import frc.robot.subsystems.vision.VisionIOLimelight; import frc.robot.subsystems.vision.VisionIOPhotonVision; import frc.robot.subsystems.vision.VisionIOPhotonVisionSim; -import frc.robot.util.Alert; -import frc.robot.util.Alert.AlertType; import frc.robot.util.LoggedTunableNumber; import frc.robot.util.OverrideSwitches; import frc.robot.util.RBSICANBusRegistry; @@ -57,7 +55,7 @@ import java.util.Arrays; import java.util.List; import java.util.Set; -import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; +import org.littletonrobotics.junction.networktables.LoggedNetworkChooser; import org.photonvision.PhotonCamera; import org.photonvision.simulation.PhotonCameraSim; import org.photonvision.simulation.VisionSystemSim; @@ -72,6 +70,8 @@ import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.geometry.Transform2d; import org.wpilib.system.Filesystem; +import org.wpilib.util.Alert; +import org.wpilib.util.Alert.Level; /** This is the location for defining robot hardware, commands, and controller button bindings. */ public class RobotContainer { @@ -114,14 +114,14 @@ public class RobotContainer { /** Dashboard inputs ***************************************************** */ // AutoChoosers for both supported path planning types - private final LoggedDashboardChooser autoChooserPathPlanner; + private final LoggedNetworkChooser autoChooserPathPlanner; // TODO(2027): Re-enable Choreo chooser/factory when ChoreoLib supports 2027 WPILib. - // private final LoggedDashboardChooser autoChooserChoreo; + // private final LoggedNetworkChooser autoChooserChoreo; // private final AutoFactory autoFactoryChoreo; - private final LoggedDashboardChooser driveStyle = - new LoggedDashboardChooser<>("Drive Style"); + private final LoggedNetworkChooser driveStyle = + new LoggedNetworkChooser<>("Drive Style"); // Input estimated battery capacity (if full, use printed value) private final LoggedTunableNumber batteryCapacity = @@ -131,7 +131,7 @@ public class RobotContainer { new LoggedTunableNumber("Flywheel Speed", 1500.0); // Alerts - private final Alert aprilTagLayoutAlert = new Alert("", AlertType.INFO); + private final Alert aprilTagLayoutAlert = new Alert("aprilTagLayout", "", Level.LOW); /** * Constructor for the Robot Container. This container holds subsystems, operator interface @@ -235,7 +235,7 @@ public RobotContainer() { case PATHPLANNER: autoChooserPathPlanner = - new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); + new LoggedNetworkChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); // Set the others to null // TODO(2027): Restore Choreo fields when ChoreoLib supports 2027 WPILib. // autoChooserChoreo = null; @@ -251,9 +251,9 @@ public RobotContainer() { // m_drivebase::followTrajectory, // true, // m_drivebase); - // autoChooserChoreo = new LoggedDashboardChooser<>("Choreo Auto Choices"); - // autoChooserChoreo.addDefaultOption("Nothing", Commands.none()); - // autoChooserChoreo.addOption("twoPieceAuto", twoPieceAuto().cmd()); + // autoChooserChoreo = new LoggedNetworkChooser<>("Choreo Auto Choices"); + // autoChooserChoreo.addDefault("Nothing", Commands.none()); + // autoChooserChoreo.add("twoPieceAuto", twoPieceAuto().cmd()); autoChooserPathPlanner = null; break; @@ -265,11 +265,10 @@ public RobotContainer() { // Get drive style from the Dashboard Chooser. The constant controls the boot default, and the // dashboard chooser lets teams swap stick layouts between drivers without recompiling. - driveStyle.addDefaultOption( - OperatorConstants.kDriveStyle.name(), OperatorConstants.kDriveStyle); + driveStyle.addDefault(OperatorConstants.kDriveStyle.name(), OperatorConstants.kDriveStyle); for (DriveStyle style : DriveStyle.values()) { if (style != OperatorConstants.kDriveStyle) { - driveStyle.addOption(style.name(), style); + driveStyle.add(style.name(), style); } } @@ -341,7 +340,7 @@ private void configureBindings() { new Transform2d( OperatorConstants.kAutopilotDemoXOffsetMeters, 0.0, - Rotation2d.kZero)); + Rotation2d.ZERO)); // Alternatively, you could define a pose in a separate module and call it here. // @@ -380,6 +379,7 @@ private void configureBindings() { // Double-press the A button on Joystick3 to run the CameraSweepEvaluator // Use WPILib's built-in double-press binding joystick3 + .getHID() .button(1) .multiPress(2, 0.2) .onTrue( @@ -466,50 +466,50 @@ public Vision getVision() { private void definesysIdRoutines() { if (Constants.getAutoType() == AutoType.PATHPLANNER) { // Drivebase characterization - autoChooserPathPlanner.addOption( + autoChooserPathPlanner.add( "Drive Wheel Radius Characterization", DriveCommands.wheelRadiusCharacterization(m_drivebase)); - autoChooserPathPlanner.addOption( + autoChooserPathPlanner.add( "Drive Simple FF Characterization", DriveCommands.feedforwardCharacterization(m_drivebase)); - autoChooserPathPlanner.addOption( + autoChooserPathPlanner.add( "Drive SysId (Quasistatic Forward)", - m_drivebase.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); - autoChooserPathPlanner.addOption( + m_drivebase.sysIdQuasistatic(SysIdRoutine.Direction.FORWARD)); + autoChooserPathPlanner.add( "Drive SysId (Quasistatic Reverse)", - m_drivebase.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); - autoChooserPathPlanner.addOption( + m_drivebase.sysIdQuasistatic(SysIdRoutine.Direction.REVERSE)); + autoChooserPathPlanner.add( "Drive SysId (Dynamic Forward)", - m_drivebase.sysIdDynamic(SysIdRoutine.Direction.kForward)); - autoChooserPathPlanner.addOption( + m_drivebase.sysIdDynamic(SysIdRoutine.Direction.FORWARD)); + autoChooserPathPlanner.add( "Drive SysId (Dynamic Reverse)", - m_drivebase.sysIdDynamic(SysIdRoutine.Direction.kReverse)); + m_drivebase.sysIdDynamic(SysIdRoutine.Direction.REVERSE)); // Example Flywheel SysId Characterization - autoChooserPathPlanner.addOption( + autoChooserPathPlanner.add( "Flywheel SysId Voltage (Quasistatic Forward)", - m_flywheel.sysIdVoltageQuasistatic(SysIdRoutine.Direction.kForward)); - autoChooserPathPlanner.addOption( + m_flywheel.sysIdVoltageQuasistatic(SysIdRoutine.Direction.FORWARD)); + autoChooserPathPlanner.add( "Flywheel SysId Voltage (Quasistatic Reverse)", - m_flywheel.sysIdVoltageQuasistatic(SysIdRoutine.Direction.kReverse)); - autoChooserPathPlanner.addOption( + m_flywheel.sysIdVoltageQuasistatic(SysIdRoutine.Direction.REVERSE)); + autoChooserPathPlanner.add( "Flywheel SysId Voltage (Dynamic Forward)", - m_flywheel.sysIdVoltageDynamic(SysIdRoutine.Direction.kForward)); - autoChooserPathPlanner.addOption( + m_flywheel.sysIdVoltageDynamic(SysIdRoutine.Direction.FORWARD)); + autoChooserPathPlanner.add( "Flywheel SysId Voltage (Dynamic Reverse)", - m_flywheel.sysIdVoltageDynamic(SysIdRoutine.Direction.kReverse)); - autoChooserPathPlanner.addOption( + m_flywheel.sysIdVoltageDynamic(SysIdRoutine.Direction.REVERSE)); + autoChooserPathPlanner.add( "Flywheel SysId Duty Cycle (Quasistatic Forward)", - m_flywheel.sysIdDutyCycleQuasistatic(SysIdRoutine.Direction.kForward)); - autoChooserPathPlanner.addOption( + m_flywheel.sysIdDutyCycleQuasistatic(SysIdRoutine.Direction.FORWARD)); + autoChooserPathPlanner.add( "Flywheel SysId Duty Cycle (Quasistatic Reverse)", - m_flywheel.sysIdDutyCycleQuasistatic(SysIdRoutine.Direction.kReverse)); - autoChooserPathPlanner.addOption( + m_flywheel.sysIdDutyCycleQuasistatic(SysIdRoutine.Direction.REVERSE)); + autoChooserPathPlanner.add( "Flywheel SysId Duty Cycle (Dynamic Forward)", - m_flywheel.sysIdDutyCycleDynamic(SysIdRoutine.Direction.kForward)); - autoChooserPathPlanner.addOption( + m_flywheel.sysIdDutyCycleDynamic(SysIdRoutine.Direction.FORWARD)); + autoChooserPathPlanner.add( "Flywheel SysId Duty Cycle (Dynamic Reverse)", - m_flywheel.sysIdDutyCycleDynamic(SysIdRoutine.Direction.kReverse)); + m_flywheel.sysIdDutyCycleDynamic(SysIdRoutine.Direction.REVERSE)); } } diff --git a/src/main/java/frc/robot/commands/AutopilotCommands.java b/src/main/java/frc/robot/commands/AutopilotCommands.java index 02e5252..bede4eb 100644 --- a/src/main/java/frc/robot/commands/AutopilotCommands.java +++ b/src/main/java/frc/robot/commands/AutopilotCommands.java @@ -178,7 +178,7 @@ private static Command autopilotToTarget(Drive drive, APTarget target) { return Commands.run( () -> { - ChassisVelocities robotRelativeSpeeds = drive.getChassisSpeeds(); + ChassisVelocities robotRelativeSpeeds = drive.getChassisVelocities(); Pose2d pose = drive.getPose(); boolean atTarget = AutoConstants.kAutopilot.atTarget(pose, target); diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 1483e91..a3920cb 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -189,7 +189,7 @@ static Translation2d getLinearVelocity(double x, double y) { double linearMagnitude = MathUtil.applyDeadband(Math.hypot(x, y), OperatorConstants.kJoystickDeadband); if (linearMagnitude <= 0.0) { - return Translation2d.kZero; + return Translation2d.ZERO; } // Square magnitude for more precise control @@ -391,7 +391,7 @@ public static Command wheelRadiusCharacterization(Drive drive) { private static class WheelRadiusCharacterizationState { double[] positions = new double[4]; - Rotation2d lastAngle = Rotation2d.kZero; + Rotation2d lastAngle = Rotation2d.ZERO; double gyroDelta = 0.0; } } diff --git a/src/main/java/frc/robot/generated/COMPBOTTunerConstants.java b/src/main/java/frc/robot/generated/COMPBOTTunerConstants.java index c0989ad..d99014a 100644 --- a/src/main/java/frc/robot/generated/COMPBOTTunerConstants.java +++ b/src/main/java/frc/robot/generated/COMPBOTTunerConstants.java @@ -75,7 +75,7 @@ public class COMPBOTTunerConstants { private static final Pigeon2Configuration pigeonConfigs = null; // Phoenix Tuner generation bus. Runtime wiring is selected in Constants.DrivebaseConstants. - public static final CANBus kCANBus = new CANBus("can_s2", "./logs/example.hoot"); + public static final CANBus kCANBus = new CANBus("can_s2"); // Theoretical free speed (m/s) at 12 V applied output; // This needs to be tuned to your individual robot @@ -103,7 +103,7 @@ public class COMPBOTTunerConstants { public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() - .withCANBusName(kCANBus.getName()) + .withNetwork(kCANBus) .withPigeon2Id(kPigeonId) .withPigeon2Configs(pigeonConfigs); diff --git a/src/main/java/frc/robot/generated/DEVBOT1TunerConstants.java b/src/main/java/frc/robot/generated/DEVBOT1TunerConstants.java index 21c1a75..c7f02dc 100755 --- a/src/main/java/frc/robot/generated/DEVBOT1TunerConstants.java +++ b/src/main/java/frc/robot/generated/DEVBOT1TunerConstants.java @@ -75,7 +75,7 @@ public class DEVBOT1TunerConstants { private static final Pigeon2Configuration pigeonConfigs = null; // Phoenix Tuner generation bus. Runtime wiring is selected in Constants.DrivebaseConstants. - public static final CANBus kCANBus = new CANBus("can_s2", "./logs/example.hoot"); + public static final CANBus kCANBus = new CANBus("can_s2"); // Theoretical free speed (m/s) at 12 V applied output; // This needs to be tuned to your individual robot @@ -103,7 +103,7 @@ public class DEVBOT1TunerConstants { public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() - .withCANBusName(kCANBus.getName()) + .withNetwork(kCANBus) .withPigeon2Id(kPigeonId) .withPigeon2Configs(pigeonConfigs); diff --git a/src/main/java/frc/robot/generated/DEVBOT2TunerConstants.java b/src/main/java/frc/robot/generated/DEVBOT2TunerConstants.java index 94b9424..6171a98 100644 --- a/src/main/java/frc/robot/generated/DEVBOT2TunerConstants.java +++ b/src/main/java/frc/robot/generated/DEVBOT2TunerConstants.java @@ -71,7 +71,7 @@ public class DEVBOT2TunerConstants { private static final Pigeon2Configuration pigeonConfigs = null; // Phoenix Tuner generation bus. Runtime wiring is selected in Constants.DrivebaseConstants. - public static final CANBus kCANBus = new CANBus("can_s2", "./logs/example.hoot"); + public static final CANBus kCANBus = new CANBus("can_s2"); // Theoretical free speed (m/s) at 12 V applied output; // This needs to be tuned to your individual robot @@ -99,7 +99,7 @@ public class DEVBOT2TunerConstants { public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() - .withCANBusName(kCANBus.getName()) + .withNetwork(kCANBus) .withPigeon2Id(kPigeonId) .withPigeon2Configs(pigeonConfigs); diff --git a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java index 29273d2..f66e4df 100644 --- a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java +++ b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java @@ -82,7 +82,7 @@ public void rbsiPeriodic() { Translation3d rioJerkThisLoop = prevRioAcc == null - ? Translation3d.kZero + ? Translation3d.ZERO : rioAcc.minus(prevRioAcc).div(Constants.kLoopPeriodSecs); prevRioAcc = rioAcc; diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index d559585..ad13876 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -30,8 +30,6 @@ import frc.robot.Constants.DrivebaseConstants; import frc.robot.Constants.RobotConstants; import frc.robot.subsystems.imu.Imu; -import frc.robot.util.Alert; -import frc.robot.util.Alert.AlertType; import frc.robot.util.ConcurrentTimeInterpolatableBuffer; import frc.robot.util.LocalADStarAK; import frc.robot.util.MathUtil; @@ -42,6 +40,7 @@ import frc.robot.util.TimedPose; import java.util.Optional; import java.util.OptionalDouble; +import java.util.concurrent.atomic.AtomicInteger; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; import org.littletonrobotics.junction.AutoLogOutput; @@ -65,7 +64,9 @@ import org.wpilib.math.kinematics.SwerveModuleVelocity; import org.wpilib.math.trajectory.TrapezoidProfile; import org.wpilib.smartdashboard.Field2d; -import org.wpilib.smartdashboard.SmartDashboard; +import org.wpilib.telemetry.Telemetry; +import org.wpilib.util.Alert; +import org.wpilib.util.Alert.Level; /** * Drive subsystem (RBSISubsystem) @@ -74,6 +75,8 @@ * The odometry is updated from both the swerve modules and (optionally) the vision subsystem. */ public class Drive extends RBSISubsystem { + private static final AtomicInteger nextAlertInstance = new AtomicInteger(); + private final int alertInstance = nextAlertInstance.getAndIncrement(); // Declare Hardware private final Imu imu; @@ -92,9 +95,15 @@ public class Drive extends RBSISubsystem { // Declare an alert private final Alert gyroDisconnectedAlert = - new Alert("Disconnected gyro, using kinematics as fallback.", AlertType.ERROR); + new Alert( + "gyroDisconnected" + alertInstance, + "Disconnected gyro, using kinematics as fallback.", + Level.HIGH); private final Alert pathPlannerStartPoseAlert = - new Alert("PathPlanner auto start pose is outside the allowed radius.", AlertType.ERROR); + new Alert( + "pathPlannerStartPose" + alertInstance, + "PathPlanner auto start pose is outside the allowed radius.", + Level.HIGH); private boolean pathPlannerStartBlocked = false; private PathPlannerStartAction pendingPathPlannerStartAction; private Pose2d pendingPathPlannerStartPose; @@ -118,7 +127,7 @@ enum PathPlannerStartAction { new SwerveModulePosition() }; private SwerveDrivePoseEstimator m_PoseEstimator = - new SwerveDrivePoseEstimator(kinematics, Rotation2d.kZero, lastModulePositions, Pose2d.kZero); + new SwerveDrivePoseEstimator(kinematics, Rotation2d.ZERO, lastModulePositions, Pose2d.ZERO); // Declare PID controller and siumulation physics private ProfiledPIDController angleController; @@ -236,7 +245,7 @@ public Drive(Imu imu) { AutoBuilder.configure( this::getPose, this::resetPoseFromPathPlanner, - this::getChassisSpeeds, + this::getChassisVelocities, (speeds, feedforwards) -> runVelocity(speeds), new PPHolonomicDriveController( new PIDConstants( @@ -285,14 +294,13 @@ public Drive(Imu imu) { (state) -> Logger.recordOutput("Drive/SysIdState", state.toString())), new SysIdRoutine.Mechanism( (voltage) -> runCharacterization(voltage.in(Volts)), null, this)); - - SmartDashboard.putData("Field", field); } /************************************************************************* */ /** Periodic function that is called each cycle by the command scheduler */ @Override public void rbsiPeriodic() { + Telemetry.log("Field", field); // The only function of the drive periodic() is to stop the modules if the DriverStation is // disabled. @@ -381,7 +389,7 @@ public void stopWithX() { Translation2d[] moduleTranslations = getModuleTranslations(); Rotation2d[] headings = new Rotation2d[moduleTranslations.length]; for (int i = 0; i < moduleTranslations.length; i++) { - headings[i] = moduleTranslations[i].getAngle(); + headings[i] = moduleTranslations[i].getAngle().orElse(Rotation2d.ZERO); } kinematics.resetHeadings(headings); stop(); @@ -411,7 +419,7 @@ public void runVelocity(ChassisVelocities speeds) { // Log unoptimized setpoints and setpoint speeds Logger.recordOutput("SwerveStates/Setpoints", setpointStates); - Logger.recordOutput("SwerveChassisSpeeds/Setpoints", discreteSpeeds); + Logger.recordOutput("SwerveChassisVelocities/Setpoints", discreteSpeeds); // Send setpoints to modules for (int i = 0; i < modules.length; i++) { @@ -585,8 +593,8 @@ SwerveModulePosition[] getModulePositions() { } /** Returns the measured chassis speeds of the robot. */ - @AutoLogOutput(key = "SwerveChassisSpeeds/Measured") - public ChassisVelocities getChassisSpeeds() { + @AutoLogOutput(key = "SwerveChassisVelocities/Measured") + public ChassisVelocities getChassisVelocities() { return kinematics.toChassisVelocities(getModuleStates()); } @@ -625,10 +633,10 @@ private Rotation2d getRawGyroHeading() { * *

+X = field forward +Y = field left CCW+ = counterclockwise */ - @AutoLogOutput(key = "SwerveChassisSpeeds/FieldMeasured") + @AutoLogOutput(key = "SwerveChassisVelocities/FieldMeasured") public ChassisVelocities getFieldRelativeSpeeds() { // Robot-relative measured speeds from modules - ChassisVelocities robotRelative = getChassisSpeeds(); + ChassisVelocities robotRelative = getChassisVelocities(); // Convert to field-relative using authoritative yaw return robotRelative.toFieldRelative(getHeading()); } @@ -864,7 +872,7 @@ private void resetPoseFromPathPlanner(Pose2d pose) { public void zeroHeadingForAlliance() { imu.zeroYaw( MatchState.getAlliance().orElse(Alliance.BLUE) == Alliance.BLUE - ? Rotation2d.kZero + ? Rotation2d.ZERO : Rotation2d.k180deg); resetHeadingController(); markPoseReset(TimeUtil.now()); @@ -872,7 +880,7 @@ public void zeroHeadingForAlliance() { /** Zeros the gyro regardless of the alliance */ public void zeroHeading() { - imu.zeroYaw(Rotation2d.kZero); + imu.zeroYaw(Rotation2d.ZERO); resetHeadingController(); markPoseReset(TimeUtil.now()); } @@ -1109,7 +1117,7 @@ public void resetOdometry(Pose2d pose) { // TODO(2027): Re-enable these Choreo trajectory followers when ChoreoLib supports 2027 WPILib. // public void choreoController(Pose2d pose, SwerveSample sample) { - // var targetSpeeds = sample.getChassisSpeeds(); + // var targetSpeeds = sample.getChassisVelocities(); // targetSpeeds.vx += m_pathXController.calculate(pose.getX(), sample.x); // targetSpeeds.vy += m_pathYController.calculate(pose.getY(), sample.y); // targetSpeeds.omega += diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSimPhysics.java b/src/main/java/frc/robot/subsystems/drive/DriveSimPhysics.java index c2f05af..a5069c0 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSimPhysics.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSimPhysics.java @@ -17,7 +17,6 @@ package frc.robot.subsystems.drive; -import frc.robot.util.MathUtil; import org.wpilib.math.geometry.Pose2d; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.geometry.Translation2d; @@ -33,7 +32,7 @@ public class DriveSimPhysics { /* ---------------- Physical state ---------------- */ - private Pose2d pose = Pose2d.kZero; + private Pose2d pose = Pose2d.ZERO; private double omegaRadPerSec = 0.0; private Translation2d linearAccel = new Translation2d(0, 0); private Translation2d prevVelocity = new Translation2d(0, 0); @@ -82,7 +81,7 @@ public void update(SwerveModuleVelocity[] moduleStates, double dtSeconds) { // Simple torque model with damping double torque = - MathUtil.clamp(commandedOmega * moiKgMetersSq / dtSeconds, -maxTorqueNm, maxTorqueNm); + Math.clamp(commandedOmega * moiKgMetersSq / dtSeconds, -maxTorqueNm, maxTorqueNm); // Stronger angular damping to simulate motor friction torque -= omegaRadPerSec * ANGULAR_DAMPING; diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index a516e0d..ecf1e22 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -12,13 +12,14 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusCode; import frc.robot.Constants.DrivebaseConstants; -import frc.robot.util.Alert; -import frc.robot.util.Alert.AlertType; +import java.util.concurrent.atomic.AtomicInteger; import org.littletonrobotics.junction.Logger; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.kinematics.SwerveModulePosition; import org.wpilib.math.kinematics.SwerveModuleVelocity; import org.wpilib.math.util.Units; +import org.wpilib.util.Alert; +import org.wpilib.util.Alert.Level; /** * Module API Class @@ -27,6 +28,7 @@ * the robot. This is not a true subsystem, but an abstraction layer. */ public class Module { + private static final AtomicInteger nextAlertInstance = new AtomicInteger(); private static final SwerveModulePosition[] EMPTY_ODOMETRY_POSITIONS = new SwerveModulePosition[0]; @@ -45,16 +47,22 @@ public class Module { public Module(ModuleIO io, int index) { this.io = io; this.index = index; + int alertInstance = nextAlertInstance.getAndIncrement(); driveDisconnectedAlert = new Alert( - "Disconnected drive motor on module " + Integer.toString(index) + ".", AlertType.ERROR); + "module" + index + "DriveDisconnected" + alertInstance, + "Disconnected drive motor on module " + index + ".", + Level.HIGH); turnDisconnectedAlert = new Alert( - "Disconnected turn motor on module " + Integer.toString(index) + ".", AlertType.ERROR); + "module" + index + "TurnDisconnected" + alertInstance, + "Disconnected turn motor on module " + index + ".", + Level.HIGH); turnEncoderDisconnectedAlert = new Alert( - "Disconnected turn encoder on module " + Integer.toString(index) + ".", - AlertType.ERROR); + "module" + index + "TurnEncoderDisconnected" + alertInstance, + "Disconnected turn encoder on module " + index + ".", + Level.HIGH); } /************************************************************************* */ @@ -125,7 +133,7 @@ public void runSetpoint(SwerveModuleVelocity state) { /** Runs the module with the specified output while controlling to zero degrees. */ public void runCharacterization(double output) { io.setDriveOpenLoop(output); - io.setTurnPosition(Rotation2d.kZero); + io.setTurnPosition(Rotation2d.ZERO); } /** Disables all outputs to motors. */ diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java index 7943317..dbeb98d 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -29,8 +29,8 @@ public static class ModuleIOInputs { public boolean turnConnected = false; public boolean turnEncoderConnected = false; - public Rotation2d turnAbsolutePosition = Rotation2d.kZero; - public Rotation2d turnPosition = Rotation2d.kZero; + public Rotation2d turnAbsolutePosition = Rotation2d.ZERO; + public Rotation2d turnPosition = Rotation2d.ZERO; public double turnVelocityRadPerSec = 0.0; public double turnAppliedVolts = 0.0; public double turnCurrentAmps = 0.0; diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java index bbfe5a5..1a9c1ec 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java @@ -40,7 +40,6 @@ import com.revrobotics.spark.config.SparkMaxConfig; import frc.robot.Constants; import frc.robot.Constants.DrivebaseConstants; -import frc.robot.util.MathUtil; import frc.robot.util.PhoenixUtil; import frc.robot.util.RBSICANBusRegistry; import frc.robot.util.RBSIEnum.CTREPro; @@ -109,11 +108,11 @@ public class ModuleIOBlended implements ModuleIO { // Connection debouncers private final Debouncer driveConnectedDebounce = - new Debouncer(0.5, Debouncer.DebounceType.kFalling); + new Debouncer(0.5, Debouncer.DebounceType.FALLING); private final Debouncer turnConnectedDebounce = - new Debouncer(0.5, Debouncer.DebounceType.kFalling); + new Debouncer(0.5, Debouncer.DebounceType.FALLING); private final Debouncer turnEncoderConnectedDebounce = - new Debouncer(0.5, Debouncer.DebounceType.kFalling); + new Debouncer(0.5, Debouncer.DebounceType.FALLING); // Config private final TalonFXConfiguration driveConfig = new TalonFXConfiguration(); @@ -229,19 +228,15 @@ public ModuleIOBlended(int module) { .idleMode(IdleMode.kBrake) .smartCurrentLimit((int) SwerveConstants.kSteerCurrentLimit) .voltageCompensation(DrivebaseConstants.kNominalVoltage); - turnConfig - .absoluteEncoder - .inverted(constants.EncoderInverted) - .positionConversionFactor(SwerveConstants.turnEncoderPositionFactor) - .velocityConversionFactor(SwerveConstants.turnEncoderVelocityFactor) - .averageDepth(2); + turnConfig.absoluteEncoder.inverted(constants.EncoderInverted).averageDepth(2); turnConfig .closedLoop .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) .positionWrappingEnabled(true) - .positionWrappingInputRange( - SwerveConstants.turnPIDMinInput, SwerveConstants.turnPIDMaxInput) - .pid(DrivebaseConstants.kSteerP, 0.0, DrivebaseConstants.kSteerD) + .pid( + SparkSwerveUnits.turnPositionGain(DrivebaseConstants.kSteerP), + 0.0, + SparkSwerveUnits.turnPositionGain(DrivebaseConstants.kSteerD)) .feedForward .kV(0.0) .kS(DrivebaseConstants.kSteerS); @@ -480,10 +475,8 @@ public void setDriveVelocity(double velocityRadPerSec) { @Override public void setTurnPosition(Rotation2d rotation) { double setpoint = - MathUtil.inputModulus( - rotation.plus(Rotation2d.fromRotations(constants.EncoderOffset)).getRadians(), - SwerveConstants.turnPIDMinInput, - SwerveConstants.turnPIDMaxInput); + SparkSwerveUnits.turnSetpointRotations( + rotation, Rotation2d.fromRotations(constants.EncoderOffset)); turnController.setSetpoint(setpoint, ControlType.kPosition); } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java index ea13a50..0c111c9 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -10,7 +10,6 @@ package frc.robot.subsystems.drive; import frc.robot.Constants; -import frc.robot.util.MathUtil; import frc.robot.util.TimeUtil; import org.wpilib.math.controller.PIDController; import org.wpilib.math.geometry.Rotation2d; @@ -81,8 +80,8 @@ public void simulationPeriodic() { } // Apply voltages - driveSim.setInputVoltage(MathUtil.clamp(driveAppliedVolts, -12.0, 12.0)); - turnSim.setInputVoltage(MathUtil.clamp(turnAppliedVolts, -12.0, 12.0)); + driveSim.setInputVoltage(Math.clamp(driveAppliedVolts, -12.0, 12.0)); + turnSim.setInputVoltage(Math.clamp(turnAppliedVolts, -12.0, 12.0)); // Advance physics driveSim.update(Constants.kLoopPeriodSecs); diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java index e449848..642c024 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java @@ -27,7 +27,6 @@ import com.revrobotics.spark.config.SparkMaxConfig; import frc.robot.Constants; import frc.robot.Constants.DrivebaseConstants; -import frc.robot.util.MathUtil; import frc.robot.util.SparkUtil; import java.util.Arrays; import java.util.Queue; @@ -63,9 +62,9 @@ public class ModuleIOSpark implements ModuleIO { // Connection debouncers private final Debouncer driveConnectedDebounce = - new Debouncer(0.5, Debouncer.DebounceType.kFalling); + new Debouncer(0.5, Debouncer.DebounceType.FALLING); private final Debouncer turnConnectedDebounce = - new Debouncer(0.5, Debouncer.DebounceType.kFalling); + new Debouncer(0.5, Debouncer.DebounceType.FALLING); // Values used for calculating feedforward from kS, kV, and kA private double lastVelocityRotPerSec = 0.0; @@ -81,7 +80,7 @@ public ModuleIOSpark(int module) { case 1 -> new Rotation2d(SwerveConstants.kFREncoderOffset); case 2 -> new Rotation2d(SwerveConstants.kBLEncoderOffset); case 3 -> new Rotation2d(SwerveConstants.kBREncoderOffset); - default -> Rotation2d.kZero; + default -> Rotation2d.ZERO; }; driveSpark = new SparkFlex( @@ -132,18 +131,15 @@ public ModuleIOSpark(int module) { .idleMode(IdleMode.kBrake) .smartCurrentLimit((int) SwerveConstants.kDriveCurrentLimit) .voltageCompensation(DrivebaseConstants.kNominalVoltage); - driveConfig - .encoder - .positionConversionFactor(SwerveConstants.driveEncoderPositionFactor) - .velocityConversionFactor(SwerveConstants.driveEncoderVelocityFactor) - .uvwMeasurementPeriod(10) - .uvwAverageDepth(2); driveConfig .closedLoop .feedbackSensor(FeedbackSensor.kPrimaryEncoder) - .pid(DrivebaseConstants.kDriveP, 0.0, DrivebaseConstants.kDriveD) + .pid( + SparkSwerveUnits.driveVelocityGain(DrivebaseConstants.kDriveP), + 0.0, + SparkSwerveUnits.driveVelocityGain(DrivebaseConstants.kDriveD)) .feedForward - .kV(DrivebaseConstants.kDriveV) + .kV(SparkSwerveUnits.driveVelocityGain(DrivebaseConstants.kDriveV)) .kS(DrivebaseConstants.kDriveS); driveConfig .signals @@ -172,19 +168,15 @@ public ModuleIOSpark(int module) { .idleMode(IdleMode.kBrake) .smartCurrentLimit((int) SwerveConstants.kSteerCurrentLimit) .voltageCompensation(DrivebaseConstants.kNominalVoltage); - turnConfig - .absoluteEncoder - .inverted(turnEncoderInverted) - .positionConversionFactor(SwerveConstants.turnEncoderPositionFactor) - .velocityConversionFactor(SwerveConstants.turnEncoderVelocityFactor) - .averageDepth(2); + turnConfig.absoluteEncoder.inverted(turnEncoderInverted).averageDepth(2); turnConfig .closedLoop .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) .positionWrappingEnabled(true) - .positionWrappingInputRange( - SwerveConstants.turnPIDMinInput, SwerveConstants.turnPIDMaxInput) - .pid(DrivebaseConstants.kSteerP, 0.0, DrivebaseConstants.kSteerD) + .pid( + SparkSwerveUnits.turnPositionGain(DrivebaseConstants.kSteerP), + 0.0, + SparkSwerveUnits.turnPositionGain(DrivebaseConstants.kSteerD)) .feedForward .kV(0.0) .kS(DrivebaseConstants.kSteerS); @@ -211,10 +203,15 @@ public ModuleIOSpark(int module) { timestampQueue = SparkOdometryThread.getInstance().makeTimestampQueue(); drivePositionQueue = SparkOdometryThread.getInstance() - .registerSignal(driveSpark, () -> driveEncoder.getPosition().get(0.0)); + .registerSignal( + driveSpark, + () -> SparkSwerveUnits.drivePositionRadians(driveEncoder.getPosition().get(0.0))); turnPositionQueue = SparkOdometryThread.getInstance() - .registerSignal(turnSpark, () -> turnEncoder.getPosition().get(0.0)); + .registerSignal( + turnSpark, + () -> + turnEncoder.getPosition().get(0.0) * SwerveConstants.turnEncoderPositionFactor); } @Override @@ -222,9 +219,14 @@ public void updateInputs(ModuleIOInputs inputs) { // Drive inputs SparkUtil.sparkStickyFault = false; SparkUtil.ifOk( - driveSpark, driveEncoder::getPosition, (value) -> inputs.drivePositionRad = value); + driveSpark, + driveEncoder::getPosition, + (value) -> inputs.drivePositionRad = SparkSwerveUnits.drivePositionRadians(value)); SparkUtil.ifOk( - driveSpark, driveEncoder::getVelocity, (value) -> inputs.driveVelocityRadPerSec = value); + driveSpark, + driveEncoder::getVelocity, + (value) -> + inputs.driveVelocityRadPerSec = SparkSwerveUnits.driveVelocityRadiansPerSecond(value)); SparkUtil.ifOk( driveSpark, new DoubleSupplier[] { @@ -240,9 +242,12 @@ public void updateInputs(ModuleIOInputs inputs) { SparkUtil.ifOk( turnSpark, turnEncoder::getPosition, - (value) -> inputs.turnPosition = new Rotation2d(value).minus(zeroRotation)); + (value) -> inputs.turnPosition = SparkSwerveUnits.turnPosition(value, zeroRotation)); SparkUtil.ifOk( - turnSpark, turnEncoder::getVelocity, (value) -> inputs.turnVelocityRadPerSec = value); + turnSpark, + turnEncoder::getVelocity, + (value) -> + inputs.turnVelocityRadPerSec = SparkSwerveUnits.turnVelocityRadiansPerSecond(value)); SparkUtil.ifOk( turnSpark, new DoubleSupplier[] { @@ -276,8 +281,9 @@ public void updateInputs(ModuleIOInputs inputs) { for (int i = 0; i < sampleCount; i++) { final Double t = timestampQueue.poll(); - final Double drivePosRad = drivePositionQueue.poll(); // already rad in your existing code - final Double turnPosRad = turnPositionQueue.poll(); // rad in your existing code + final Double drivePosRad = + drivePositionQueue.poll(); // converted to wheel radians at sampling + final Double turnPosRad = turnPositionQueue.poll(); // converted to radians at sampling if (t == null || drivePosRad == null || turnPosRad == null) { inputs.odometryTimestamps = Arrays.copyOf(outTs, i); @@ -348,7 +354,7 @@ public void setDriveVelocity(double velocityRadPerSec) { double scaledFFVolts = nominalFFVolts * DrivebaseConstants.kNominalVoltage / busVoltage; driveController.setSetpoint( - velocityRadPerSec, + SparkSwerveUnits.driveVelocityRpm(velocityRadPerSec), ControlType.kVelocity, ClosedLoopSlot.kSlot0, scaledFFVolts, @@ -368,11 +374,7 @@ public void setDriveVelocity(double velocityRadPerSec) { */ @Override public void setTurnPosition(Rotation2d rotation) { - double setpoint = - MathUtil.inputModulus( - rotation.plus(zeroRotation).getRadians(), - SwerveConstants.turnPIDMinInput, - SwerveConstants.turnPIDMaxInput); + double setpoint = SparkSwerveUnits.turnSetpointRotations(rotation, zeroRotation); turnController.setSetpoint(setpoint, ControlType.kPosition); } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java index fb416ff..86c753b 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java @@ -30,7 +30,6 @@ import com.revrobotics.spark.config.SparkMaxConfig; import frc.robot.Constants; import frc.robot.Constants.DrivebaseConstants; -import frc.robot.util.MathUtil; import frc.robot.util.RBSICANBusRegistry; import frc.robot.util.SparkUtil; import java.util.Arrays; @@ -81,11 +80,11 @@ public class ModuleIOSparkCANcoder implements ModuleIO { // Connection debouncers private final Debouncer driveConnectedDebounce = - new Debouncer(0.5, Debouncer.DebounceType.kFalling); + new Debouncer(0.5, Debouncer.DebounceType.FALLING); private final Debouncer turnConnectedDebounce = - new Debouncer(0.5, Debouncer.DebounceType.kFalling); + new Debouncer(0.5, Debouncer.DebounceType.FALLING); private final Debouncer turnEncoderConnectedDebounce = - new Debouncer(0.5, Debouncer.DebounceType.kFalling); + new Debouncer(0.5, Debouncer.DebounceType.FALLING); // Values used for calculating feedforward from kS, kV, and kA private double lastVelocityRotPerSec = 0.0; @@ -104,7 +103,7 @@ public ModuleIOSparkCANcoder(int module) { case 1 -> new Rotation2d(SwerveConstants.kFREncoderOffset); case 2 -> new Rotation2d(SwerveConstants.kBLEncoderOffset); case 3 -> new Rotation2d(SwerveConstants.kBREncoderOffset); - default -> Rotation2d.kZero; + default -> Rotation2d.ZERO; }; driveSpark = new SparkFlex( @@ -164,18 +163,15 @@ public ModuleIOSparkCANcoder(int module) { .idleMode(IdleMode.kBrake) .smartCurrentLimit((int) SwerveConstants.kDriveCurrentLimit) .voltageCompensation(DrivebaseConstants.kNominalVoltage); - driveConfig - .encoder - .positionConversionFactor(SwerveConstants.driveEncoderPositionFactor) - .velocityConversionFactor(SwerveConstants.driveEncoderVelocityFactor) - .uvwMeasurementPeriod(10) - .uvwAverageDepth(2); driveConfig .closedLoop .feedbackSensor(FeedbackSensor.kPrimaryEncoder) - .pid(DrivebaseConstants.kDriveP, 0.0, DrivebaseConstants.kDriveD) + .pid( + SparkSwerveUnits.driveVelocityGain(DrivebaseConstants.kDriveP), + 0.0, + SparkSwerveUnits.driveVelocityGain(DrivebaseConstants.kDriveD)) .feedForward - .kV(DrivebaseConstants.kDriveV) + .kV(SparkSwerveUnits.driveVelocityGain(DrivebaseConstants.kDriveV)) .kS(DrivebaseConstants.kDriveS); driveConfig .signals @@ -201,19 +197,15 @@ public ModuleIOSparkCANcoder(int module) { .idleMode(IdleMode.kBrake) .smartCurrentLimit((int) SwerveConstants.kSteerCurrentLimit) .voltageCompensation(DrivebaseConstants.kNominalVoltage); - turnConfig - .absoluteEncoder - .inverted(turnEncoderInverted) - .positionConversionFactor(SwerveConstants.turnEncoderPositionFactor) - .velocityConversionFactor(SwerveConstants.turnEncoderVelocityFactor) - .averageDepth(2); + turnConfig.absoluteEncoder.inverted(turnEncoderInverted).averageDepth(2); turnConfig .closedLoop .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) .positionWrappingEnabled(true) - .positionWrappingInputRange( - SwerveConstants.turnPIDMinInput, SwerveConstants.turnPIDMaxInput) - .pid(DrivebaseConstants.kSteerP, 0.0, DrivebaseConstants.kSteerD) + .pid( + SparkSwerveUnits.turnPositionGain(DrivebaseConstants.kSteerP), + 0.0, + SparkSwerveUnits.turnPositionGain(DrivebaseConstants.kSteerD)) .feedForward .kV(0.0) .kS(DrivebaseConstants.kSteerS); @@ -248,7 +240,9 @@ public ModuleIOSparkCANcoder(int module) { timestampQueue = SparkOdometryThread.getInstance().makeTimestampQueue(); drivePositionQueue = SparkOdometryThread.getInstance() - .registerSignal(driveSpark, () -> driveEncoder.getPosition().get(0.0)); + .registerSignal( + driveSpark, + () -> SparkSwerveUnits.drivePositionRadians(driveEncoder.getPosition().get(0.0))); turnPositionQueue = SparkOdometryThread.getInstance() .registerSignal( @@ -280,9 +274,14 @@ public void updateInputs(ModuleIOInputs inputs) { // Drive inputs (Spark) SparkUtil.sparkStickyFault = false; SparkUtil.ifOk( - driveSpark, driveEncoder::getPosition, (value) -> inputs.drivePositionRad = value); + driveSpark, + driveEncoder::getPosition, + (value) -> inputs.drivePositionRad = SparkSwerveUnits.drivePositionRadians(value)); SparkUtil.ifOk( - driveSpark, driveEncoder::getVelocity, (value) -> inputs.driveVelocityRadPerSec = value); + driveSpark, + driveEncoder::getVelocity, + (value) -> + inputs.driveVelocityRadPerSec = SparkSwerveUnits.driveVelocityRadiansPerSecond(value)); SparkUtil.ifOk( driveSpark, new DoubleSupplier[] { @@ -328,10 +327,11 @@ public void updateInputs(ModuleIOInputs inputs) { for (int i = 0; i < sampleCount; i++) { final Double t = timestampQueue.poll(); - final Double drivePosRad = drivePositionQueue.poll(); // already rad in your existing code - final Double turnPosRad = turnPositionQueue.poll(); // rad, then minus zeroRotation below + final Double drivePosRad = + drivePositionQueue.poll(); // converted to wheel radians at sampling + final Double turnPosRot = turnPositionQueue.poll(); // CANcoder mechanism rotations - if (t == null || drivePosRad == null || turnPosRad == null) { + if (t == null || drivePosRad == null || turnPosRot == null) { inputs.odometryTimestamps = Arrays.copyOf(outTs, i); inputs.odometryDrivePositionsRad = Arrays.copyOf(outDriveRad, i); inputs.odometryTurnPositions = Arrays.copyOf(outTurn, i); @@ -340,7 +340,7 @@ public void updateInputs(ModuleIOInputs inputs) { outTs[i] = t.doubleValue(); outDriveRad[i] = drivePosRad.doubleValue(); - outTurn[i] = new Rotation2d(turnPosRad.doubleValue()).minus(zeroRotation); + outTurn[i] = Rotation2d.fromRotations(turnPosRot.doubleValue()); } inputs.odometryTimestamps = outTs; @@ -406,7 +406,7 @@ public void setDriveVelocity(double velocityRadPerSec) { double scaledFFVolts = nominalFFVolts * DrivebaseConstants.kNominalVoltage / busVoltage; driveController.setSetpoint( - velocityRadPerSec, + SparkSwerveUnits.driveVelocityRpm(velocityRadPerSec), ControlType.kVelocity, ClosedLoopSlot.kSlot0, scaledFFVolts, @@ -426,11 +426,7 @@ public void setDriveVelocity(double velocityRadPerSec) { */ @Override public void setTurnPosition(Rotation2d rotation) { - double setpoint = - MathUtil.inputModulus( - rotation.plus(zeroRotation).getRadians(), - SwerveConstants.turnPIDMinInput, - SwerveConstants.turnPIDMaxInput); + double setpoint = SparkSwerveUnits.turnSetpointRotations(rotation, zeroRotation); turnController.setSetpoint(setpoint, ControlType.kPosition); } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index ca6e787..12e9790 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -98,11 +98,11 @@ public class ModuleIOTalonFX implements ModuleIO { // Connection debouncers private final Debouncer driveConnectedDebounce = - new Debouncer(0.5, Debouncer.DebounceType.kFalling); + new Debouncer(0.5, Debouncer.DebounceType.FALLING); private final Debouncer turnConnectedDebounce = - new Debouncer(0.5, Debouncer.DebounceType.kFalling); + new Debouncer(0.5, Debouncer.DebounceType.FALLING); private final Debouncer turnEncoderConnectedDebounce = - new Debouncer(0.5, Debouncer.DebounceType.kFalling); + new Debouncer(0.5, Debouncer.DebounceType.FALLING); // Config private final TalonFXConfiguration driveConfig = new TalonFXConfiguration(); diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java index 8b6a162..e374119 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java @@ -122,11 +122,11 @@ // // Connection debouncers // private final Debouncer driveConnectedDebounce = -// new Debouncer(0.5, Debouncer.DebounceType.kFalling); +// new Debouncer(0.5, Debouncer.DebounceType.FALLING); // private final Debouncer turnConnectedDebounce = -// new Debouncer(0.5, Debouncer.DebounceType.kFalling); +// new Debouncer(0.5, Debouncer.DebounceType.FALLING); // private final Debouncer turnEncoderConnectedDebounce = -// new Debouncer(0.5, Debouncer.DebounceType.kFalling); +// new Debouncer(0.5, Debouncer.DebounceType.FALLING); // // Config // private final TalonFXSConfiguration driveConfig = new TalonFXSConfiguration(); diff --git a/src/main/java/frc/robot/subsystems/drive/SparkSwerveUnits.java b/src/main/java/frc/robot/subsystems/drive/SparkSwerveUnits.java new file mode 100644 index 0000000..7b5f091 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/SparkSwerveUnits.java @@ -0,0 +1,44 @@ +package frc.robot.subsystems.drive; + +import frc.robot.util.MathUtil; +import org.wpilib.math.geometry.Rotation2d; + +/** Unit conversions for REVLib 2027 SPARK encoders and closed-loop controllers. */ +final class SparkSwerveUnits { + private SparkSwerveUnits() {} + + // REVLib 2027 exposes raw motor rotations/RPM and absolute encoder rotations/RPM. + static double drivePositionRadians(double motorRotations) { + return motorRotations * SwerveConstants.driveEncoderPositionFactor; + } + + static double driveVelocityRadiansPerSecond(double motorRpm) { + return motorRpm * SwerveConstants.driveEncoderVelocityFactor; + } + + static double driveVelocityRpm(double wheelRadiansPerSecond) { + return wheelRadiansPerSecond / SwerveConstants.driveEncoderVelocityFactor; + } + + static Rotation2d turnPosition(double absoluteRotations, Rotation2d zeroRotation) { + return Rotation2d.fromRotations(absoluteRotations).minus(zeroRotation); + } + + static double turnVelocityRadiansPerSecond(double absoluteRpm) { + return absoluteRpm * SwerveConstants.turnEncoderVelocityFactor; + } + + static double turnSetpointRotations(Rotation2d rotation, Rotation2d zeroRotation) { + return MathUtil.inputModulus(rotation.plus(zeroRotation).getRotations(), 0.0, 1.0); + } + + // Maintain the old response as the controller's input changes from rad/s to RPM. + static double driveVelocityGain(double gainPerRadianPerSecond) { + return gainPerRadianPerSecond * SwerveConstants.driveEncoderVelocityFactor; + } + + // Maintain the old response as the controller's input changes from radians to rotations. + static double turnPositionGain(double gainPerRadian) { + return gainPerRadian * SwerveConstants.turnEncoderPositionFactor; + } +} diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java b/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java index 7c3e99e..fbe369a 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java @@ -15,7 +15,6 @@ import static org.wpilib.units.Units.Volts; import frc.robot.Constants; -import frc.robot.util.MathUtil; import frc.robot.util.RBSISubsystem; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -72,7 +71,7 @@ public void runVolts(double volts) { */ public void runDutyCycleForSysIdVolts(double volts) { double batteryVoltage = RobotController.getBatteryVoltage(); - io.setPercent(batteryVoltage > 0.0 ? MathUtil.clamp(volts / batteryVoltage, -1.0, 1.0) : 0.0); + io.setPercent(batteryVoltage > 0.0 ? Math.clamp(volts / batteryVoltage, -1.0, 1.0) : 0.0); } /** Run closed loop at the specified velocity. */ @@ -171,6 +170,6 @@ private void recordSysIdState(String routineName, SysIdRoutine.State state) { } private static String directionLabel(SysIdRoutine.Direction direction) { - return direction == SysIdRoutine.Direction.kForward ? "Forward" : "Reverse"; + return direction == SysIdRoutine.Direction.FORWARD ? "Forward" : "Reverse"; } } diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSim.java index 76ebf36..015a66f 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSim.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSim.java @@ -11,7 +11,6 @@ import frc.robot.Constants; import frc.robot.Constants.FlywheelConstants; -import frc.robot.util.MathUtil; import org.wpilib.math.controller.PIDController; import org.wpilib.math.controller.SimpleMotorFeedforward; import org.wpilib.math.numbers.N1; @@ -41,7 +40,7 @@ public class FlywheelIOSim implements FlywheelIO { public void updateInputs(FlywheelIOInputs inputs) { if (closedLoop) { appliedVolts = - MathUtil.clamp( + Math.clamp( pid.calculate(sim.getAngularVelocity()) + ffVolts, -FlywheelConstants.kMaxVoltage, FlywheelConstants.kMaxVoltage); diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java index c531137..4c25e1e 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java @@ -46,9 +46,15 @@ public class FlywheelIOSpark implements FlywheelIO { // Define the leader / follower motors from the RobotDevices section of RobotContainer private final SparkMax leader = - new SparkMax(0, FLYWHEEL_LEADER.getDeviceNumber(), MotorType.kBrushless); + new SparkMax( + Constants.CANBuses.revPort(FLYWHEEL_LEADER.getBus()), + FLYWHEEL_LEADER.getDeviceNumber(), + MotorType.kBrushless); private final SparkMax follower = - new SparkMax(0, FLYWHEEL_FOLLOWER.getDeviceNumber(), MotorType.kBrushless); + new SparkMax( + Constants.CANBuses.revPort(FLYWHEEL_FOLLOWER.getBus()), + FLYWHEEL_FOLLOWER.getDeviceNumber(), + MotorType.kBrushless); private final RelativeEncoder encoder = leader.getEncoder(); private final SparkClosedLoopController pid = leader.getClosedLoopController(); // IMPORTANT: Include here all devices listed above that are part of this mechanism! @@ -74,7 +80,6 @@ public FlywheelIOSpark() { }) .smartCurrentLimit((int) SwerveConstants.kDriveCurrentLimit) .voltageCompensation(DrivebaseConstants.kNominalVoltage); - leaderConfig.encoder.uvwMeasurementPeriod(10).uvwAverageDepth(2); leaderConfig .closedLoop .feedbackSensor(FeedbackSensor.kPrimaryEncoder) diff --git a/src/main/java/frc/robot/subsystems/imu/Imu.java b/src/main/java/frc/robot/subsystems/imu/Imu.java index 8927ff0..4162692 100644 --- a/src/main/java/frc/robot/subsystems/imu/Imu.java +++ b/src/main/java/frc/robot/subsystems/imu/Imu.java @@ -29,9 +29,9 @@ public class Imu extends VirtualSubsystem { // Per-cycle cached objects (to avoid repeated allocations) private long cacheStampNs = -1L; - private Rotation2d cachedYaw = Rotation2d.kZero; - private Translation3d cachedAccel = Translation3d.kZero; - private Translation3d cachedJerk = Translation3d.kZero; + private Rotation2d cachedYaw = Rotation2d.ZERO; + private Translation3d cachedAccel = Translation3d.ZERO; + private Translation3d cachedJerk = Translation3d.ZERO; /** Constructor */ public Imu(ImuIO io) { diff --git a/src/main/java/frc/robot/subsystems/imu/ImuIO.java b/src/main/java/frc/robot/subsystems/imu/ImuIO.java index 8f71b2b..94d8309 100644 --- a/src/main/java/frc/robot/subsystems/imu/ImuIO.java +++ b/src/main/java/frc/robot/subsystems/imu/ImuIO.java @@ -39,9 +39,9 @@ class ImuIOInputs { // Yaw angular rate in radians/sec public double yawRateRadPerSec = 0.0; // Linear acceleration in robot frame (m/s^2) - public Translation3d linearAccel = Translation3d.kZero; + public Translation3d linearAccel = Translation3d.ZERO; // Linear jerk in robot frame (m/s^3) - public Translation3d linearJerk = Translation3d.kZero; + public Translation3d linearJerk = Translation3d.ZERO; // Time spent in the IO update call (seconds) public double latencySeconds = 0.0; // Odometry samples (timestamps in seconds) diff --git a/src/main/java/frc/robot/subsystems/imu/ImuIOPigeon2.java b/src/main/java/frc/robot/subsystems/imu/ImuIOPigeon2.java index 2fde327..77af8c9 100644 --- a/src/main/java/frc/robot/subsystems/imu/ImuIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/imu/ImuIOPigeon2.java @@ -54,7 +54,7 @@ public class ImuIOPigeon2 implements ImuIO { private final Queue odomYawsDeg; // Previous accel for jerk calculation (m/s/s) - private Translation3d prevAcc = Translation3d.kZero; + private Translation3d prevAcc = Translation3d.ZERO; private long prevTimestampNs = 0L; // Reusable buffers for queue-drain (to avoid using streams) diff --git a/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java b/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java index aaa3959..a9182ad 100644 --- a/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java +++ b/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java @@ -70,7 +70,7 @@ public void updateInputs(ImuIOInputs inputs) { // Jerk: SIM doesn't have a prior accel here unless you want it; set to 0 by default. // If you do want jerk, you can add prevAx/prevAy/prevAz + dt just like the real IO. - inputs.linearJerk = Translation3d.kZero; + inputs.linearJerk = Translation3d.ZERO; // Maintain odometry history pushOdomSample(TimeUtil.now(), yawRad); diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java index 0e0ec9f..b593169 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIO.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java @@ -20,7 +20,7 @@ class VisionIOInputs { // Latest "camera to target" observation (optional) public TargetObservation latestTargetObservation = - new TargetObservation(Rotation2d.kZero, Rotation2d.kZero); + new TargetObservation(Rotation2d.ZERO, Rotation2d.ZERO); // Pose observations generated by the camera pipeline (each has its own timestamp) public PoseObservation[] poseObservations = new PoseObservation[0]; diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java index 5923813..f95172b 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java @@ -45,8 +45,8 @@ public void updateInputs(VisionIOInputs inputs) { final ArrayList poseObservations = new ArrayList<>(1); double newestTargetTs = Double.NEGATIVE_INFINITY; - Rotation2d bestYaw = Rotation2d.kZero; - Rotation2d bestPitch = Rotation2d.kZero; + Rotation2d bestYaw = Rotation2d.ZERO; + Rotation2d bestPitch = Rotation2d.ZERO; List unreadResults = camera.getAllUnreadResults(); @@ -134,7 +134,7 @@ public void updateInputs(VisionIOInputs inputs) { inputs.latestTargetObservation = (newestTargetTs > Double.NEGATIVE_INFINITY) ? new TargetObservation(bestYaw, bestPitch) - : new TargetObservation(Rotation2d.kZero, Rotation2d.kZero); + : new TargetObservation(Rotation2d.ZERO, Rotation2d.ZERO); inputs.poseObservations = poseObservations.toArray(new PoseObservation[0]); diff --git a/src/main/java/frc/robot/util/Alert.java b/src/main/java/frc/robot/util/Alert.java deleted file mode 100644 index 6af6199..0000000 --- a/src/main/java/frc/robot/util/Alert.java +++ /dev/null @@ -1,138 +0,0 @@ -// Copyright (c) 2024-2026 Az-FIRST -// http://github.com/AZ-First -// Copyright (c) 2021-2026 Littleton Robotics -// http://github.com/Mechanical-Advantage -// -// Use of this source code is governed by a BSD -// license that can be found in the AdvantageKit-License.md file -// at the root directory of this project. - -package frc.robot.util; - -import java.util.ArrayList; -import java.util.Comparator; -import java.util.HashMap; -import java.util.List; -import java.util.Map; -import org.wpilib.driverstation.DriverStationErrors; -import org.wpilib.smartdashboard.SmartDashboard; -import org.wpilib.util.sendable.Sendable; -import org.wpilib.util.sendable.SendableBuilder; - -/** Class for managing persistent alerts to be sent over NetworkTables. */ -public class Alert { - private static final Map groups = new HashMap<>(); - - private final AlertType type; - private boolean active = false; - private double activeStartTime = 0.0; - private String text; - - /** - * Creates a new Alert in the default group - "Alerts". If this is the first to be instantiated, - * the appropriate entries will be added to NetworkTables. - * - * @param text Text to be displayed when the alert is active. - * @param type Alert level specifying urgency. - */ - public Alert(String text, AlertType type) { - this("Alerts", text, type); - } - - /** - * Creates a new Alert. If this is the first to be instantiated in its group, the appropriate - * entries will be added to NetworkTables. - * - * @param group Group identifier, also used as NetworkTables title - * @param text Text to be displayed when the alert is active. - * @param type Alert level specifying urgency. - */ - public Alert(String group, String text, AlertType type) { - SendableAlerts alertGroup = - groups.computeIfAbsent( - group, - name -> { - SendableAlerts createdGroup = new SendableAlerts(); - SmartDashboard.putData(name, createdGroup); - return createdGroup; - }); - - this.text = text; - this.type = type; - alertGroup.alerts.add(this); - } - - /** - * Sets whether the alert should currently be displayed. When activated, the alert text will also - * be sent to the console. - */ - public void set(boolean active) { - if (active && !this.active) { - activeStartTime = TimeUtil.now(); - reportToConsole(); - } - this.active = active; - } - - /** Updates current alert text. */ - public void setText(String text) { - if (active && !text.equals(this.text)) { - this.text = text; - reportToConsole(); - return; - } - this.text = text; - } - - private void reportToConsole() { - switch (type) { - case ERROR -> DriverStationErrors.reportError(text, false); - case WARNING -> DriverStationErrors.reportWarning(text, false); - case INFO -> System.out.println(text); - } - } - - private static class SendableAlerts implements Sendable { - public final List alerts = new ArrayList<>(); - - public String[] getStrings(AlertType type) { - return alerts.stream() - .filter(alert -> alert.type == type && alert.active) - .sorted(Comparator.comparingDouble((Alert alert) -> alert.activeStartTime).reversed()) - .map(alert -> alert.text) - .toArray(String[]::new); - } - - @Override - public void initSendable(SendableBuilder builder) { - builder.setSmartDashboardType("Alerts"); - builder.addStringArrayProperty("errors", () -> getStrings(AlertType.ERROR), null); - builder.addStringArrayProperty("warnings", () -> getStrings(AlertType.WARNING), null); - builder.addStringArrayProperty("infos", () -> getStrings(AlertType.INFO), null); - } - } - - /** Represents an alert's level of urgency. */ - public static enum AlertType { - /** - * High priority alert - displayed first on the dashboard with a red "X" symbol. Use this type - * for problems which will seriously affect the robot's functionality and thus require immediate - * attention. - */ - ERROR, - - /** - * Medium priority alert - displayed second on the dashboard with a yellow "!" symbol. Use this - * type for problems which could affect the robot's functionality but do not necessarily require - * immediate attention. - */ - WARNING, - - /** - * Low priority alert - displayed last on the dashboard with a green "i" symbol. Use this type - * for problems which are unlikely to affect the robot's functionality, or any other alerts - * which do not fall under "ERROR" or "WARNING". - */ - INFO - } -} diff --git a/src/main/java/frc/robot/util/ConcurrentTimeInterpolatableBuffer.java b/src/main/java/frc/robot/util/ConcurrentTimeInterpolatableBuffer.java index e50e3e5..347b9c3 100644 --- a/src/main/java/frc/robot/util/ConcurrentTimeInterpolatableBuffer.java +++ b/src/main/java/frc/robot/util/ConcurrentTimeInterpolatableBuffer.java @@ -144,7 +144,7 @@ public Optional getSample(double timeSeconds) { if (Math.abs(timeSpan) < 1e-9) return Optional.of(bottomBound.getValue()); double ratio = (timeSeconds - t0) / timeSpan; - ratio = MathUtil.clamp(ratio, 0.0, 1.0); + ratio = Math.clamp(ratio, 0.0, 1.0); return Optional.of( m_interpolatingFunc.interpolate(bottomBound.getValue(), topBound.getValue(), ratio)); diff --git a/src/main/java/frc/robot/util/GeomUtil.java b/src/main/java/frc/robot/util/GeomUtil.java index 42c2e3c..b23abd3 100644 --- a/src/main/java/frc/robot/util/GeomUtil.java +++ b/src/main/java/frc/robot/util/GeomUtil.java @@ -27,7 +27,7 @@ public class GeomUtil { * @return The resulting transform */ public static Transform2d toTransform2d(Translation2d translation) { - return new Transform2d(translation, Rotation2d.kZero); + return new Transform2d(translation, Rotation2d.ZERO); } /** @@ -38,7 +38,7 @@ public static Transform2d toTransform2d(Translation2d translation) { * @return The resulting transform */ public static Transform2d toTransform2d(double x, double y) { - return new Transform2d(x, y, Rotation2d.kZero); + return new Transform2d(x, y, Rotation2d.ZERO); } /** @@ -48,7 +48,7 @@ public static Transform2d toTransform2d(double x, double y) { * @return The resulting transform */ public static Transform2d toTransform2d(Rotation2d rotation) { - return new Transform2d(Translation2d.kZero, rotation); + return new Transform2d(Translation2d.ZERO, rotation); } /** @@ -85,7 +85,7 @@ public static Pose2d toPose2d(Transform2d transform) { * @return The resulting pose */ public static Pose2d toPose2d(Translation2d translation) { - return new Pose2d(translation, Rotation2d.kZero); + return new Pose2d(translation, Rotation2d.ZERO); } /** @@ -95,7 +95,7 @@ public static Pose2d toPose2d(Translation2d translation) { * @return The resulting pose */ public static Pose2d toPose2d(Rotation2d rotation) { - return new Pose2d(Translation2d.kZero, rotation); + return new Pose2d(Translation2d.ZERO, rotation); } /** diff --git a/src/main/java/frc/robot/util/LocalADStarAK.java b/src/main/java/frc/robot/util/LocalADStarAK.java index 0aff129..ee2f928 100644 --- a/src/main/java/frc/robot/util/LocalADStarAK.java +++ b/src/main/java/frc/robot/util/LocalADStarAK.java @@ -22,7 +22,7 @@ import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.inputs.LoggableInputs; import org.wpilib.math.geometry.Translation2d; -import org.wpilib.math.util.Pair; +import org.wpilib.util.Pair; // PathPlannerLib pathfinder compatible with AdvantageKit log replay // NOTE: This file is available at diff --git a/src/main/java/frc/robot/util/OverrideSwitches.java b/src/main/java/frc/robot/util/OverrideSwitches.java index 9fa2553..c249b9a 100644 --- a/src/main/java/frc/robot/util/OverrideSwitches.java +++ b/src/main/java/frc/robot/util/OverrideSwitches.java @@ -20,6 +20,7 @@ import org.wpilib.command2.button.Trigger; import org.wpilib.driverstation.GenericHID; +import org.wpilib.driverstation.Joystick; import org.wpilib.driverstation.internal.DriverStationBackend; /** Interface for physical override switches on operator console. */ @@ -27,7 +28,7 @@ public class OverrideSwitches { private final GenericHID consoleSwitches; public OverrideSwitches(int port) { - consoleSwitches = new GenericHID(port); + consoleSwitches = new Joystick(port).getHID(); } /** Returns whether the controller is connected. */ diff --git a/src/main/java/frc/robot/util/RBSIController.java b/src/main/java/frc/robot/util/RBSIController.java index 505067d..943eb45 100644 --- a/src/main/java/frc/robot/util/RBSIController.java +++ b/src/main/java/frc/robot/util/RBSIController.java @@ -210,10 +210,10 @@ public Trigger button(Button button) { case RIGHT_BUMPER -> controller.rightBumper(); case LEFT_STICK -> controller.leftStick(); case RIGHT_STICK -> controller.rightStick(); - case POV_LEFT -> controller.povLeft(); - case POV_RIGHT -> controller.povRight(); - case POV_UP -> controller.povUp(); - case POV_DOWN -> controller.povDown(); + case POV_LEFT -> controller.getHID().povLeft(); + case POV_RIGHT -> controller.getHID().povRight(); + case POV_UP -> controller.getHID().povUp(); + case POV_DOWN -> controller.getHID().povDown(); }; } @@ -273,10 +273,10 @@ public Trigger button(Button button) { case RIGHT_BUMPER -> controller.R1(); case LEFT_STICK -> controller.L3(); case RIGHT_STICK -> controller.R3(); - case POV_LEFT -> controller.povLeft(); - case POV_RIGHT -> controller.povRight(); - case POV_UP -> controller.povUp(); - case POV_DOWN -> controller.povDown(); + case POV_LEFT -> controller.getHID().povLeft(); + case POV_RIGHT -> controller.getHID().povRight(); + case POV_UP -> controller.getHID().povUp(); + case POV_DOWN -> controller.getHID().povDown(); }; } @@ -328,10 +328,10 @@ public Trigger button(Button button) { case RIGHT_BUMPER -> controller.R1(); case LEFT_STICK -> controller.L3(); case RIGHT_STICK -> controller.R3(); - case POV_LEFT -> controller.povLeft(); - case POV_RIGHT -> controller.povRight(); - case POV_UP -> controller.povUp(); - case POV_DOWN -> controller.povDown(); + case POV_LEFT -> controller.getHID().povLeft(); + case POV_RIGHT -> controller.getHID().povRight(); + case POV_UP -> controller.getHID().povUp(); + case POV_DOWN -> controller.getHID().povDown(); }; } diff --git a/src/main/java/frc/robot/util/RBSIPowerMonitor.java b/src/main/java/frc/robot/util/RBSIPowerMonitor.java index 45c8aea..50793a4 100644 --- a/src/main/java/frc/robot/util/RBSIPowerMonitor.java +++ b/src/main/java/frc/robot/util/RBSIPowerMonitor.java @@ -15,10 +15,11 @@ import frc.robot.Constants.PowerConstants; import frc.robot.Constants.RobotDevices; -import frc.robot.util.Alert.AlertType; import org.littletonrobotics.conduit.ConduitApi; import org.littletonrobotics.junction.Logger; import org.wpilib.system.RobotController; +import org.wpilib.util.Alert; +import org.wpilib.util.Alert.Level; /** * Power monitoring virtual subsystem that periodically polls the Power Distribution Module. Each @@ -56,11 +57,12 @@ public class RBSIPowerMonitor extends VirtualSubsystem { }; private final Alert totalCurrentAlert = - new Alert("Total current draw exceeds limit!", AlertType.WARNING); + new Alert("totalCurrentHigh", "Total current draw exceeds limit!", Level.MEDIUM); private final Alert[] portAlerts = new Alert[24]; // or pdh.getNumChannels() after construct - private final Alert lowVoltageAlert = new Alert("Low battery voltage!", AlertType.WARNING); + private final Alert lowVoltageAlert = + new Alert("lowBatteryVoltage", "Low battery voltage!", Level.MEDIUM); private final Alert criticalVoltageAlert = - new Alert("Critical battery voltage!", AlertType.ERROR); + new Alert("criticalBatteryVoltage", "Critical battery voltage!", Level.HIGH); private final double[] channelCurrents = new double[24]; private long loops = 0; @@ -71,7 +73,11 @@ public RBSIPowerMonitor(LoggedTunableNumber batteryCapacityAh, RBSISubsystem... this.subsystems = subsystems; for (int i = 0; i < portAlerts.length; i++) { - portAlerts[i] = new Alert("Port " + i + " current exceeds limit!", AlertType.WARNING); + portAlerts[i] = + new Alert( + "powerPort" + i + "CurrentHigh", + "Port " + i + " current exceeds limit!", + Level.MEDIUM); } } diff --git a/src/main/java/org/wpilib/math/util/Pair.java b/src/main/java/org/wpilib/math/util/Pair.java new file mode 100644 index 0000000..4c92edb --- /dev/null +++ b/src/main/java/org/wpilib/math/util/Pair.java @@ -0,0 +1,8 @@ +package org.wpilib.math.util; + +/** Temporary PhotonVision alpha-2 binary compatibility type. See doc/PHOTONVISION-COMPAT.md. */ +public class Pair extends org.wpilib.util.Pair { + public Pair(A first, B second) { + super(first, second); + } +} diff --git a/src/main/java/org/wpilib/vision/apriltag/AprilTag.java b/src/main/java/org/wpilib/vision/apriltag/AprilTag.java new file mode 100644 index 0000000..2579a52 --- /dev/null +++ b/src/main/java/org/wpilib/vision/apriltag/AprilTag.java @@ -0,0 +1,14 @@ +package org.wpilib.vision.apriltag; + +import org.wpilib.math.geometry.Pose3d; + +/** Temporary PhotonVision alpha-2 binary compatibility type. See doc/PHOTONVISION-COMPAT.md. */ +public final class AprilTag { + public final int ID; + public final Pose3d pose; + + public AprilTag(int id, Pose3d pose) { + this.ID = id; + this.pose = pose; + } +} diff --git a/src/main/java/org/wpilib/vision/apriltag/AprilTagFieldLayout.java b/src/main/java/org/wpilib/vision/apriltag/AprilTagFieldLayout.java new file mode 100644 index 0000000..c83fcb6 --- /dev/null +++ b/src/main/java/org/wpilib/vision/apriltag/AprilTagFieldLayout.java @@ -0,0 +1,86 @@ +package org.wpilib.vision.apriltag; + +import com.fasterxml.jackson.databind.JsonNode; +import com.fasterxml.jackson.databind.ObjectMapper; +import java.io.IOException; +import java.nio.file.Path; +import java.util.List; +import org.wpilib.fields.Field; +import org.wpilib.fields.FieldTag; +import org.wpilib.math.geometry.Pose3d; +import org.wpilib.math.geometry.Quaternion; +import org.wpilib.math.geometry.Rotation3d; +import org.wpilib.math.geometry.Translation3d; + +/** + * Bridges PhotonVision alpha-2's removed layout API to WPILib 2027's {@link Field}. + * + *

Remove this class and its companion compatibility types after upgrading PhotonVision. See + * doc/PHOTONVISION-COMPAT.md. + */ +public final class AprilTagFieldLayout { + private static final ObjectMapper MAPPER = new ObjectMapper(); + private final Field field; + + public AprilTagFieldLayout(Path path) throws IOException { + JsonNode root = MAPPER.readTree(path.toFile()); + JsonNode dimensions = root.required("field"); + List tags = new java.util.ArrayList<>(); + for (JsonNode tag : root.required("tags")) { + JsonNode translation = tag.required("pose").required("translation"); + JsonNode quaternion = tag.required("pose").required("rotation").required("quaternion"); + Pose3d pose = + new Pose3d( + new Translation3d( + translation.required("x").asDouble(), + translation.required("y").asDouble(), + translation.required("z").asDouble()), + new Rotation3d( + new Quaternion( + quaternion.required("W").asDouble(), + quaternion.required("X").asDouble(), + quaternion.required("Y").asDouble(), + quaternion.required("Z").asDouble()))); + tags.add(new FieldTag(tag.required("ID").asInt(), pose)); + } + String name = path.getFileName().toString().replaceFirst("\\.json$", ""); + field = + new Field( + name, + name.matches("^\\d{4}.*") ? name.substring(0, 4) : "custom", + name, + null, + dimensions.required("length").asDouble(), + dimensions.required("width").asDouble(), + "frc", + tags); + } + + private AprilTagFieldLayout(Field field) { + this.field = field; + } + + public static AprilTagFieldLayout loadField(AprilTagFields field) { + return new AprilTagFieldLayout(Field.loadField(field.field)); + } + + public List getTags() { + return field.getTags().stream().map(tag -> new AprilTag(tag.getID(), tag.getPose())).toList(); + } + + public java.util.Optional getTagPose(int id) { + return field.getTagPose(id); + } + + public Pose3d getOrigin() { + return field.getOrigin(); + } + + public double getFieldLength() { + return field.getFieldLength(); + } + + public double getFieldWidth() { + return field.getFieldWidth(); + } +} diff --git a/src/main/java/org/wpilib/vision/apriltag/AprilTagFields.java b/src/main/java/org/wpilib/vision/apriltag/AprilTagFields.java new file mode 100644 index 0000000..4ba4c01 --- /dev/null +++ b/src/main/java/org/wpilib/vision/apriltag/AprilTagFields.java @@ -0,0 +1,14 @@ +package org.wpilib.vision.apriltag; + +import org.wpilib.fields.Fields; + +/** Temporary PhotonVision alpha-2 binary compatibility type. See doc/PHOTONVISION-COMPAT.md. */ +public enum AprilTagFields { + kDefaultField(Fields.DEFAULT_FIELD); + + final Fields field; + + AprilTagFields(Fields field) { + this.field = field; + } +} diff --git a/src/test/CurrentLimitTests.java b/src/test/CurrentLimitTests.java index 740d829..ec461a2 100644 --- a/src/test/CurrentLimitTests.java +++ b/src/test/CurrentLimitTests.java @@ -43,7 +43,7 @@ public void close() { @BeforeEach public void constructDevices() { - assert HAL.initialize(500, 0); + assert HAL.initialize(); talon = new TalonFX(0, new CANBus("")); diff --git a/src/test/FusedCANcoderTests.java b/src/test/FusedCANcoderTests.java index 995f8e6..be7eae3 100644 --- a/src/test/FusedCANcoderTests.java +++ b/src/test/FusedCANcoderTests.java @@ -36,7 +36,7 @@ public class FusedCANcoderTests { @BeforeEach public void constructDevices() { - assert HAL.initialize(500, 0); + assert HAL.initialize(); talon = new TalonFX(0, new CANBus("")); cancoder = new CANcoder(0, new CANBus("")); diff --git a/src/test/LatencyCompensationTests.java b/src/test/LatencyCompensationTests.java index 90b9665..430b207 100644 --- a/src/test/LatencyCompensationTests.java +++ b/src/test/LatencyCompensationTests.java @@ -32,7 +32,7 @@ public class LatencyCompensationTests { @BeforeEach public void constructDevices() { - assert HAL.initialize(500, 0); + assert HAL.initialize(); talonfx = new TalonFX(0, new CANBus("")); cancoder = new CANcoder(0, new CANBus("")); diff --git a/src/test/frc/robot/RobotConstantsTest.java b/src/test/frc/robot/RobotConstantsTest.java index 4950a3d..da6edc5 100644 --- a/src/test/frc/robot/RobotConstantsTest.java +++ b/src/test/frc/robot/RobotConstantsTest.java @@ -8,6 +8,7 @@ import frc.robot.subsystems.drive.SwerveConstants; import java.util.Arrays; import org.junit.jupiter.api.Test; +import org.wpilib.hardware.bus.CANPort; class RobotConstantsTest { @Test @@ -70,9 +71,9 @@ void eachModuleHasOneRegisteredCANBusForAllThreeDevices() { @Test void systemcorePortsAreExplicit() { assertEquals("can_s1", Constants.CANBuses.RIO); - assertEquals(0, Constants.CANBuses.revPort(Constants.CANBuses.SYSTEMCORE_0)); - assertEquals(4, Constants.CANBuses.revPort(Constants.CANBuses.SYSTEMCORE_4)); - assertEquals(1, Constants.CANBuses.revPort(Constants.CANBuses.RIO)); + assertEquals(CANPort.CAN_S0, Constants.CANBuses.revPort(Constants.CANBuses.SYSTEMCORE_0)); + assertEquals(CANPort.CAN_S4, Constants.CANBuses.revPort(Constants.CANBuses.SYSTEMCORE_4)); + assertEquals(CANPort.CAN_S1, Constants.CANBuses.revPort(Constants.CANBuses.RIO)); assertThrows(IllegalArgumentException.class, () -> Constants.CANBuses.revPort("DriveTrain")); } diff --git a/src/test/frc/robot/commands/DriveCommandsTests.java b/src/test/frc/robot/commands/DriveCommandsTests.java index c0db9fe..15ce1a5 100644 --- a/src/test/frc/robot/commands/DriveCommandsTests.java +++ b/src/test/frc/robot/commands/DriveCommandsTests.java @@ -20,18 +20,18 @@ class DriveCommandsTests { @BeforeEach void setupHal() { - HAL.initialize(500, 0); + HAL.initialize(); DriverStationSim.setEnabled(false); DriverStationSim.notifyNewData(); } @Test void linearVelocityAppliesDeadbandAndSquaresMagnitudeWithoutChangingDirection() { - assertEquals(Translation2d.kZero, DriveCommands.getLinearVelocity(0.01, 0.01)); + assertEquals(Translation2d.ZERO, DriveCommands.getLinearVelocity(0.01, 0.01)); Translation2d velocity = DriveCommands.getLinearVelocity(0.6, 0.8); - assertEquals(1.0, velocity.getAngle().getCos() / 0.6, 1e-6); - assertEquals(1.0, velocity.getAngle().getSin() / 0.8, 1e-6); + assertEquals(1.0, velocity.getAngle().orElseThrow().getCos() / 0.6, 1e-6); + assertEquals(1.0, velocity.getAngle().orElseThrow().getSin() / 0.8, 1e-6); assertEquals(1.0, velocity.getNorm(), EPSILON); } diff --git a/src/test/frc/robot/subsystems/drive/DriveSubsystemTests.java b/src/test/frc/robot/subsystems/drive/DriveSubsystemTests.java index 501039d..eca5dde 100644 --- a/src/test/frc/robot/subsystems/drive/DriveSubsystemTests.java +++ b/src/test/frc/robot/subsystems/drive/DriveSubsystemTests.java @@ -20,7 +20,7 @@ class DriveSubsystemTests { @BeforeEach void setupHal() { - assertTrue(HAL.initialize(500, 0)); + assertTrue(HAL.initialize()); DriverStationSim.setEnabled(false); DriverStationSim.notifyNewData(); } @@ -42,8 +42,8 @@ void poseBufferAccessorsAreSafeWhenEmptyAndInterpolateWhenPopulated() { assertTrue(Double.isNaN(drive.getPoseBufferNewestTime())); assertTrue(drive.getPoseAtTime(1.0).isEmpty()); - drive.poseBufferAddSample(1.0, new Pose2d(1.0, 0.0, Rotation2d.kZero)); - drive.poseBufferAddSample(2.0, new Pose2d(3.0, 0.0, Rotation2d.kZero)); + drive.poseBufferAddSample(1.0, new Pose2d(1.0, 0.0, Rotation2d.ZERO)); + drive.poseBufferAddSample(2.0, new Pose2d(3.0, 0.0, Rotation2d.ZERO)); assertEquals(1.0, drive.getPoseBufferOldestTime(), EPSILON); assertEquals(2.0, drive.getPoseBufferNewestTime(), EPSILON); @@ -62,7 +62,9 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.odometryTimestamps = new double[] {1.0, 2.0, 3.0}; inputs.odometryDrivePositionsRad = new double[] {4.0, 5.0}; inputs.odometryTurnPositions = - new Rotation2d[] {Rotation2d.kZero, Rotation2d.kCCW_Pi_2, Rotation2d.kPi}; + new Rotation2d[] { + Rotation2d.ZERO, Rotation2d.fromDegrees(90.0), Rotation2d.fromDegrees(180.0) + }; } }; Module module = new Module(fakeIo, 0); @@ -76,10 +78,10 @@ void disabledCoastDoesNotCountBaselineSampleAsStationary() { Drive drive = new Drive(new Imu(new ImuIOSim())); SwerveModulePosition[] stationaryPositions = new SwerveModulePosition[] { - new SwerveModulePosition(1.0, Rotation2d.kZero), - new SwerveModulePosition(1.0, Rotation2d.kZero), - new SwerveModulePosition(1.0, Rotation2d.kZero), - new SwerveModulePosition(1.0, Rotation2d.kZero) + new SwerveModulePosition(1.0, Rotation2d.ZERO), + new SwerveModulePosition(1.0, Rotation2d.ZERO), + new SwerveModulePosition(1.0, Rotation2d.ZERO), + new SwerveModulePosition(1.0, Rotation2d.ZERO) }; DriverStationSim.setEnabled(true); @@ -101,28 +103,28 @@ void disabledCoastDoesNotCountBaselineSampleAsStationary() { @Test void pathPlannerStartResetsWhenVisionIsMissingOrEstimatorIsUninitialized() { - Pose2d pathStart = new Pose2d(4.0, 6.0, Rotation2d.kZero); + Pose2d pathStart = new Pose2d(4.0, 6.0, Rotation2d.ZERO); assertEquals( Drive.PathPlannerStartAction.RESET_TO_PATH_START, Drive.determinePathPlannerStartAction( - new Pose2d(6.0, 6.0, Rotation2d.kZero), pathStart, false, 0.5)); + new Pose2d(6.0, 6.0, Rotation2d.ZERO), pathStart, false, 0.5)); assertEquals( Drive.PathPlannerStartAction.RESET_TO_PATH_START, - Drive.determinePathPlannerStartAction(Pose2d.kZero, pathStart, true, 0.5)); + Drive.determinePathPlannerStartAction(Pose2d.ZERO, pathStart, true, 0.5)); } @Test void pathPlannerStartUsesNearbyVisionAndBlocksDistantVision() { - Pose2d pathStart = new Pose2d(4.0, 6.0, Rotation2d.kZero); + Pose2d pathStart = new Pose2d(4.0, 6.0, Rotation2d.ZERO); assertEquals( Drive.PathPlannerStartAction.USE_VISION_POSE, Drive.determinePathPlannerStartAction( - new Pose2d(4.4, 6.0, Rotation2d.kZero), pathStart, true, 0.5)); + new Pose2d(4.4, 6.0, Rotation2d.ZERO), pathStart, true, 0.5)); assertEquals( Drive.PathPlannerStartAction.BLOCK_AUTO, Drive.determinePathPlannerStartAction( - new Pose2d(4.6, 6.0, Rotation2d.kZero), pathStart, true, 0.5)); + new Pose2d(4.6, 6.0, Rotation2d.ZERO), pathStart, true, 0.5)); } } diff --git a/src/test/frc/robot/subsystems/drive/SparkSwerveUnitsTest.java b/src/test/frc/robot/subsystems/drive/SparkSwerveUnitsTest.java new file mode 100644 index 0000000..e1ec636 --- /dev/null +++ b/src/test/frc/robot/subsystems/drive/SparkSwerveUnitsTest.java @@ -0,0 +1,44 @@ +package frc.robot.subsystems.drive; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import org.junit.jupiter.api.Test; +import org.wpilib.math.geometry.Rotation2d; + +class SparkSwerveUnitsTest { + private static final double EPSILON = 1e-9; + + @Test + void driveEncoderAndControllerUseInverseUnits() { + double wheelRadiansPerSecond = 5.0; + double motorRpm = SparkSwerveUnits.driveVelocityRpm(wheelRadiansPerSecond); + + assertEquals( + wheelRadiansPerSecond, SparkSwerveUnits.driveVelocityRadiansPerSecond(motorRpm), EPSILON); + assertEquals( + wheelRadiansPerSecond * 60.0, SparkSwerveUnits.drivePositionRadians(motorRpm), EPSILON); + assertEquals( + DRIVE_GAIN * wheelRadiansPerSecond, + SparkSwerveUnits.driveVelocityGain(DRIVE_GAIN) * motorRpm, + EPSILON); + } + + @Test + void turnSetpointWrapsInAbsoluteEncoderRotations() { + Rotation2d zeroRotation = Rotation2d.fromDegrees(20.0); + double setpoint = + SparkSwerveUnits.turnSetpointRotations(Rotation2d.fromDegrees(350.0), zeroRotation); + + assertEquals(10.0 / 360.0, setpoint, EPSILON); + assertEquals( + Rotation2d.fromDegrees(-10.0).getRadians(), + SparkSwerveUnits.turnPosition(setpoint, zeroRotation).getRadians(), + EPSILON); + assertEquals(2.0 * Math.PI, SparkSwerveUnits.turnVelocityRadiansPerSecond(60.0), EPSILON); + assertEquals( + STEER_GAIN * Math.PI, SparkSwerveUnits.turnPositionGain(STEER_GAIN) * 0.5, EPSILON); + } + + private static final double DRIVE_GAIN = 0.7; + private static final double STEER_GAIN = 100.0; +} diff --git a/src/test/frc/robot/subsystems/flywheel_example/FlywheelSysIdTests.java b/src/test/frc/robot/subsystems/flywheel_example/FlywheelSysIdTests.java index 0d4e83c..a361e3d 100644 --- a/src/test/frc/robot/subsystems/flywheel_example/FlywheelSysIdTests.java +++ b/src/test/frc/robot/subsystems/flywheel_example/FlywheelSysIdTests.java @@ -14,16 +14,16 @@ void sysIdRoutinesUseDistinctMechanismNames() { assertEquals( "Flywheel SysId Voltage Forward Quasistatic", - flywheel.sysIdVoltageQuasistatic(SysIdRoutine.Direction.kForward).getName()); + flywheel.sysIdVoltageQuasistatic(SysIdRoutine.Direction.FORWARD).getName()); assertEquals( "Flywheel SysId Voltage Reverse Dynamic", - flywheel.sysIdVoltageDynamic(SysIdRoutine.Direction.kReverse).getName()); + flywheel.sysIdVoltageDynamic(SysIdRoutine.Direction.REVERSE).getName()); assertEquals( "Flywheel SysId Duty Cycle Forward Quasistatic", - flywheel.sysIdDutyCycleQuasistatic(SysIdRoutine.Direction.kForward).getName()); + flywheel.sysIdDutyCycleQuasistatic(SysIdRoutine.Direction.FORWARD).getName()); assertEquals( "Flywheel SysId Duty Cycle Reverse Dynamic", - flywheel.sysIdDutyCycleDynamic(SysIdRoutine.Direction.kReverse).getName()); + flywheel.sysIdDutyCycleDynamic(SysIdRoutine.Direction.REVERSE).getName()); } @Test diff --git a/src/test/frc/robot/util/UtilTests.java b/src/test/frc/robot/util/UtilTests.java index 3a9e01e..7b7a36d 100644 --- a/src/test/frc/robot/util/UtilTests.java +++ b/src/test/frc/robot/util/UtilTests.java @@ -16,7 +16,7 @@ class UtilTests { @Test void timeUtilReturnsSecondsFromAdvantageKitMicrosecondTimestamp() { - assertTrue(HAL.initialize(500, 0)); + assertTrue(HAL.initialize()); double expectedSeconds = RobotController.getTime() * 1.0e-6; double actualSeconds = TimeUtil.now(); diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index 1a8abd5..62a2a71 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,9 +1,9 @@ { "fileName": "AdvantageKit.json", "name": "AdvantageKit", - "version": "27.0.0-alpha-4", + "version": "27.0.0-alpha-7", "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", - "wpilibYear": "2027_alpha5", + "wpilibYear": "2027_alpha7", "mavenUrls": [ "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/" ], @@ -12,14 +12,14 @@ { "groupId": "org.littletonrobotics.akit", "artifactId": "akit-java", - "version": "27.0.0-alpha-4" + "version": "27.0.0-alpha-7" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.akit", "artifactId": "akit-wpilibio", - "version": "27.0.0-alpha-4", + "version": "27.0.0-alpha-7", "skipInvalidPlatforms": false, "isJar": false, "validPlatforms": [ @@ -27,7 +27,8 @@ "linuxx86-64", "linuxarm64", "osxuniversal", - "windowsx86-64" + "windowsx86-64", + "windowsarm64" ] } ], diff --git a/vendordeps/AprilTag.json b/vendordeps/AprilTag.json new file mode 100644 index 0000000..8cac425 --- /dev/null +++ b/vendordeps/AprilTag.json @@ -0,0 +1,91 @@ +{ + "fileName": "AprilTag.json", + "name": "AprilTag", + "version": "1.0.0", + "uuid": "3ac3403d-7f6b-4b5a-a09c-a49a2c32e5a3", + "wpilibYear": "2027_alpha7", + "mavenUrls": [], + "jsonUrl": "", + "javaDependencies": [ + { + "groupId": "org.wpilib.thirdparty.opencv", + "artifactId": "opencv-java", + "version": "2027-4.13.0-3" + }, + { + "groupId": "org.wpilib.apriltag", + "artifactId": "apriltag-java", + "version": "wpilib" + } + ], + "jniDependencies": [ + { + "groupId": "org.wpilib.thirdparty.opencv", + "artifactId": "opencv-cpp", + "version": "2027-4.13.0-3", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "linuxsystemcore", + "linuxarm64", + "windowsx86-64", + "windowsarm64", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.wpilib.apriltag", + "artifactId": "apriltag-cpp", + "version": "wpilib", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "linuxsystemcore", + "linuxarm64", + "windowsx86-64", + "windowsarm64", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "org.wpilib.thirdparty.opencv", + "artifactId": "opencv-cpp", + "version": "2027-4.13.0-3", + "libName": "opencv", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxsystemcore", + "linuxarm64", + "windowsx86-64", + "windowsarm64", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.wpilib.apriltag", + "artifactId": "apriltag-cpp", + "version": "wpilib", + "libName": "apriltag", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxsystemcore", + "linuxarm64", + "windowsx86-64", + "windowsarm64", + "linuxx86-64", + "osxuniversal" + ] + } + ] +} diff --git a/vendordeps/Autopilot.json b/vendordeps/Autopilot.json index 2e4137b..465790f 100644 --- a/vendordeps/Autopilot.json +++ b/vendordeps/Autopilot.json @@ -4,7 +4,7 @@ "fileName": "Autopilot.json", "version": "1.7.0-alpha-6", "frcYear": "2027", - "wpilibYear": "2027_alpha5", + "wpilibYear": "2027_alpha7", "jsonUrl": "https://raw.githubusercontent.com/AZ-First/autopilot/refs/heads/sc2027/vendordep.json", "mavenUrls": [ "https://raw.githubusercontent.com/AZ-First/autopilot/refs/heads/sc2027/maven" diff --git a/vendordeps/CommandsV2.json b/vendordeps/CommandsV2.json index 8358b09..4216f8d 100644 --- a/vendordeps/CommandsV2.json +++ b/vendordeps/CommandsV2.json @@ -3,12 +3,12 @@ "name": "Commands V2", "version": "1.0.0", "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", - "wpilibYear": "2027_alpha5", + "wpilibYear": "2027_alpha7", "mavenUrls": [], "jsonUrl": "", "conflictsWith": [ { - "uuid": "4decdc05-a056-46cf-9561-39449bbb01306", + "uuid": "4decdc05-a056-46cf-9561-39449bbb0130", "errorMessage": "Users can not have both Commands v2 and Commands v3 vendordeps in their robot program.", "offlineFileName": "CommandsV3.json" } @@ -33,11 +33,9 @@ "skipInvalidPlatforms": true, "binaryPlatforms": [ "linuxsystemcore", - "linuxathena", - "linuxarm32", "linuxarm64", "windowsx86-64", - "windowsx86", + "windowsarm64", "linuxx86-64", "osxuniversal" ] diff --git a/vendordeps/PathplannerLibSystemCoreAlpha.json b/vendordeps/PathplannerLibSystemCoreAlpha.json index 9b36d2b..fa1f904 100644 --- a/vendordeps/PathplannerLibSystemCoreAlpha.json +++ b/vendordeps/PathplannerLibSystemCoreAlpha.json @@ -1,9 +1,9 @@ { "fileName": "PathplannerLibSystemCoreAlpha.json", "name": "PathplannerLib", - "version": "2027.0.0-alpha-3", + "version": "2027.0.0-alpha-4.1", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", - "wpilibYear": "2027_alpha5", + "wpilibYear": "2027_alpha7", "mavenUrls": [ "https://3015rangerrobotics.github.io/pathplannerlib/repo" ], @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2027.0.0-alpha-3" + "version": "2027.0.0-alpha-4.1" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2027.0.0-alpha-3", + "version": "2027.0.0-alpha-4.1", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/Phoenix6-26.50.0-alpha-1.json b/vendordeps/Phoenix6-26.70.0-alpha-2.json similarity index 89% rename from vendordeps/Phoenix6-26.50.0-alpha-1.json rename to vendordeps/Phoenix6-26.70.0-alpha-2.json index 40dd410..9fa030a 100644 --- a/vendordeps/Phoenix6-26.50.0-alpha-1.json +++ b/vendordeps/Phoenix6-26.70.0-alpha-2.json @@ -1,8 +1,8 @@ { - "fileName": "Phoenix6-26.50.0-alpha-1.json", + "fileName": "Phoenix6-26.70.0-alpha-2.json", "name": "CTRE-Phoenix (v6)", - "version": "26.50.0-alpha-1", - "wpilibYear": "2027_alpha5", + "version": "26.70.0-alpha-2", + "wpilibYear": "2027_alpha7", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ "https://maven.ctr-electronics.com/release/" @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "26.50.0-alpha-1" + "version": "26.70.0-alpha-2" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "26.50.0-alpha-1", + "version": "26.70.0-alpha-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.50.0-alpha-1", + "version": "26.70.0-alpha-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "26.50.0-alpha-1", + "version": "26.70.0-alpha-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.50.0-alpha-1", + "version": "26.70.0-alpha-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.50.0-alpha-1", + "version": "26.70.0-alpha-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.50.0-alpha-1", + "version": "26.70.0-alpha-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.50.0-alpha-1", + "version": "26.70.0-alpha-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.50.0-alpha-1", + "version": "26.70.0-alpha-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.50.0-alpha-1", + "version": "26.70.0-alpha-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.50.0-alpha-1", + "version": "26.70.0-alpha-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.50.0-alpha-1", + "version": "26.70.0-alpha-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.50.0-alpha-1", + "version": "26.70.0-alpha-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +194,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.50.0-alpha-1", + "version": "26.70.0-alpha-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -208,7 +208,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.50.0-alpha-1", + "version": "26.70.0-alpha-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -224,7 +224,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "26.50.0-alpha-1", + "version": "26.70.0-alpha-2", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -240,7 +240,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.50.0-alpha-1", + "version": "26.70.0-alpha-2", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -256,7 +256,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "26.50.0-alpha-1", + "version": "26.70.0-alpha-2", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -272,7 +272,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.50.0-alpha-1", + "version": "26.70.0-alpha-2", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -288,7 +288,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.50.0-alpha-1", + "version": "26.70.0-alpha-2", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -304,7 +304,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.50.0-alpha-1", + "version": "26.70.0-alpha-2", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -320,7 +320,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.50.0-alpha-1", + "version": "26.70.0-alpha-2", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -336,7 +336,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.50.0-alpha-1", + "version": "26.70.0-alpha-2", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -352,7 +352,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.50.0-alpha-1", + "version": "26.70.0-alpha-2", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -368,7 +368,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.50.0-alpha-1", + "version": "26.70.0-alpha-2", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -384,7 +384,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.50.0-alpha-1", + "version": "26.70.0-alpha-2", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -400,7 +400,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.50.0-alpha-1", + "version": "26.70.0-alpha-2", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -416,7 +416,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.50.0-alpha-1", + "version": "26.70.0-alpha-2", "libName": "CTRE_SimProCANdi", "headerClassifier": "headers", "sharedLibrary": true, @@ -432,7 +432,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.50.0-alpha-1", + "version": "26.70.0-alpha-2", "libName": "CTRE_SimProCANdle", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index 2b6bd6b..8e9d07b 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,8 +1,8 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2027.0.0-alpha-4", - "wpilibYear": "2027_alpha5", + "version": "2027.0.0-alpha-8", + "wpilibYear": "2027_alpha7", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ "https://maven.revrobotics.com/" @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2027.0.0-alpha-4" + "version": "2027.0.0-alpha-8" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2027.0.0-alpha-4", + "version": "2027.0.0-alpha-8", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -33,7 +33,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibBackendDriver", - "version": "2027.0.0-alpha-4", + "version": "2027.0.0-alpha-8", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -47,7 +47,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibWpiBackendDriver", - "version": "2027.0.0-alpha-4", + "version": "2027.0.0-alpha-8", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -63,7 +63,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2027.0.0-alpha-4", + "version": "2027.0.0-alpha-8", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -79,7 +79,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2027.0.0-alpha-4", + "version": "2027.0.0-alpha-8", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, @@ -95,7 +95,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibBackendDriver", - "version": "2027.0.0-alpha-4", + "version": "2027.0.0-alpha-8", "libName": "BackendDriver", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -110,7 +110,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibWpiBackendDriver", - "version": "2027.0.0-alpha-4", + "version": "2027.0.0-alpha-8", "libName": "REVLibWpi", "sharedLibrary": true, "skipInvalidPlatforms": true, diff --git a/vendordeps/URCL.json b/vendordeps/URCL.json index f623d34..a46325d 100644 --- a/vendordeps/URCL.json +++ b/vendordeps/URCL.json @@ -2,7 +2,7 @@ "fileName": "URCL.json", "name": "URCL", "version": "2026.0.0", - "wpilibYear": "2027_alpha5", + "wpilibYear": "2027_alpha7", "uuid": "84246d17-a797-4d1e-bd9f-c59cd8d2477c", "mavenUrls": [ "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/" diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 3f6b31f..1cb9ff7 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -3,7 +3,7 @@ "name": "photonlib", "version": "v2027.0.0-alpha-2", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", - "wpilibYear": "2027_alpha5", + "wpilibYear": "2027_alpha7", "mavenUrls": [ "https://maven.photonvision.org/repository/internal", "https://maven.photonvision.org/repository/snapshots" diff --git a/vendordeps/yagsl_commands2.json b/vendordeps/yagsl_commands2.json new file mode 100644 index 0000000..456db22 --- /dev/null +++ b/vendordeps/yagsl_commands2.json @@ -0,0 +1,40 @@ +{ + "fileName": "yagsl_commands2.json", + "name": "YAGSL (Commands v2)", + "version": "2026.10.09", + "wpilibYear": "2027_alpha7", + "uuid": "1ccce5a4-acd2-4d18-bca3-4b8047188400", + "mavenUrls": [ + "https://cdn.yassrobotics.com/" + ], + "jsonUrl": "https://cdn.yassrobotics.com/yagsl_commands2.json", + "javaDependencies": [ + { + "groupId": "swervelib", + "artifactId": "YAGSL-core", + "version": "2026.10.09" + }, + { + "groupId": "swervelib", + "artifactId": "YAGSL-commands2", + "version": "2026.10.09" + } + ], + "cppDependencies": [], + "jniDependencies": [], + "requires": [ + { + "uuid": "a1051e86-a979-4880-a28b-a0d5362d1d96", + "errorMessage": "YAMS (Commands v2) is required!", + "offlineFileName": "yams_commands2.json", + "onlineUrl": "https://cdn.yassrobotics.com/yams_commands2.json" + } + ], + "conflictsWith": [ + { + "uuid": "e2b4dc30-23dc-4981-89ef-d2bb52b08928", + "errorMessage": "YAGSL (Commands v2) cannot be used with YAGSL (Commands v3). Remove one of them.", + "offlineFileName": "yagsl_commands3.json" + } + ] +} diff --git a/vendordeps/yams_commands2.json b/vendordeps/yams_commands2.json new file mode 100644 index 0000000..c68f509 --- /dev/null +++ b/vendordeps/yams_commands2.json @@ -0,0 +1,56 @@ +{ + "fileName": "yams_commands2.json", + "name": "Yet Another Mechanism System (Commands v2)", + "version": "2026.10.22", + "wpilibYear": "2027_alpha7", + "uuid": "a1051e86-a979-4880-a28b-a0d5362d1d96", + "mavenUrls": [ + "https://cdn.yassrobotics.com/" + ], + "jsonUrl": "https://cdn.yassrobotics.com/yams_commands2.json", + "javaDependencies": [ + { + "groupId": "yams", + "artifactId": "YAMS-core", + "version": "2026.10.22" + }, + { + "groupId": "yams", + "artifactId": "YAMS-commands2", + "version": "2026.10.22" + } + ], + "cppDependencies": [ + { + "groupId": "yams", + "artifactId": "YAMS-cpp", + "version": "2026.10.22", + "libName": "YAMS", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxsystemcore", + "osxuniversal" + ] + } + ], + "jniDependencies": [], + "requires": [ + { + "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", + "errorMessage": "Commands V2 is required! Install it from the WPILib vendor libraries.", + "offlineFileName": "CommandsV2.json" + } + ], + "conflictsWith": [ + { + "uuid": "9eadd383-6bdc-4107-957a-b50cd3146f17", + "errorMessage": "Yet Another Mechanism System (Commands v2) cannot be used with Yet Another Mechanism System (Commands v3). Remove one of them.", + "offlineFileName": "yams_commands3.json" + } + ] +} From ebd757d88f0b88b38262ff7b76a9e4fbcea7c4a6 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Fri, 30 Oct 2026 11:49:24 -0700 Subject: [PATCH 02/10] Complete alpha-8 utility, CAN health, and dashboard follow-ups Correct timestamp units across robot and vision code after the 2027 API transition. RobotController.getTime(), NetworkTables Java sample timestamps, and AdvantageKit's clock now use nanoseconds; the old TimeUtil path treated raw time as microseconds. Replace TimeUtil call sites with WPILib Timer seconds, including drive, odometry, accelerometer, simulation, and power monitoring. Use Timer for Spark and Phoenix odometry sample timestamps. In Limelight IO, convert NetworkTables sample timestamps from nanoseconds to seconds, express heartbeat and orientation intervals in nanoseconds, and handle clock reversal. Apply nanosecond update intervals and an initial- sample sentinel guard to PhotonVision simulation. Clamp the power monitor's elapsed time at zero so a clock reset cannot subtract accumulated usage. Replace the local MathUtil helpers with WPILib applyDeadband, angleModulus, inputModulus, and lerp at their callers. Remove the unused GeomUtil, and remove MathUtil and TimeUtil after migrating their call sites. Keep the concurrent interpolation buffer because Drive currently uses its thread-safe access and oldest/newest/range queries; add a TODO to recheck whether a future WPILib buffer can provide those properties. Replace LoggedTunableNumber callers with AdvantageKit LoggedNetworkNumber behind DoubleSupplier fields. Preserve the existing /Tuning dashboard paths, tuning-mode defaults, and recorded-input replay behavior for battery amp- hours and flywheel speed. Pass the battery supplier into RBSIPowerMonitor. Keep the monitor's Conduit snapshot, current aggregation, battery estimate, energy integration, and alerts: WPILib PowerDistribution total power and energy are unsupported for the configured REV PDH and would return zero. Read CAN bus utilization, TX-full, receive-error, transmit-error, and bus-off status through RobotController.getCANStatus on the matching Systemcore CANPort. Retain the existing CAN log paths below each bus name. Remove the health-only CANBusLike adapters and simulation stubs from RBSICANBusRegistry while retaining real Phoenix CANBus instances for CTRE device constructors. Simplify the simulation/replay registry initializer. The WPILib receive/transmit error counters may not be directly comparable to the previous Phoenix REC/TEC values in historical logs. Refresh both root AdvantageScope layouts for the current swerve telemetry names and struct types: SwerveChassisVelocities, ChassisVelocities, SwerveModuleVelocity, and each module's velocity field. Point vision field traces to TagsSeenThisLoop and per-camera InjectedPose2d, and remove stale vision debug traces with no current output. Use the current estimated slot feedforward key. Expand CAN health plotting to the four active Systemcore buses, can_s1 through can_s4, with their utilization and error counters. Update unit tests for the WPILib math functions and nanosecond Limelight sample/heartbeat behavior. Add a simulation smoke check for native Systemcore CAN status polling. Verify with ./gradlew test, JSON parsing of both AdvantageScope layouts, and git diff --check. Co-authored-by: Codex --- AdvantageScope RBSI Standard.json | 219 ++++++++++++------ AdvantageScope Swerve Calibration.json | 22 +- src/main/java/frc/robot/RobotContainer.java | 22 +- .../frc/robot/commands/DriveCommands.java | 2 +- .../accelerometer/Accelerometer.java | 4 +- .../frc/robot/subsystems/drive/Drive.java | 16 +- .../robot/subsystems/drive/DriveOdometry.java | 12 +- .../robot/subsystems/drive/ModuleIOSim.java | 4 +- .../drive/PhoenixOdometryThread.java | 4 +- .../subsystems/drive/SparkOdometryThread.java | 4 +- .../subsystems/drive/SparkSwerveUnits.java | 2 +- .../frc/robot/subsystems/imu/ImuIOSim.java | 4 +- .../subsystems/vision/VisionIOLimelight.java | 22 +- .../vision/VisionIOPhotonVisionSim.java | 12 +- .../ConcurrentTimeInterpolatableBuffer.java | 8 +- src/main/java/frc/robot/util/GeomUtil.java | 164 ------------- .../frc/robot/util/LoggedTunableNumber.java | 134 ----------- src/main/java/frc/robot/util/MathUtil.java | 58 ----- .../frc/robot/util/RBSICANBusRegistry.java | 83 +------ .../java/frc/robot/util/RBSICANHealth.java | 23 +- .../java/frc/robot/util/RBSIPowerMonitor.java | 19 +- src/main/java/frc/robot/util/TimeUtil.java | 26 --- .../robot/commands/DriveCommandsTests.java | 2 +- .../subsystems/vision/VisionIOTests.java | 11 +- src/test/frc/robot/util/UtilTests.java | 22 +- 25 files changed, 277 insertions(+), 622 deletions(-) delete mode 100644 src/main/java/frc/robot/util/GeomUtil.java delete mode 100644 src/main/java/frc/robot/util/LoggedTunableNumber.java delete mode 100644 src/main/java/frc/robot/util/MathUtil.java delete mode 100644 src/main/java/frc/robot/util/TimeUtil.java diff --git a/AdvantageScope RBSI Standard.json b/AdvantageScope RBSI Standard.json index da386f6..3b9f6bd 100644 --- a/AdvantageScope RBSI Standard.json +++ b/AdvantageScope RBSI Standard.json @@ -11,15 +11,9 @@ "expanded": [ "/RealOutputs", "/RealOutputs/Drive/OdomThread", - "/RealOutputs/CAN/DriveTrain", - "/RealOutputs/CAN/Module0", "/AdvantageKit", "/RealOutputs/Odometry/Yaw", "/RealOutputs/Vision/Camera0", - "/RealOutputs/Vision/Camera0/TagPoses", - "/RealOutputs/Vision/Summary/TagPoses", - "/RealOutputs/Vision/Camera1/RobotPosesAccepted", - "/RealOutputs/Vision/Camera0/RobotPosesAccepted", "/RealOutputs/Power", "/RealOutputs/Power/Subsystems", "/NetworkInputs/SmartDashboard", @@ -27,11 +21,15 @@ "/RealOutputs/LogPeriodic/CodeLoop", "/RealOutputs/LogPeriodic/Subsystem", "/RealOutputs/LogPeriodic/VirtualSubsystem", - "/RealOutputs/CAN/rio", "/RealOutputs/Odometry/Coast", "/RealOutputs/Drive/SparkOdomThread", "/RealOutputs/Vision/Debug", - "/RealOutputs/Swerve/Drive" + "/RealOutputs/Swerve/Drive", + "/RealOutputs/CAN/can_s1", + "/RealOutputs/CAN/can_s2", + "/RealOutputs/CAN/can_s3", + "/RealOutputs/CAN/can_s4", + "/RealOutputs/Vision/TagsSeenThisLoop" ] }, "tabs": { @@ -84,7 +82,7 @@ }, { "type": "arrow", - "logKey": "/RealOutputs/Vision/Summary/TagPoses", + "logKey": "/RealOutputs/Vision/TagsSeenThisLoop", "logType": "Pose3d[]", "visible": true, "options": { @@ -93,8 +91,8 @@ }, { "type": "ghost", - "logKey": "/RealOutputs/Vision/Camera0/RobotPosesAccepted", - "logType": "Pose3d[]", + "logKey": "/RealOutputs/Vision/Camera0/InjectedPose2d", + "logType": "Pose2d", "visible": true, "options": { "color": "#00ff00" @@ -102,8 +100,8 @@ }, { "type": "ghost", - "logKey": "/RealOutputs/Vision/Camera1/RobotPosesAccepted", - "logType": "Pose3d[]", + "logKey": "/RealOutputs/Vision/Camera1/InjectedPose2d", + "logType": "Pose2d", "visible": true, "options": { "color": "#ffff00" @@ -321,7 +319,7 @@ }, { "type": "arrow", - "logKey": "/ReplayOutputs/Vision/Summary/TagPoses", + "logKey": "/ReplayOutputs/Vision/TagsSeenThisLoop", "logType": "Pose3d[]", "visible": true, "options": { @@ -330,8 +328,8 @@ }, { "type": "ghost", - "logKey": "/ReplayOutputs/Vision/Camera0/RobotPosesAccepted", - "logType": "Pose3d[]", + "logKey": "/ReplayOutputs/Vision/Camera0/InjectedPose2d", + "logType": "Pose2d", "visible": true, "options": { "color": "#00ff00" @@ -491,26 +489,6 @@ "color": "#e5b31b", "size": "normal" } - }, - { - "type": "stepped", - "logKey": "/RealOutputs/Vision/Debug/dTransFromLastVision", - "logType": "Number", - "visible": true, - "options": { - "color": "#af2437", - "size": "normal" - } - }, - { - "type": "stepped", - "logKey": "/RealOutputs/Vision/Debug/dRotFromLastVision", - "logType": "Number", - "visible": true, - "options": { - "color": "#80588e", - "size": "normal" - } } ], "rightSources": [ @@ -533,16 +511,6 @@ "color": "#c0b487", "size": "normal" } - }, - { - "type": "stepped", - "logKey": "/RealOutputs/Vision/Debug/disabledCoastAge", - "logType": "Number", - "visible": true, - "options": { - "color": "#858584", - "size": "normal" - } } ], "discreteSources": [ @@ -573,15 +541,6 @@ "color": "#80588e" } }, - { - "type": "stripes", - "logKey": "/RealOutputs/Vision/Debug/disabledCoast", - "logType": "Boolean", - "visible": true, - "options": { - "color": "#e48b32" - } - }, { "type": "stripes", "logKey": "/DriverStation/Enabled", @@ -617,7 +576,7 @@ { "type": "states", "logKey": "/RealOutputs/SwerveStates/Measured", - "logType": "SwerveModuleState[]", + "logType": "SwerveModuleVelocity[]", "visible": true, "options": { "color": "#ff0000", @@ -627,7 +586,7 @@ { "type": "states", "logKey": "/RealOutputs/SwerveStates/SetpointsOptimized", - "logType": "SwerveModuleState[]", + "logType": "SwerveModuleVelocity[]", "visible": true, "options": { "color": "#00ffff", @@ -643,8 +602,8 @@ }, { "type": "chassisSpeeds", - "logKey": "/RealOutputs/SwerveChassisSpeeds/Measured", - "logType": "ChassisSpeeds", + "logKey": "/RealOutputs/SwerveChassisVelocities/Measured", + "logType": "ChassisVelocities", "visible": true, "options": { "color": "#ff0000" @@ -709,7 +668,7 @@ }, { "type": "stepped", - "logKey": "/RealOutputs/Swerve/Drive/FeedForwardVolts", + "logKey": "/RealOutputs/Swerve/Drive/EstimatedSlotFeedForwardVolts", "logType": "Number", "visible": true, "options": { @@ -977,12 +936,12 @@ }, { "type": 1, - "title": "CAN Health (utilization / counts)", + "title": "Systemcore CAN Health (utilization / counts)", "controller": { "leftSources": [ { "type": "stepped", - "logKey": "/RealOutputs/CAN/rio/Utilization", + "logKey": "/RealOutputs/CAN/can_s1/Utilization", "logType": "Number", "visible": true, "options": { @@ -992,19 +951,59 @@ }, { "type": "stepped", - "logKey": "/RealOutputs/CAN/DriveTrain/Utilization", + "logKey": "/RealOutputs/CAN/can_s2/Utilization", "logType": "Number", "visible": true, "options": { "color": "#e5b31b", "size": "normal" } + }, + { + "type": "stepped", + "logKey": "/RealOutputs/CAN/can_s3/Utilization", + "logType": "Number", + "visible": true, + "options": { + "color": "#3b875a", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/RealOutputs/CAN/can_s4/Utilization", + "logType": "Number", + "visible": true, + "options": { + "color": "#80588e", + "size": "normal" + } } ], "rightSources": [ { "type": "stepped", - "logKey": "/RealOutputs/CAN/rio/RxErrorCount", + "logKey": "/RealOutputs/CAN/can_s1/RxErrorCount", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/RealOutputs/CAN/can_s1/TxErrorCount", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/RealOutputs/CAN/can_s1/BusOffCount", "logType": "Number", "visible": true, "options": { @@ -1014,7 +1013,7 @@ }, { "type": "stepped", - "logKey": "/RealOutputs/CAN/rio/TxErrorCount", + "logKey": "/RealOutputs/CAN/can_s1/TxFullCount", "logType": "Number", "visible": true, "options": { @@ -1024,7 +1023,7 @@ }, { "type": "stepped", - "logKey": "/RealOutputs/CAN/rio/BusOffCount", + "logKey": "/RealOutputs/CAN/can_s2/RxErrorCount", "logType": "Number", "visible": true, "options": { @@ -1034,7 +1033,7 @@ }, { "type": "stepped", - "logKey": "/RealOutputs/CAN/DriveTrain/RxErrorCount", + "logKey": "/RealOutputs/CAN/can_s2/TxErrorCount", "logType": "Number", "visible": true, "options": { @@ -1044,7 +1043,7 @@ }, { "type": "stepped", - "logKey": "/RealOutputs/CAN/DriveTrain/TxErrorCount", + "logKey": "/RealOutputs/CAN/can_s2/BusOffCount", "logType": "Number", "visible": true, "options": { @@ -1054,13 +1053,93 @@ }, { "type": "stepped", - "logKey": "/RealOutputs/CAN/DriveTrain/TxFullCount", + "logKey": "/RealOutputs/CAN/can_s2/TxFullCount", "logType": "Number", "visible": true, "options": { "color": "#3b875a", "size": "normal" } + }, + { + "type": "stepped", + "logKey": "/RealOutputs/CAN/can_s3/RxErrorCount", + "logType": "Number", + "visible": true, + "options": { + "color": "#18a999", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/RealOutputs/CAN/can_s3/TxErrorCount", + "logType": "Number", + "visible": true, + "options": { + "color": "#e36414", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/RealOutputs/CAN/can_s3/BusOffCount", + "logType": "Number", + "visible": true, + "options": { + "color": "#6d597a", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/RealOutputs/CAN/can_s3/TxFullCount", + "logType": "Number", + "visible": true, + "options": { + "color": "#457b9d", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/RealOutputs/CAN/can_s4/RxErrorCount", + "logType": "Number", + "visible": true, + "options": { + "color": "#fb8500", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/RealOutputs/CAN/can_s4/TxErrorCount", + "logType": "Number", + "visible": true, + "options": { + "color": "#8d99ae", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/RealOutputs/CAN/can_s4/BusOffCount", + "logType": "Number", + "visible": true, + "options": { + "color": "#b56576", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/RealOutputs/CAN/can_s4/TxFullCount", + "logType": "Number", + "visible": true, + "options": { + "color": "#2a9d8f", + "size": "normal" + } } ], "discreteSources": [ @@ -1369,7 +1448,7 @@ "sources": [ { "type": "robot", - "logKey": "/RealOutputs/Vision/Summary/TagPoses", + "logKey": "/RealOutputs/Vision/TagsSeenThisLoop", "logType": "Pose3d[]", "visible": true, "options": { diff --git a/AdvantageScope Swerve Calibration.json b/AdvantageScope Swerve Calibration.json index a84f77f..fe493c1 100644 --- a/AdvantageScope Swerve Calibration.json +++ b/AdvantageScope Swerve Calibration.json @@ -12,7 +12,7 @@ "/Drive", "/RealOutputs", "/RealOutputs/SwerveStates", - "/RealOutputs/SwerveChassisSpeeds", + "/RealOutputs/SwerveChassisVelocities", "/RealOutputs/Odometry" ] }, @@ -35,7 +35,7 @@ { "type": "states", "logKey": "/RealOutputs/SwerveStates/Measured", - "logType": "SwerveModuleState[]", + "logType": "SwerveModuleVelocity[]", "visible": true, "options": { "color": "#ff0000", @@ -45,7 +45,7 @@ { "type": "states", "logKey": "/RealOutputs/SwerveStates/SetpointsOptimized", - "logType": "SwerveModuleState[]", + "logType": "SwerveModuleVelocity[]", "visible": true, "options": { "color": "#00ffff", @@ -54,8 +54,8 @@ }, { "type": "chassisSpeeds", - "logKey": "/RealOutputs/SwerveChassisSpeeds/Measured", - "logType": "ChassisSpeeds", + "logKey": "/RealOutputs/SwerveChassisVelocities/Measured", + "logType": "ChassisVelocities", "visible": true, "options": { "color": "#ff0000" @@ -63,8 +63,8 @@ }, { "type": "chassisSpeeds", - "logKey": "/RealOutputs/SwerveChassisSpeeds/Setpoints", - "logType": "ChassisSpeeds", + "logKey": "/RealOutputs/SwerveChassisVelocities/Setpoints", + "logType": "ChassisVelocities", "visible": true, "options": { "color": "#00ffff" @@ -323,7 +323,7 @@ "leftSources": [ { "type": "stepped", - "logKey": "/RealOutputs/SwerveStates/Measured/0/speed", + "logKey": "/RealOutputs/SwerveStates/Measured/0/velocity", "logType": "Number", "visible": true, "options": { @@ -333,7 +333,7 @@ }, { "type": "stepped", - "logKey": "/RealOutputs/SwerveStates/SetpointsOptimized/0/speed", + "logKey": "/RealOutputs/SwerveStates/SetpointsOptimized/0/velocity", "logType": "Number", "visible": true, "options": { @@ -368,7 +368,7 @@ "leftSources": [ { "type": "stepped", - "logKey": "/RealOutputs/SwerveChassisSpeeds/Measured/vx", + "logKey": "/RealOutputs/SwerveChassisVelocities/Measured/vx", "logType": "Number", "visible": true, "options": { @@ -378,7 +378,7 @@ }, { "type": "stepped", - "logKey": "/RealOutputs/SwerveChassisSpeeds/Measured/vy", + "logKey": "/RealOutputs/SwerveChassisVelocities/Measured/vy", "logType": "Number", "visible": true, "options": { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0bab0d1..59fe64c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -43,7 +43,6 @@ import frc.robot.subsystems.vision.VisionIOLimelight; import frc.robot.subsystems.vision.VisionIOPhotonVision; import frc.robot.subsystems.vision.VisionIOPhotonVisionSim; -import frc.robot.util.LoggedTunableNumber; import frc.robot.util.OverrideSwitches; import frc.robot.util.RBSICANBusRegistry; import frc.robot.util.RBSICANHealth; @@ -55,7 +54,9 @@ import java.util.Arrays; import java.util.List; import java.util.Set; +import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.networktables.LoggedNetworkChooser; +import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; import org.photonvision.PhotonCamera; import org.photonvision.simulation.PhotonCameraSim; import org.photonvision.simulation.VisionSystemSim; @@ -124,11 +125,16 @@ public class RobotContainer { new LoggedNetworkChooser<>("Drive Style"); // Input estimated battery capacity (if full, use printed value) - private final LoggedTunableNumber batteryCapacity = - new LoggedTunableNumber("Battery Amp-Hours", 18.0); + // AdvantageKit's logged input preserves the selected value during replay. + private final DoubleSupplier batteryCapacity = + Constants.isTuningMode() + ? new LoggedNetworkNumber("/Tuning/Battery Amp-Hours", 18.0) + : () -> 18.0; // EXAMPLE TUNABLE FLYWHEEL SPEED INPUT FROM DASHBOARD - private final LoggedTunableNumber flywheelSpeedInput = - new LoggedTunableNumber("Flywheel Speed", 1500.0); + private final DoubleSupplier flywheelSpeedInput = + Constants.isTuningMode() + ? new LoggedNetworkNumber("/Tuning/Flywheel Speed", 1500.0) + : () -> 1500.0; // Alerts private final Alert aprilTagLayoutAlert = new Alert("aprilTagLayout", "", Level.LOW); @@ -162,7 +168,7 @@ public RobotContainer() { break; case SIM: - RBSICANBusRegistry.initSim(CANBuses.ALL); + RBSICANBusRegistry.initSim(); m_imu = new Imu(new ImuIOSim()); m_drivebase = new Drive(m_imu); @@ -196,7 +202,7 @@ public RobotContainer() { default: // Replayed robot, disable IO implementations - RBSICANBusRegistry.initSim(CANBuses.ALL); + RBSICANBusRegistry.initSim(); m_imu = new Imu(new ImuIO() {}); m_drivebase = new Drive(m_imu); m_driveOdometry = new DriveOdometry(m_drivebase, m_imu, m_drivebase.getModules()); @@ -322,7 +328,7 @@ private void configureBindings() { .rightBumper() .whileTrue( Commands.startEnd( - () -> m_flywheel.runVelocity(flywheelSpeedInput.get()), + () -> m_flywheel.runVelocity(flywheelSpeedInput.getAsDouble()), m_flywheel::stop, m_flywheel)); diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index a3920cb..ad962c5 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -13,7 +13,6 @@ import frc.robot.Constants.OperatorConstants; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.SwerveConstants; -import frc.robot.util.MathUtil; import java.text.DecimalFormat; import java.text.NumberFormat; import java.util.ArrayList; @@ -29,6 +28,7 @@ import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.geometry.Translation2d; import org.wpilib.math.kinematics.ChassisVelocities; +import org.wpilib.math.util.MathUtil; import org.wpilib.math.util.Units; import org.wpilib.system.Timer; diff --git a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java index f66e4df..d1f4365 100644 --- a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java +++ b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java @@ -17,10 +17,10 @@ import frc.robot.Constants.RobotConstants; import frc.robot.Constants.SensorConstants; import frc.robot.subsystems.imu.Imu; -import frc.robot.util.TimeUtil; import frc.robot.util.VirtualSubsystem; import org.littletonrobotics.junction.Logger; import org.wpilib.math.geometry.Translation3d; +import org.wpilib.system.Timer; /** * Accelerometer subsystem (VirtualSubsystem) @@ -104,7 +104,7 @@ public void rbsiPeriodic() { final double[] ts = imuInputs.odometryYawTimestamps; if (ts.length > 0) { - Logger.recordOutput("Odometry/IMULatencySec", TimeUtil.now() - ts[ts.length - 1]); + Logger.recordOutput("Odometry/IMULatencySec", Timer.getTimestamp() - ts[ts.length - 1]); } } } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index ad13876..aa13101 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -32,11 +32,9 @@ import frc.robot.subsystems.imu.Imu; import frc.robot.util.ConcurrentTimeInterpolatableBuffer; import frc.robot.util.LocalADStarAK; -import frc.robot.util.MathUtil; import frc.robot.util.RBSIEnum.Mode; import frc.robot.util.RBSIParsing; import frc.robot.util.RBSISubsystem; -import frc.robot.util.TimeUtil; import frc.robot.util.TimedPose; import java.util.Optional; import java.util.OptionalDouble; @@ -63,7 +61,9 @@ import org.wpilib.math.kinematics.SwerveModulePosition; import org.wpilib.math.kinematics.SwerveModuleVelocity; import org.wpilib.math.trajectory.TrapezoidProfile; +import org.wpilib.math.util.MathUtil; import org.wpilib.smartdashboard.Field2d; +import org.wpilib.system.Timer; import org.wpilib.telemetry.Telemetry; import org.wpilib.util.Alert; import org.wpilib.util.Alert.Level; @@ -731,7 +731,7 @@ public static Translation2d[] getModuleTranslations() { /** Returns whether the robot is currently in the DISABLED_COAST state */ public boolean isDisabledCoast() { - return isDisabledCoast(TimeUtil.now()); + return isDisabledCoast(Timer.getTimestamp()); } /** Returns whether the robot was in the DISABLED_COAST state at time `timestamp` */ @@ -771,7 +771,7 @@ public double getFFCharacterizationVelocity() { * @param pose The specified pose to which to reset the pose estimator */ public void resetPose(Pose2d pose) { - final double now = TimeUtil.now(); + final double now = Timer.getTimestamp(); m_PoseEstimator.resetPosition(getRawGyroHeading(), getModulePositions(), pose); lastAcceptedVisionReceiptTimestamp = Double.NEGATIVE_INFINITY; markPoseReset(now); @@ -786,7 +786,7 @@ public boolean validatePathPlannerAutoStart(Pose2d pathStartPose) { } private PathPlannerStartAction evaluatePathPlannerStart(Pose2d pathStartPose) { - final double now = TimeUtil.now(); + final double now = Timer.getTimestamp(); final double visionAge = now - lastAcceptedVisionReceiptTimestamp; final boolean hasRecentVision = Double.isFinite(visionAge) @@ -875,14 +875,14 @@ public void zeroHeadingForAlliance() { ? Rotation2d.ZERO : Rotation2d.k180deg); resetHeadingController(); - markPoseReset(TimeUtil.now()); + markPoseReset(Timer.getTimestamp()); } /** Zeros the gyro regardless of the alliance */ public void zeroHeading() { imu.zeroYaw(Rotation2d.ZERO); resetHeadingController(); - markPoseReset(TimeUtil.now()); + markPoseReset(Timer.getTimestamp()); } /** @@ -997,7 +997,7 @@ private void applyVisionMeasurementLocked(TimedPose meas) { private void markVisionMeasurementAccepted(double measurementTimestamp) { lastAcceptedVisionMeasurementTimestamp = measurementTimestamp; - lastAcceptedVisionReceiptTimestamp = TimeUtil.now(); + lastAcceptedVisionReceiptTimestamp = Timer.getTimestamp(); } /** diff --git a/src/main/java/frc/robot/subsystems/drive/DriveOdometry.java b/src/main/java/frc/robot/subsystems/drive/DriveOdometry.java index e66b4cc..aa09c70 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveOdometry.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveOdometry.java @@ -22,13 +22,13 @@ import frc.robot.Constants; import frc.robot.subsystems.imu.Imu; import frc.robot.util.RBSIEnum.Mode; -import frc.robot.util.TimeUtil; import frc.robot.util.VirtualSubsystem; import java.util.ArrayList; import org.littletonrobotics.junction.Logger; import org.wpilib.driverstation.RobotState; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.kinematics.SwerveModulePosition; +import org.wpilib.system.Timer; public final class DriveOdometry extends VirtualSubsystem { private static final int MAX_VISION_MEASUREMENTS_PER_LOOP = 2; @@ -124,7 +124,7 @@ public void rbsiPeriodic() { // Pure SIM (not replaying a log): use sim pose/yaw // ---------------------------------------------------------------------- if (Constants.isPureSim()) { - final double now = TimeUtil.now(); + final double now = Timer.getTimestamp(); // Keep buffers alive drive.poseBufferAddSample(now, drive.getSimPose()); @@ -146,7 +146,7 @@ public void rbsiPeriodic() { // measurements can still be applied while the robot is carried or repositioned. // ---------------------------------------------------------------------- if (RobotState.isDisabled() && !isReplayActive) { - final double now = TimeUtil.now(); + final double now = Timer.getTimestamp(); // keep yaw buffers alive if (imuInputs.connected) { @@ -182,7 +182,7 @@ public void rbsiPeriodic() { // Always keep yaw buffers “alive” even if no samples if (n == 0) { if (Constants.getMode() != Mode.REPLAY) { - final double now = TimeUtil.now(); + final double now = Timer.getTimestamp(); drive.yawBuffersAddSample(now, imuInputs.yawPositionRad, imuInputs.yawRateRadPerSec); // Coast state update (no per-sample positions available; use current) @@ -233,7 +233,7 @@ public void rbsiPeriodic() { if (hasYawQueue && !yawIndexAligned) { drive.yawBuffersFillFromQueue(yawTs, yawPos); } else if (!hasYawQueue) { - final double now = TimeUtil.now(); + final double now = Timer.getTimestamp(); drive.yawBuffersAddSample(now, imuInputs.yawPositionRad, imuInputs.yawRateRadPerSec); } @@ -282,7 +282,7 @@ public void rbsiPeriodic() { if (logDebug && i == n - 1) { Logger.recordOutput("Odometry/Debug/timestamp", t); - Logger.recordOutput("Odometry/Debug/now", TimeUtil.now()); + Logger.recordOutput("Odometry/Debug/now", Timer.getTimestamp()); if (i > 0) { Logger.recordOutput("Odometry/Debug/timeNowDiff", t - ts[i - 1]); } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java index 0c111c9..abd5073 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -10,13 +10,13 @@ package frc.robot.subsystems.drive; import frc.robot.Constants; -import frc.robot.util.TimeUtil; import org.wpilib.math.controller.PIDController; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.system.DCMotor; import org.wpilib.math.system.Models; import org.wpilib.math.util.Units; import org.wpilib.simulation.DCMotorSim; +import org.wpilib.system.Timer; /** * Physics sim implementation of module IO. The sim models are configured using a set of module @@ -113,7 +113,7 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.turnCurrentAmps = Math.abs(turnSim.getCurrentDraw()); // Odometry (single sample per loop is fine) - inputs.odometryTimestamps = new double[] {TimeUtil.now()}; + inputs.odometryTimestamps = new double[] {Timer.getTimestamp()}; inputs.odometryDrivePositionsRad = new double[] {mechanismPositionRad}; inputs.odometryTurnPositions = new Rotation2d[] {inputs.turnPosition}; } diff --git a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java index 4dfb496..8a28c3d 100644 --- a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java @@ -22,7 +22,7 @@ import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.Logger; import org.wpilib.driverstation.DriverStationErrors; -import org.wpilib.system.RobotController; +import org.wpilib.system.Timer; import org.wpilib.units.measure.Angle; /** @@ -157,7 +157,7 @@ public void run() { // Sample timestamp is current FPGA time minus average CAN latency // Default timestamps from Phoenix are NOT compatible with // FPGA timestamps, this solution is imperfect but close - double timestamp = RobotController.getTime() / 1e6; + double timestamp = Timer.getTimestamp(); double totalLatency = 0.0; for (BaseStatusSignal signal : phoenixSignals) { totalLatency += signal.getTimestamp().getLatency(); diff --git a/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java index 00e9b4a..b4a10b6 100644 --- a/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java @@ -17,7 +17,7 @@ import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.Logger; import org.wpilib.system.Notifier; -import org.wpilib.system.RobotController; +import org.wpilib.system.Timer; /** * Provides an interface for asynchronously reading high-frequency measurements to a set of queues. @@ -101,7 +101,7 @@ private void run() { Drive.odometryLock.lock(); try { // Get sample timestamp - double timestamp = RobotController.getTime() / 1e6; + double timestamp = Timer.getTimestamp(); // Read Spark values, mark invalid in case of error double[] sparkValues = new double[sparkSignals.size()]; diff --git a/src/main/java/frc/robot/subsystems/drive/SparkSwerveUnits.java b/src/main/java/frc/robot/subsystems/drive/SparkSwerveUnits.java index 7b5f091..670c826 100644 --- a/src/main/java/frc/robot/subsystems/drive/SparkSwerveUnits.java +++ b/src/main/java/frc/robot/subsystems/drive/SparkSwerveUnits.java @@ -1,7 +1,7 @@ package frc.robot.subsystems.drive; -import frc.robot.util.MathUtil; import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.util.MathUtil; /** Unit conversions for REVLib 2027 SPARK encoders and closed-loop controllers. */ final class SparkSwerveUnits { diff --git a/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java b/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java index a9182ad..21d9281 100644 --- a/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java +++ b/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java @@ -17,10 +17,10 @@ package frc.robot.subsystems.imu; -import frc.robot.util.TimeUtil; import org.littletonrobotics.junction.Logger; import org.wpilib.math.geometry.Translation3d; import org.wpilib.math.util.Units; +import org.wpilib.system.Timer; /** Simulated IMU for full robot simulation & replay logging */ public class ImuIOSim implements ImuIO { @@ -73,7 +73,7 @@ public void updateInputs(ImuIOInputs inputs) { inputs.linearJerk = Translation3d.ZERO; // Maintain odometry history - pushOdomSample(TimeUtil.now(), yawRad); + pushOdomSample(Timer.getTimestamp(), yawRad); // Export odometry arrays (copy out in chronological order) final int n = odomSize; diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java index 0ed6ee6..f640b11 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java @@ -35,8 +35,9 @@ public class VisionIOLimelight implements VisionIO { private static final int BOTPOSE_FIRST_TAG_ID_INDEX = 11; private static final int BOTPOSE_FIRST_TAG_AMBIGUITY_INDEX = 17; private static final int BOTPOSE_TAG_STRIDE = 7; - private static final long ORIENTATION_FLUSH_PERIOD_US = 20_000; - private static long lastOrientationFlushUs = 0; + private static final long ORIENTATION_FLUSH_PERIOD_NS = 20_000_000; + private static final long HEARTBEAT_TIMEOUT_NS = 250_000_000; + private static long lastOrientationFlushNs = 0; private final Supplier rotationSupplier; private final DoubleArrayPublisher orientationPublisher; @@ -69,7 +70,7 @@ public VisionIOLimelight(String name, Supplier rotationSupplier) { public void updateInputs(VisionIOInputs inputs) { // Update connection status based on whether an update has been seen in the last 250ms inputs.connected = - ((RobotController.getTime() - heartbeatSubscriber.getLastChange()) / 1000) < 250; + isHeartbeatRecent(RobotController.getTime(), heartbeatSubscriber.getLastChange()); // Update target observation inputs.latestTargetObservation = @@ -207,8 +208,12 @@ static double timestampSeconds(TimestampedDoubleArray rawSample) { return timestampSeconds(rawSample.timestamp, rawSample.value); } - static double timestampSeconds(long ntTimestampMicros, double[] rawLLArray) { - return ntTimestampMicros * 1.0e-6 - rawLLArray[BOTPOSE_LATENCY_INDEX] * 1.0e-3; + static double timestampSeconds(long ntTimestampNanos, double[] rawLLArray) { + return ntTimestampNanos * 1.0e-9 - rawLLArray[BOTPOSE_LATENCY_INDEX] * 1.0e-3; + } + + static boolean isHeartbeatRecent(long nowNs, long lastChangeNs) { + return lastChangeNs > 0 && nowNs >= lastChangeNs && nowNs - lastChangeNs < HEARTBEAT_TIMEOUT_NS; } private static int getTagCount(double[] rawLLArray) { @@ -216,10 +221,11 @@ private static int getTagCount(double[] rawLLArray) { } private static void flushOrientationIfDue() { - long nowUs = RobotController.getTime(); - if (nowUs - lastOrientationFlushUs < ORIENTATION_FLUSH_PERIOD_US) return; + long nowNs = RobotController.getTime(); + if (nowNs >= lastOrientationFlushNs + && nowNs - lastOrientationFlushNs < ORIENTATION_FLUSH_PERIOD_NS) return; NetworkTableInstance.getDefault().flush(); - lastOrientationFlushUs = nowUs; + lastOrientationFlushNs = nowNs; } } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java index 575b33d..8da10c9 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java @@ -21,8 +21,8 @@ /** IO implementation for physics sim using PhotonVision simulator. */ public class VisionIOPhotonVisionSim extends VisionIOPhotonVision { private static VisionSystemSim visionSim; - private static long lastUpdateUs = Long.MIN_VALUE; - private static final long MIN_UPDATE_PERIOD_US = 1_000; + private static long lastUpdateNs = Long.MIN_VALUE; + private static final long MIN_UPDATE_PERIOD_NS = 1_000_000; private final Supplier poseSupplier; @@ -73,10 +73,12 @@ public void updateInputs(VisionIOInputs inputs) { } private void updateVisionSimOncePerLoop() { - long nowUs = RobotController.getTime(); - if (nowUs - lastUpdateUs < MIN_UPDATE_PERIOD_US) return; + long nowNs = RobotController.getTime(); + if (lastUpdateNs != Long.MIN_VALUE + && nowNs >= lastUpdateNs + && nowNs - lastUpdateNs < MIN_UPDATE_PERIOD_NS) return; visionSim.update(poseSupplier.get()); - lastUpdateUs = nowUs; + lastUpdateNs = nowNs; } } diff --git a/src/main/java/frc/robot/util/ConcurrentTimeInterpolatableBuffer.java b/src/main/java/frc/robot/util/ConcurrentTimeInterpolatableBuffer.java index 347b9c3..cc930d1 100644 --- a/src/main/java/frc/robot/util/ConcurrentTimeInterpolatableBuffer.java +++ b/src/main/java/frc/robot/util/ConcurrentTimeInterpolatableBuffer.java @@ -28,11 +28,15 @@ import java.util.concurrent.ConcurrentSkipListMap; import org.wpilib.math.interpolation.Interpolatable; import org.wpilib.math.interpolation.Interpolator; +import org.wpilib.math.util.MathUtil; /** - * A concurrent version of WPIlib's TimeInterpolatableBuffer class to avoid the need for explicit + * A concurrent version of WPILib's TimeInterpolatableBuffer class to avoid the need for explicit * synchronization in our robot code. * + *

TODO: Revisit whether this wrapper is still needed when WPILib provides safe concurrent access + * and the oldest/newest/range queries used by Drive. + * * @param The type stored in this buffer. */ public final class ConcurrentTimeInterpolatableBuffer { @@ -80,7 +84,7 @@ public static > ConcurrentTimeInterpolatableBuffer createDoubleBuffer( double historySizeSeconds) { - return new ConcurrentTimeInterpolatableBuffer<>(MathUtil::interpolate, historySizeSeconds); + return new ConcurrentTimeInterpolatableBuffer<>(MathUtil::lerp, historySizeSeconds); } /** diff --git a/src/main/java/frc/robot/util/GeomUtil.java b/src/main/java/frc/robot/util/GeomUtil.java deleted file mode 100644 index b23abd3..0000000 --- a/src/main/java/frc/robot/util/GeomUtil.java +++ /dev/null @@ -1,164 +0,0 @@ -// Copyright (c) 2024-2026 Az-FIRST -// http://github.com/AZ-First -// Copyright (c) 2021-2026 Littleton Robotics -// http://github.com/Mechanical-Advantage -// -// Use of this source code is governed by a BSD -// license that can be found in the AdvantageKit-License.md file -// at the root directory of this project. - -package frc.robot.util; - -import org.wpilib.math.geometry.Pose2d; -import org.wpilib.math.geometry.Pose3d; -import org.wpilib.math.geometry.Rotation2d; -import org.wpilib.math.geometry.Transform2d; -import org.wpilib.math.geometry.Transform3d; -import org.wpilib.math.geometry.Translation2d; -import org.wpilib.math.geometry.Twist2d; -import org.wpilib.math.kinematics.ChassisVelocities; - -/** Geometry utilities for working with translations, rotations, transforms, and poses. */ -public class GeomUtil { - /** - * Creates a pure translating transform - * - * @param translation The translation to create the transform with - * @return The resulting transform - */ - public static Transform2d toTransform2d(Translation2d translation) { - return new Transform2d(translation, Rotation2d.ZERO); - } - - /** - * Creates a pure translating transform - * - * @param x The x coordinate of the translation - * @param y The y coordinate of the translation - * @return The resulting transform - */ - public static Transform2d toTransform2d(double x, double y) { - return new Transform2d(x, y, Rotation2d.ZERO); - } - - /** - * Creates a pure rotating transform - * - * @param rotation The rotation to create the transform with - * @return The resulting transform - */ - public static Transform2d toTransform2d(Rotation2d rotation) { - return new Transform2d(Translation2d.ZERO, rotation); - } - - /** - * Converts a Pose2d to a Transform2d to be used in a kinematic chain - * - * @param pose The pose that will represent the transform - * @return The resulting transform - */ - public static Transform2d toTransform2d(Pose2d pose) { - return new Transform2d(pose.getTranslation(), pose.getRotation()); - } - - public static Pose2d inverse(Pose2d pose) { - Rotation2d rotationInverse = pose.getRotation().unaryMinus(); - return new Pose2d( - pose.getTranslation().unaryMinus().rotateBy(rotationInverse), rotationInverse); - } - - /** - * Converts a Transform2d to a Pose2d to be used as a position or as the start of a kinematic - * chain - * - * @param transform The transform that will represent the pose - * @return The resulting pose - */ - public static Pose2d toPose2d(Transform2d transform) { - return new Pose2d(transform.getTranslation(), transform.getRotation()); - } - - /** - * Creates a pure translated pose - * - * @param translation The translation to create the pose with - * @return The resulting pose - */ - public static Pose2d toPose2d(Translation2d translation) { - return new Pose2d(translation, Rotation2d.ZERO); - } - - /** - * Creates a pure rotated pose - * - * @param rotation The rotation to create the pose with - * @return The resulting pose - */ - public static Pose2d toPose2d(Rotation2d rotation) { - return new Pose2d(Translation2d.ZERO, rotation); - } - - /** - * Multiplies a twist by a scaling factor - * - * @param twist The twist to multiply - * @param factor The scaling factor for the twist components - * @return The new twist - */ - public static Twist2d multiply(Twist2d twist, double factor) { - return new Twist2d(twist.dx * factor, twist.dy * factor, twist.dtheta * factor); - } - - /** - * Converts a Pose3d to a Transform3d to be used in a kinematic chain - * - * @param pose The pose that will represent the transform - * @return The resulting transform - */ - public static Transform3d toTransform3d(Pose3d pose) { - return new Transform3d(pose.getTranslation(), pose.getRotation()); - } - - /** - * Converts a Transform3d to a Pose3d to be used as a position or as the start of a kinematic - * chain - * - * @param transform The transform that will represent the pose - * @return The resulting pose - */ - public static Pose3d toPose3d(Transform3d transform) { - return new Pose3d(transform.getTranslation(), transform.getRotation()); - } - - /** - * Converts a ChassisVelocities to a Twist2d by extracting two dimensions (Y and Z). chain - * - * @param speeds The original translation - * @return The resulting translation - */ - public static Twist2d toTwist2d(ChassisVelocities speeds) { - return new Twist2d(speeds.vx, speeds.vy, speeds.omega); - } - - /** - * Creates a new pose from an existing one using a different translation value. - * - * @param pose The original pose - * @param translation The new translation to use - * @return The new pose with the new translation and original rotation - */ - public static Pose2d withTranslation(Pose2d pose, Translation2d translation) { - return new Pose2d(translation, pose.getRotation()); - } - - /** - * Creates a new pose from an existing one using a different rotation value. - * - * @param pose The original pose - * @param rotation The new rotation to use - * @return The new pose with the original translation and new rotation - */ - public static Pose2d withRotation(Pose2d pose, Rotation2d rotation) { - return new Pose2d(pose.getTranslation(), rotation); - } -} diff --git a/src/main/java/frc/robot/util/LoggedTunableNumber.java b/src/main/java/frc/robot/util/LoggedTunableNumber.java deleted file mode 100644 index b9c7b1b..0000000 --- a/src/main/java/frc/robot/util/LoggedTunableNumber.java +++ /dev/null @@ -1,134 +0,0 @@ -// Copyright (c) 2024-2026 Az-FIRST -// http://github.com/AZ-First -// Copyright (c) 2021-2026 Littleton Robotics -// http://github.com/Mechanical-Advantage -// -// Use of this source code is governed by a BSD -// license that can be found in the AdvantageKit-License.md file -// at the root directory of this project. - -package frc.robot.util; - -import frc.robot.Constants; -import java.util.Arrays; -import java.util.HashMap; -import java.util.Map; -import java.util.function.Consumer; -import java.util.function.DoubleSupplier; -import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; - -/** - * Replay-safe tunable number backed by AdvantageKit NetworkTables inputs. - * - *

When tuning mode is enabled, the live value is read from {@code /Tuning/...}. Otherwise, the - * default value is returned. - */ -public class LoggedTunableNumber implements DoubleSupplier { - private static final String tableKey = "/Tuning"; - - private final String key; - private boolean hasDefault = false; - private double defaultValue; - private LoggedNetworkNumber dashboardNumber; - private final Map lastHasChangedValues = new HashMap<>(); - - /** - * Create a new LoggedTunableNumber. - * - * @param dashboardKey Key under /Tuning - */ - public LoggedTunableNumber(String dashboardKey) { - this.key = tableKey + "/" + dashboardKey; - } - - /** - * Create a new LoggedTunableNumber with a default value. - * - * @param dashboardKey Key under /Tuning - * @param defaultValue Default value - */ - public LoggedTunableNumber(String dashboardKey, double defaultValue) { - this(dashboardKey); - initDefault(defaultValue); - } - - /** - * Set the default value. Can only be set once. - * - * @param defaultValue The default value - */ - public void initDefault(double defaultValue) { - if (hasDefault) { - return; - } - - hasDefault = true; - this.defaultValue = defaultValue; - - if (isTuningMode()) { - dashboardNumber = new LoggedNetworkNumber(key, defaultValue); - } - } - - /** - * Get the current value. - * - * @return Live tuning value in tuning mode, otherwise the default - */ - public double get() { - if (!hasDefault) { - return 0.0; - } - - if (dashboardNumber != null) { - return dashboardNumber.get(); - } - - return defaultValue; - } - - /** - * Returns true when this value has changed since the last check by the given caller. - * - * @param id Unique identifier for the caller, often hashCode() - */ - public boolean hasChanged(int id) { - double currentValue = get(); - Double lastValue = lastHasChangedValues.get(id); - - if (lastValue == null || Double.compare(currentValue, lastValue) != 0) { - lastHasChangedValues.put(id, currentValue); - return true; - } - - return false; - } - - /** - * Runs an action if any of the tunable numbers changed. - * - * @param id Unique identifier for the caller, often hashCode() - * @param action Callback with values in the same order as provided - * @param tunableNumbers Tunable numbers to watch - */ - public static void ifChanged( - int id, Consumer action, LoggedTunableNumber... tunableNumbers) { - if (Arrays.stream(tunableNumbers).anyMatch(t -> t.hasChanged(id))) { - action.accept(Arrays.stream(tunableNumbers).mapToDouble(LoggedTunableNumber::get).toArray()); - } - } - - /** Runs an action if any of the tunable numbers changed. */ - public static void ifChanged(int id, Runnable action, LoggedTunableNumber... tunableNumbers) { - ifChanged(id, values -> action.run(), tunableNumbers); - } - - @Override - public double getAsDouble() { - return get(); - } - - private static boolean isTuningMode() { - return Constants.isTuningMode(); - } -} diff --git a/src/main/java/frc/robot/util/MathUtil.java b/src/main/java/frc/robot/util/MathUtil.java deleted file mode 100644 index 6554e95..0000000 --- a/src/main/java/frc/robot/util/MathUtil.java +++ /dev/null @@ -1,58 +0,0 @@ -// Copyright (c) 2024-2026 Az-FIRST -// http://github.com/AZ-First -// -// 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. - -package frc.robot.util; - -/** Small compatibility subset of the WPILib 2026 MathUtil helpers. */ -public final class MathUtil { - private MathUtil() {} - - public static double clamp(double value, double low, double high) { - return Math.max(low, Math.min(value, high)); - } - - public static double interpolate(double startValue, double endValue, double t) { - return startValue + (endValue - startValue) * clamp(t, 0.0, 1.0); - } - - public static double applyDeadband(double value, double deadband) { - return applyDeadband(value, deadband, 1.0); - } - - public static double applyDeadband(double value, double deadband, double maxMagnitude) { - if (Math.abs(value) <= deadband) { - return 0.0; - } - - if (maxMagnitude / deadband > 1.0e12) { - return value > 0.0 ? value - deadband : value + deadband; - } - - if (value > 0.0) { - return maxMagnitude * (value - deadband) / (maxMagnitude - deadband); - } else { - return maxMagnitude * (value + deadband) / (maxMagnitude - deadband); - } - } - - public static double inputModulus(double input, double minimumInput, double maximumInput) { - double modulus = maximumInput - minimumInput; - - int numMax = (int) ((input - minimumInput) / modulus); - input -= numMax * modulus; - - int numMin = (int) ((input - maximumInput) / modulus); - input -= numMin * modulus; - - return input; - } - - public static double angleModulus(double angleRadians) { - return inputModulus(angleRadians, -Math.PI, Math.PI); - } -} diff --git a/src/main/java/frc/robot/util/RBSICANBusRegistry.java b/src/main/java/frc/robot/util/RBSICANBusRegistry.java index d574cd9..d37c4fd 100644 --- a/src/main/java/frc/robot/util/RBSICANBusRegistry.java +++ b/src/main/java/frc/robot/util/RBSICANBusRegistry.java @@ -17,10 +17,9 @@ import java.util.Map; import java.util.concurrent.ConcurrentHashMap; -/** Centralized CAN bus singleton registry */ +/** Centralized CAN bus singleton registry for Phoenix device constructors. */ public final class RBSICANBusRegistry { private static final Map realBuses = new ConcurrentHashMap<>(); - private static final Map likeBuses = new ConcurrentHashMap<>(); private static volatile boolean initialized = false; private static volatile boolean sim = false; @@ -32,22 +31,14 @@ public final class RBSICANBusRegistry { public static void initReal(String... busNames) { sim = false; for (String name : busNames) { - CANBus bus = realBuses.computeIfAbsent(name, CANBus::new); - likeBuses.computeIfAbsent(name, n -> new RealCANBusAdapter(bus)); + realBuses.computeIfAbsent(name, CANBus::new); } initialized = true; } - /** - * Initialize the SIM CANBusses - * - * @param busNames The list of bus names to initialize - */ - public static void initSim(String... busNames) { + /** Initialize the registry for simulation or replay. */ + public static void initSim() { sim = true; - for (String name : busNames) { - likeBuses.computeIfAbsent(name, SimCANBusStub::new); - } initialized = true; } @@ -60,22 +51,11 @@ public static void initSim(String... busNames) { public static CANBus getBus(String name) { checkInit(); if (sim) { - throw new IllegalStateException("No real CANBus in SIM. Use getLike() or skip CTRE devices."); + throw new IllegalStateException("No real CANBus in SIM. Skip CTRE devices."); } return getRegisteredBus(name, realBuses); } - /** - * Get a CANBus-like object for health logging - * - * @param name Name of the bus to health log - * @return CANBusLike object - */ - public static CANBusLike getLike(String name) { - checkInit(); - return getRegisteredBus(name, likeBuses); - } - /** Check that the Registry is initialized */ private static void checkInit() { if (!initialized) throw new IllegalStateException("RBSICANBusRegistry not initialized."); @@ -89,57 +69,4 @@ private static T getRegisteredBus(String name, Map buses) { } return bus; } - - /** Nested types for Registry Function *********************************** */ - - /** CANBusLike interface */ - public interface CANBusLike { - String getName(); - - CANBus.CANBusStatus getStatus(); - } - - /** Real CAN Bus Adapter */ - static final class RealCANBusAdapter implements CANBusLike { - private final CANBus bus; - - /** Constructor */ - RealCANBusAdapter(CANBus bus) { - this.bus = bus; - } - - /** Get the name of this CANBus instance */ - @Override - public String getName() { - return bus.getName(); - } - - /** Get the status of this CANBus instance */ - @Override - public CANBus.CANBusStatus getStatus() { - return bus.getStatus(); - } - } - - /** Simulated CAN Bus Stub */ - static final class SimCANBusStub implements CANBusLike { - private final String name; - - /** Constructor */ - SimCANBusStub(String name) { - this.name = name; - } - - /** Get the name of this simulated CANBus */ - @Override - public String getName() { - return name; - } - - /** Get the status of this simulated CANBus */ - @Override - public CANBus.CANBusStatus getStatus() { - return new CANBus.CANBusStatus(); - } - } } diff --git a/src/main/java/frc/robot/util/RBSICANHealth.java b/src/main/java/frc/robot/util/RBSICANHealth.java index 0da22e9..178170a 100644 --- a/src/main/java/frc/robot/util/RBSICANHealth.java +++ b/src/main/java/frc/robot/util/RBSICANHealth.java @@ -13,7 +13,10 @@ package frc.robot.util; +import frc.robot.Constants.CANBuses; import org.littletonrobotics.junction.Logger; +import org.wpilib.hardware.bus.CANPort; +import org.wpilib.system.RobotController; /** * Virtual subsystem to monitor CAN health @@ -22,11 +25,13 @@ */ public class RBSICANHealth extends VirtualSubsystem { private long loops = 0; - private final RBSICANBusRegistry.CANBusLike bus; + private final String busName; + private final CANPort port; - /** Constructur */ + /** Constructor */ public RBSICANHealth(String busName) { - bus = RBSICANBusRegistry.getLike(busName); + this.busName = busName; + port = CANBuses.revPort(busName); } @Override @@ -41,11 +46,11 @@ protected void rbsiPeriodic() { if ((loops++ % 5) != 0) return; // Get status and log - var status = bus.getStatus(); - Logger.recordOutput("CAN/" + bus.getName() + "/Utilization", status.BusUtilization); - Logger.recordOutput("CAN/" + bus.getName() + "/TxFullCount", status.TxFullCount); - Logger.recordOutput("CAN/" + bus.getName() + "/RxErrorCount", status.REC); - Logger.recordOutput("CAN/" + bus.getName() + "/TxErrorCount", status.TEC); - Logger.recordOutput("CAN/" + bus.getName() + "/BusOffCount", status.BusOffCount); + var status = RobotController.getCANStatus(port); + Logger.recordOutput("CAN/" + busName + "/Utilization", status.percentBusUtilization); + Logger.recordOutput("CAN/" + busName + "/TxFullCount", status.txFullCount); + Logger.recordOutput("CAN/" + busName + "/RxErrorCount", status.receiveErrorCount); + Logger.recordOutput("CAN/" + busName + "/TxErrorCount", status.transmitErrorCount); + Logger.recordOutput("CAN/" + busName + "/BusOffCount", status.busOffCount); } } diff --git a/src/main/java/frc/robot/util/RBSIPowerMonitor.java b/src/main/java/frc/robot/util/RBSIPowerMonitor.java index 50793a4..f12777d 100644 --- a/src/main/java/frc/robot/util/RBSIPowerMonitor.java +++ b/src/main/java/frc/robot/util/RBSIPowerMonitor.java @@ -15,9 +15,10 @@ import frc.robot.Constants.PowerConstants; import frc.robot.Constants.RobotDevices; +import java.util.function.DoubleSupplier; import org.littletonrobotics.conduit.ConduitApi; import org.littletonrobotics.junction.Logger; -import org.wpilib.system.RobotController; +import org.wpilib.system.Timer; import org.wpilib.util.Alert; import org.wpilib.util.Alert.Level; @@ -30,13 +31,15 @@ public class RBSIPowerMonitor extends VirtualSubsystem { private final RBSISubsystem[] subsystems; + // Shares the snapshot already collected by AdvantageKit's LoggedPowerDistribution. + // WPILib PowerDistribution reports zero for total power/energy on a REV PDH. private final ConduitApi conduit = ConduitApi.getInstance(); // Define local variables - private final LoggedTunableNumber batteryCapacityAh; + private final DoubleSupplier batteryCapacityAh; private double totalAmpHours = 0.0; private double totalEnergyJoules = 0.0; - private long lastTimestampUs = RobotController.getTime(); // In microseconds + private double lastTimestampSeconds = Timer.getTimestamp(); private double lastVoltage = 0.0; private double lastTotalCurrent = 0.0; private boolean totalCurrentOverLimit = false; @@ -58,7 +61,7 @@ public class RBSIPowerMonitor extends VirtualSubsystem { private final Alert totalCurrentAlert = new Alert("totalCurrentHigh", "Total current draw exceeds limit!", Level.MEDIUM); - private final Alert[] portAlerts = new Alert[24]; // or pdh.getNumChannels() after construct + private final Alert[] portAlerts = new Alert[24]; private final Alert lowVoltageAlert = new Alert("lowBatteryVoltage", "Low battery voltage!", Level.MEDIUM); private final Alert criticalVoltageAlert = @@ -68,7 +71,7 @@ public class RBSIPowerMonitor extends VirtualSubsystem { private long loops = 0; // Constructor, including inputs of optional subsystems - public RBSIPowerMonitor(LoggedTunableNumber batteryCapacityAh, RBSISubsystem... subsystems) { + public RBSIPowerMonitor(DoubleSupplier batteryCapacityAh, RBSISubsystem... subsystems) { this.batteryCapacityAh = batteryCapacityAh; this.subsystems = subsystems; @@ -114,9 +117,9 @@ public void rbsiPeriodic() { } // --- Battery estimation --- - long nowUs = RobotController.getTime(); - double dtSec = (nowUs - lastTimestampUs) / 1e6; - lastTimestampUs = nowUs; + double nowSeconds = Timer.getTimestamp(); + double dtSec = Math.max(0.0, nowSeconds - lastTimestampSeconds); + lastTimestampSeconds = nowSeconds; totalAmpHours += totalCurrent * dtSec / 3600.0; // accumulate amp-hours double capacityAh = batteryCapacityAh.getAsDouble(); diff --git a/src/main/java/frc/robot/util/TimeUtil.java b/src/main/java/frc/robot/util/TimeUtil.java deleted file mode 100644 index c0c4d20..0000000 --- a/src/main/java/frc/robot/util/TimeUtil.java +++ /dev/null @@ -1,26 +0,0 @@ -// Copyright (c) 2024-2026 Az-FIRST -// http://github.com/AZ-First -// -// 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. - -package frc.robot.util; - -import org.littletonrobotics.junction.Logger; - -/** Time utility that works for REAL, SIM, and REPLAY */ -public final class TimeUtil { - private TimeUtil() {} - - /** Always seconds, regardless of real/sim/replay timebase quirks. */ - public static double now() { - return Logger.getTimestamp() * 1.0e-6; - } -} diff --git a/src/test/frc/robot/commands/DriveCommandsTests.java b/src/test/frc/robot/commands/DriveCommandsTests.java index 15ce1a5..5368348 100644 --- a/src/test/frc/robot/commands/DriveCommandsTests.java +++ b/src/test/frc/robot/commands/DriveCommandsTests.java @@ -7,12 +7,12 @@ import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.imu.Imu; import frc.robot.subsystems.imu.ImuIOSim; -import frc.robot.util.MathUtil; import org.junit.jupiter.api.BeforeEach; import org.junit.jupiter.api.Test; import org.wpilib.command2.Command; import org.wpilib.hardware.hal.HAL; import org.wpilib.math.geometry.Translation2d; +import org.wpilib.math.util.MathUtil; import org.wpilib.simulation.DriverStationSim; class DriveCommandsTests { diff --git a/src/test/frc/robot/subsystems/vision/VisionIOTests.java b/src/test/frc/robot/subsystems/vision/VisionIOTests.java index ceec191..bd140a7 100644 --- a/src/test/frc/robot/subsystems/vision/VisionIOTests.java +++ b/src/test/frc/robot/subsystems/vision/VisionIOTests.java @@ -63,7 +63,16 @@ void limelightUsedTagExtractionTrimsMissingPerTagData() { void limelightTimestampSubtractsTotalLatencyMilliseconds() { double[] sample = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 35.0, 1.0, 0.0, 1.0, 0.0}; - assertEquals(9.965, VisionIOLimelight.timestampSeconds(10_000_000L, sample), EPSILON); + assertEquals(9.965, VisionIOLimelight.timestampSeconds(10_000_000_000L, sample), EPSILON); + } + + @Test + void limelightHeartbeatUsesNanosecondTimeout() { + long lastChangeNs = 10_000_000_000L; + assertFalse(VisionIOLimelight.isHeartbeatRecent(lastChangeNs, 0)); + assertFalse(VisionIOLimelight.isHeartbeatRecent(lastChangeNs - 1, lastChangeNs)); + assertTrue(VisionIOLimelight.isHeartbeatRecent(lastChangeNs + 249_999_999, lastChangeNs)); + assertFalse(VisionIOLimelight.isHeartbeatRecent(lastChangeNs + 250_000_000, lastChangeNs)); } @Test diff --git a/src/test/frc/robot/util/UtilTests.java b/src/test/frc/robot/util/UtilTests.java index 7b7a36d..c8febab 100644 --- a/src/test/frc/robot/util/UtilTests.java +++ b/src/test/frc/robot/util/UtilTests.java @@ -1,5 +1,6 @@ package frc.robot.util; +import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertFalse; import static org.junit.jupiter.api.Assertions.assertThrows; @@ -8,22 +9,10 @@ import frc.robot.Constants; import java.util.NavigableMap; import org.junit.jupiter.api.Test; -import org.wpilib.hardware.hal.HAL; -import org.wpilib.system.RobotController; class UtilTests { private static final double EPSILON = 1e-9; - @Test - void timeUtilReturnsSecondsFromAdvantageKitMicrosecondTimestamp() { - assertTrue(HAL.initialize()); - - double expectedSeconds = RobotController.getTime() * 1.0e-6; - double actualSeconds = TimeUtil.now(); - - assertEquals(expectedSeconds, actualSeconds, 0.050); - } - @Test void robotDeviceIdUsesValueEqualityAndHandlesMissingPowerPort() { RobotDeviceId first = new RobotDeviceId(7, Constants.CANBuses.RIO, null); @@ -39,12 +28,19 @@ void robotDeviceIdUsesValueEqualityAndHandlesMissingPowerPort() { @Test void robotDeviceIdLooksUpCanBusThroughRegistry() { - RBSICANBusRegistry.initSim(Constants.CANBuses.RIO); + RBSICANBusRegistry.initSim(); RobotDeviceId device = new RobotDeviceId(1, Constants.CANBuses.RIO, 0); assertThrows(IllegalStateException.class, device::getCANBus); } + @Test + void canHealthCanPollSystemcorePortInSimulation() { + RBSICANHealth health = new RBSICANHealth(Constants.CANBuses.RIO); + + assertDoesNotThrow(health::rbsiPeriodic); + } + @Test void concurrentTimeInterpolatableBufferInterpolatesAndExposesReadOnlyRanges() { ConcurrentTimeInterpolatableBuffer buffer = From 046826d4a54cd5cdb5483394a46b9ef1e69b809d Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Fri, 30 Oct 2026 12:01:37 -0700 Subject: [PATCH 03/10] Extend Az-FIRST source copyright notices through 2027 Update the Az-FIRST copyright year range in all 65 Java source and test files that carry an Az-FIRST notice. Preserve each file's original start year: notices that began in 2024 now read 2024-2027, and notices that began in 2026 now read 2026-2027. Leave third-party copyright lines and licensing statements intact in mixed-origin files, including those attributed to Littleton Robotics, FIRST/WPILib, SleipnirGroup, and FRC 6328. Keep the repository's full GPL license and per-project BSD license copies. The bundled AdvantageKit and WPILib license files already match their current upstream Git blobs byte for byte, so this commit does not alter or invent their copyright years. Stage only the copyright line changes, separate from the ChoreoLib code restoration and REVLib vendordep update. Verify the staged diff contains exactly one notice replacement per file and passes git diff --check. Co-authored-by: Codex --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/FieldConstants.java | 2 +- src/main/java/frc/robot/FieldState.java | 2 +- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 2 +- src/main/java/frc/robot/commands/AutopilotCommands.java | 2 +- src/main/java/frc/robot/commands/ChoreoAutoController.java | 2 +- src/main/java/frc/robot/commands/DriveCommands.java | 2 +- .../java/frc/robot/subsystems/accelerometer/Accelerometer.java | 2 +- .../java/frc/robot/subsystems/accelerometer/RioAccelIO.java | 2 +- .../frc/robot/subsystems/accelerometer/RioAccelIORoboRIO.java | 2 +- src/main/java/frc/robot/subsystems/drive/Drive.java | 2 +- src/main/java/frc/robot/subsystems/drive/DriveOdometry.java | 2 +- src/main/java/frc/robot/subsystems/drive/DriveSimPhysics.java | 2 +- src/main/java/frc/robot/subsystems/drive/LatestSampleQueue.java | 2 +- src/main/java/frc/robot/subsystems/drive/Module.java | 2 +- src/main/java/frc/robot/subsystems/drive/ModuleIO.java | 2 +- src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java | 2 +- src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java | 2 +- src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java | 2 +- .../java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java | 2 +- src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java | 2 +- src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java | 2 +- .../java/frc/robot/subsystems/drive/PhoenixOdometryThread.java | 2 +- .../java/frc/robot/subsystems/drive/SparkOdometryThread.java | 2 +- src/main/java/frc/robot/subsystems/drive/SwerveConstants.java | 2 +- .../java/frc/robot/subsystems/flywheel_example/Flywheel.java | 2 +- .../java/frc/robot/subsystems/flywheel_example/FlywheelIO.java | 2 +- .../frc/robot/subsystems/flywheel_example/FlywheelIOSim.java | 2 +- .../frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java | 2 +- .../robot/subsystems/flywheel_example/FlywheelIOTalonFX.java | 2 +- src/main/java/frc/robot/subsystems/imu/Imu.java | 2 +- src/main/java/frc/robot/subsystems/imu/ImuIO.java | 2 +- src/main/java/frc/robot/subsystems/imu/ImuIONavX.java | 2 +- src/main/java/frc/robot/subsystems/imu/ImuIOPigeon2.java | 2 +- src/main/java/frc/robot/subsystems/imu/ImuIOSim.java | 2 +- .../java/frc/robot/subsystems/vision/CameraSweepEvaluator.java | 2 +- src/main/java/frc/robot/subsystems/vision/Vision.java | 2 +- src/main/java/frc/robot/subsystems/vision/VisionIO.java | 2 +- .../java/frc/robot/subsystems/vision/VisionIOLimelight.java | 2 +- .../java/frc/robot/subsystems/vision/VisionIOPhotonVision.java | 2 +- .../frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java | 2 +- .../java/frc/robot/util/ConcurrentTimeInterpolatableBuffer.java | 2 +- src/main/java/frc/robot/util/LocalADStarAK.java | 2 +- src/main/java/frc/robot/util/OverrideSwitches.java | 2 +- src/main/java/frc/robot/util/PhoenixUtil.java | 2 +- src/main/java/frc/robot/util/RBSICANBusRegistry.java | 2 +- src/main/java/frc/robot/util/RBSICANHealth.java | 2 +- src/main/java/frc/robot/util/RBSIController.java | 2 +- src/main/java/frc/robot/util/RBSIEnum.java | 2 +- src/main/java/frc/robot/util/RBSIIO.java | 2 +- src/main/java/frc/robot/util/RBSIParsing.java | 2 +- src/main/java/frc/robot/util/RBSIPowerMonitor.java | 2 +- src/main/java/frc/robot/util/RBSISubsystem.java | 2 +- src/main/java/frc/robot/util/RobotDeviceId.java | 2 +- src/main/java/frc/robot/util/SparkUtil.java | 2 +- src/main/java/frc/robot/util/TimedPose.java | 2 +- src/main/java/frc/robot/util/VirtualSubsystem.java | 2 +- src/main/java/frc/robot/util/YagslConstants.java | 2 +- src/test/CurrentLimitTests.java | 2 +- src/test/FusedCANcoderTests.java | 2 +- src/test/LatencyCompensationTests.java | 2 +- src/test/frc/robot/FieldStateTest.java | 2 +- src/test/frc/robot/RobotContainerTest.java | 2 +- src/test/frc/robot/subsystems/drive/LatestSampleQueueTest.java | 2 +- 65 files changed, 65 insertions(+), 65 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e1fe7c9..83737d0 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/FieldConstants.java b/src/main/java/frc/robot/FieldConstants.java index 9780a98..af45478 100644 --- a/src/main/java/frc/robot/FieldConstants.java +++ b/src/main/java/frc/robot/FieldConstants.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // Copyright (c) 2025-2026 Littleton Robotics // http://github.com/Mechanical-Advantage diff --git a/src/main/java/frc/robot/FieldState.java b/src/main/java/frc/robot/FieldState.java index 51f2a74..dbe5d18 100644 --- a/src/main/java/frc/robot/FieldState.java +++ b/src/main/java/frc/robot/FieldState.java @@ -1,4 +1,4 @@ -// Copyright (c) 2026 Az-FIRST +// Copyright (c) 2026-2027 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 56df2fe..4d206d7 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 59fe64c..e1c6685 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // Copyright (c) 2021-2026 Littleton Robotics // http://github.com/Mechanical-Advantage diff --git a/src/main/java/frc/robot/commands/AutopilotCommands.java b/src/main/java/frc/robot/commands/AutopilotCommands.java index bede4eb..2f9dc3b 100644 --- a/src/main/java/frc/robot/commands/AutopilotCommands.java +++ b/src/main/java/frc/robot/commands/AutopilotCommands.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/commands/ChoreoAutoController.java b/src/main/java/frc/robot/commands/ChoreoAutoController.java index c7bb2b4..15c5be0 100644 --- a/src/main/java/frc/robot/commands/ChoreoAutoController.java +++ b/src/main/java/frc/robot/commands/ChoreoAutoController.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // Copyright 2024 SleipnirGroup // https://choreo.autos/ diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index ad962c5..5c5a930 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // Copyright (c) 2021-2026 Littleton Robotics // http://github.com/Mechanical-Advantage diff --git a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java index d1f4365..7ea75a2 100644 --- a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java +++ b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/subsystems/accelerometer/RioAccelIO.java b/src/main/java/frc/robot/subsystems/accelerometer/RioAccelIO.java index 2a60a89..3507fa6 100644 --- a/src/main/java/frc/robot/subsystems/accelerometer/RioAccelIO.java +++ b/src/main/java/frc/robot/subsystems/accelerometer/RioAccelIO.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/subsystems/accelerometer/RioAccelIORoboRIO.java b/src/main/java/frc/robot/subsystems/accelerometer/RioAccelIORoboRIO.java index 5d5ff88..5945510 100644 --- a/src/main/java/frc/robot/subsystems/accelerometer/RioAccelIORoboRIO.java +++ b/src/main/java/frc/robot/subsystems/accelerometer/RioAccelIORoboRIO.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index aa13101..d1fe5cd 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/subsystems/drive/DriveOdometry.java b/src/main/java/frc/robot/subsystems/drive/DriveOdometry.java index aa09c70..504a266 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveOdometry.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveOdometry.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSimPhysics.java b/src/main/java/frc/robot/subsystems/drive/DriveSimPhysics.java index a5069c0..bcea632 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSimPhysics.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSimPhysics.java @@ -1,4 +1,4 @@ -// Copyright (c) 2026 Az-FIRST +// Copyright (c) 2026-2027 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/subsystems/drive/LatestSampleQueue.java b/src/main/java/frc/robot/subsystems/drive/LatestSampleQueue.java index 2018c25..a3506be 100644 --- a/src/main/java/frc/robot/subsystems/drive/LatestSampleQueue.java +++ b/src/main/java/frc/robot/subsystems/drive/LatestSampleQueue.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index ecf1e22..5c9ccae 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // Copyright (c) 2021-2026 Littleton Robotics // http://github.com/Mechanical-Advantage diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java index dbeb98d..0847af9 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // Copyright (c) 2021-2026 Littleton Robotics // http://github.com/Mechanical-Advantage diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java index 1a9c1ec..697ee43 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // Copyright 2024-2025 FRC 2486 // https://github.com/Coconuts2486-FRC diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java index abd5073..3caa5bc 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // Copyright (c) 2021-2026 Littleton Robotics // http://github.com/Mechanical-Advantage diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java index 642c024..89b4e46 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // Copyright (c) 2021-2026 Littleton Robotics // http://github.com/Mechanical-Advantage diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java index 86c753b..4299dee 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // Copyright (c) 2021-2026 Littleton Robotics // http://github.com/Mechanical-Advantage diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index 12e9790..b7a6805 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // Copyright (c) 2021-2026 Littleton Robotics // http://github.com/Mechanical-Advantage diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java index e374119..cf14f03 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // Copyright (c) 2021-2026 Littleton Robotics // http://github.com/Mechanical-Advantage diff --git a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java index 8a28c3d..e379e50 100644 --- a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // Copyright (c) 2021-2026 Littleton Robotics // http://github.com/Mechanical-Advantage diff --git a/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java index b4a10b6..5ff4370 100644 --- a/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // Copyright (c) 2021-2026 Littleton Robotics // http://github.com/Mechanical-Advantage diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java b/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java index 9e30101..49de118 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java b/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java index fbe369a..9ff4b08 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // Copyright (c) 2021-2026 Littleton Robotics // http://github.com/Mechanical-Advantage diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIO.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIO.java index ac4f7cf..7dfd1ad 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIO.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIO.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // Copyright (c) 2021-2026 Littleton Robotics // http://github.com/Mechanical-Advantage diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSim.java index 015a66f..c109964 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSim.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSim.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // Copyright (c) 2021-2026 Littleton Robotics // http://github.com/Mechanical-Advantage diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java index 4c25e1e..6579a15 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // Copyright (c) 2021-2026 Littleton Robotics // http://github.com/Mechanical-Advantage diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java index 0b1cf34..ea02911 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // Copyright (c) 2021-2026 Littleton Robotics // http://github.com/Mechanical-Advantage diff --git a/src/main/java/frc/robot/subsystems/imu/Imu.java b/src/main/java/frc/robot/subsystems/imu/Imu.java index 4162692..b50e8c5 100644 --- a/src/main/java/frc/robot/subsystems/imu/Imu.java +++ b/src/main/java/frc/robot/subsystems/imu/Imu.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/subsystems/imu/ImuIO.java b/src/main/java/frc/robot/subsystems/imu/ImuIO.java index 94d8309..45cb7a8 100644 --- a/src/main/java/frc/robot/subsystems/imu/ImuIO.java +++ b/src/main/java/frc/robot/subsystems/imu/ImuIO.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/subsystems/imu/ImuIONavX.java b/src/main/java/frc/robot/subsystems/imu/ImuIONavX.java index 267a491..73161a9 100644 --- a/src/main/java/frc/robot/subsystems/imu/ImuIONavX.java +++ b/src/main/java/frc/robot/subsystems/imu/ImuIONavX.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/subsystems/imu/ImuIOPigeon2.java b/src/main/java/frc/robot/subsystems/imu/ImuIOPigeon2.java index 77af8c9..992c023 100644 --- a/src/main/java/frc/robot/subsystems/imu/ImuIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/imu/ImuIOPigeon2.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java b/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java index 21d9281..a4470e0 100644 --- a/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java +++ b/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/subsystems/vision/CameraSweepEvaluator.java b/src/main/java/frc/robot/subsystems/vision/CameraSweepEvaluator.java index 689417f..d235ca7 100644 --- a/src/main/java/frc/robot/subsystems/vision/CameraSweepEvaluator.java +++ b/src/main/java/frc/robot/subsystems/vision/CameraSweepEvaluator.java @@ -1,4 +1,4 @@ -// Copyright (c) 2026 Az-FIRST +// Copyright (c) 2026-2027 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 9ea5afe..9137403 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java index b593169..8c401fb 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIO.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // Copyright (c) 2021-2026 Littleton Robotics // http://github.com/Mechanical-Advantage diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java index f640b11..9626e83 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // Copyright (c) 2021-2026 Littleton Robotics // http://github.com/Mechanical-Advantage diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java index f95172b..558b7fd 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // Copyright (c) 2021-2026 Littleton Robotics // http://github.com/Mechanical-Advantage diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java index 8da10c9..ffac5ce 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // Copyright (c) 2021-2026 Littleton Robotics // http://github.com/Mechanical-Advantage diff --git a/src/main/java/frc/robot/util/ConcurrentTimeInterpolatableBuffer.java b/src/main/java/frc/robot/util/ConcurrentTimeInterpolatableBuffer.java index cc930d1..caad609 100644 --- a/src/main/java/frc/robot/util/ConcurrentTimeInterpolatableBuffer.java +++ b/src/main/java/frc/robot/util/ConcurrentTimeInterpolatableBuffer.java @@ -1,4 +1,4 @@ -// Copyright (c) 2026 Az-FIRST +// Copyright (c) 2026-2027 Az-FIRST // http://github.com/AZ-First // Copyright (c) 2024 FRC 254 // https://github.com/team254 diff --git a/src/main/java/frc/robot/util/LocalADStarAK.java b/src/main/java/frc/robot/util/LocalADStarAK.java index ee2f928..9cf0308 100644 --- a/src/main/java/frc/robot/util/LocalADStarAK.java +++ b/src/main/java/frc/robot/util/LocalADStarAK.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // Copyright (c) 2021-2026 Littleton Robotics // http://github.com/Mechanical-Advantage diff --git a/src/main/java/frc/robot/util/OverrideSwitches.java b/src/main/java/frc/robot/util/OverrideSwitches.java index c249b9a..c8782cf 100644 --- a/src/main/java/frc/robot/util/OverrideSwitches.java +++ b/src/main/java/frc/robot/util/OverrideSwitches.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // Copyright (c) 2021-2026 Littleton Robotics // http://github.com/Mechanical-Advantage diff --git a/src/main/java/frc/robot/util/PhoenixUtil.java b/src/main/java/frc/robot/util/PhoenixUtil.java index fd7d10b..d26f098 100644 --- a/src/main/java/frc/robot/util/PhoenixUtil.java +++ b/src/main/java/frc/robot/util/PhoenixUtil.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // Copyright (c) 2021-2026 Littleton Robotics // http://github.com/Mechanical-Advantage diff --git a/src/main/java/frc/robot/util/RBSICANBusRegistry.java b/src/main/java/frc/robot/util/RBSICANBusRegistry.java index d37c4fd..b837869 100644 --- a/src/main/java/frc/robot/util/RBSICANBusRegistry.java +++ b/src/main/java/frc/robot/util/RBSICANBusRegistry.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/util/RBSICANHealth.java b/src/main/java/frc/robot/util/RBSICANHealth.java index 178170a..c742fec 100644 --- a/src/main/java/frc/robot/util/RBSICANHealth.java +++ b/src/main/java/frc/robot/util/RBSICANHealth.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/util/RBSIController.java b/src/main/java/frc/robot/util/RBSIController.java index 943eb45..74f8b18 100644 --- a/src/main/java/frc/robot/util/RBSIController.java +++ b/src/main/java/frc/robot/util/RBSIController.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/util/RBSIEnum.java b/src/main/java/frc/robot/util/RBSIEnum.java index dde50a7..352127f 100644 --- a/src/main/java/frc/robot/util/RBSIEnum.java +++ b/src/main/java/frc/robot/util/RBSIEnum.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/util/RBSIIO.java b/src/main/java/frc/robot/util/RBSIIO.java index 7bcdd64..dfe0422 100644 --- a/src/main/java/frc/robot/util/RBSIIO.java +++ b/src/main/java/frc/robot/util/RBSIIO.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/util/RBSIParsing.java b/src/main/java/frc/robot/util/RBSIParsing.java index 4b41b8d..badde46 100644 --- a/src/main/java/frc/robot/util/RBSIParsing.java +++ b/src/main/java/frc/robot/util/RBSIParsing.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/util/RBSIPowerMonitor.java b/src/main/java/frc/robot/util/RBSIPowerMonitor.java index f12777d..f3e6aae 100644 --- a/src/main/java/frc/robot/util/RBSIPowerMonitor.java +++ b/src/main/java/frc/robot/util/RBSIPowerMonitor.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/util/RBSISubsystem.java b/src/main/java/frc/robot/util/RBSISubsystem.java index d4bda6e..379856a 100644 --- a/src/main/java/frc/robot/util/RBSISubsystem.java +++ b/src/main/java/frc/robot/util/RBSISubsystem.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/util/RobotDeviceId.java b/src/main/java/frc/robot/util/RobotDeviceId.java index 94db581..26fde7d 100644 --- a/src/main/java/frc/robot/util/RobotDeviceId.java +++ b/src/main/java/frc/robot/util/RobotDeviceId.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // Copyright (c) 2024 FRC 254 // https://github.com/team254 diff --git a/src/main/java/frc/robot/util/SparkUtil.java b/src/main/java/frc/robot/util/SparkUtil.java index 5b9f808..c727a6f 100644 --- a/src/main/java/frc/robot/util/SparkUtil.java +++ b/src/main/java/frc/robot/util/SparkUtil.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // Copyright (c) 2021-2026 Littleton Robotics // http://github.com/Mechanical-Advantage diff --git a/src/main/java/frc/robot/util/TimedPose.java b/src/main/java/frc/robot/util/TimedPose.java index e30e00f..43e09dc 100644 --- a/src/main/java/frc/robot/util/TimedPose.java +++ b/src/main/java/frc/robot/util/TimedPose.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/util/VirtualSubsystem.java b/src/main/java/frc/robot/util/VirtualSubsystem.java index f078388..7a94316 100644 --- a/src/main/java/frc/robot/util/VirtualSubsystem.java +++ b/src/main/java/frc/robot/util/VirtualSubsystem.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // Copyright (c) 2021-2026 Littleton Robotics // http://github.com/Mechanical-Advantage diff --git a/src/main/java/frc/robot/util/YagslConstants.java b/src/main/java/frc/robot/util/YagslConstants.java index f2e9f95..bd277b6 100644 --- a/src/main/java/frc/robot/util/YagslConstants.java +++ b/src/main/java/frc/robot/util/YagslConstants.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // // Use of this source code is governed by a BSD diff --git a/src/test/CurrentLimitTests.java b/src/test/CurrentLimitTests.java index ec461a2..96cd647 100644 --- a/src/test/CurrentLimitTests.java +++ b/src/test/CurrentLimitTests.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // Copyright (c) 2024-2025 Cross The Road Electronics // https://github.com/CrossTheRoadElec/Phoenix6-Examples diff --git a/src/test/FusedCANcoderTests.java b/src/test/FusedCANcoderTests.java index be7eae3..cc999bd 100644 --- a/src/test/FusedCANcoderTests.java +++ b/src/test/FusedCANcoderTests.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // Copyright (c) 2024-2025 Cross The Road Electronics // https://github.com/CrossTheRoadElec/Phoenix6-Examples diff --git a/src/test/LatencyCompensationTests.java b/src/test/LatencyCompensationTests.java index 430b207..cddd830 100644 --- a/src/test/LatencyCompensationTests.java +++ b/src/test/LatencyCompensationTests.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // Copyright (c) -2025 Cross The Road Electronics // https://github.com/CrossTheRoadElec/Phoenix6-Examples diff --git a/src/test/frc/robot/FieldStateTest.java b/src/test/frc/robot/FieldStateTest.java index 37fd8f5..55f2618 100644 --- a/src/test/frc/robot/FieldStateTest.java +++ b/src/test/frc/robot/FieldStateTest.java @@ -1,4 +1,4 @@ -// Copyright (c) 2026 Az-FIRST +// Copyright (c) 2026-2027 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or diff --git a/src/test/frc/robot/RobotContainerTest.java b/src/test/frc/robot/RobotContainerTest.java index c400666..e643216 100644 --- a/src/test/frc/robot/RobotContainerTest.java +++ b/src/test/frc/robot/RobotContainerTest.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // Copyright (c) 2024 FRC 6328 // http://github.com/Mechanical-Advantage diff --git a/src/test/frc/robot/subsystems/drive/LatestSampleQueueTest.java b/src/test/frc/robot/subsystems/drive/LatestSampleQueueTest.java index 83f1b99..8751350 100644 --- a/src/test/frc/robot/subsystems/drive/LatestSampleQueueTest.java +++ b/src/test/frc/robot/subsystems/drive/LatestSampleQueueTest.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2024-2027 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or From 8154f3a94835098a43f20b96fc42d325d245deb9 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Fri, 30 Oct 2026 12:02:07 -0700 Subject: [PATCH 04/10] Restore ChoreoLib autonomous integration for 2027 alpha Add the compatible ChoreoLib 2027.0.0-alpha-3 vendordep supplied for the Systemcore/WPILib 2027 alpha toolchain. Restore the previously disabled Choreo imports, AutoFactory, LoggedNetworkChooser, and AutoRoutine wiring in RobotContainer. Keep separate manual, PathPlanner, and Choreo chooser initialization paths. Return the selected Choreo command from the Choreo accessor used by Robot.autonomousInit. Restore the example two-piece routine: load pickupGamepiece and scoreGamepiece trajectories, reset odometry before the pickup trajectory, and chain the scoring trajectory after pickup completes. Keep "Nothing" as the Choreo chooser default. These example trajectory files are not yet present under src/main/deploy/choreo, so they still need to be supplied before the two-piece option can drive a path. The configured AutoType remains MANUAL until explicitly selected for a robot. Re-enable Drive's Choreo trajectory followers using the 2027 ChassisVelocities API. Adapt SwerveSample.getChassisSpeeds() to the current vendordep, add X/Y/heading PID feedback to trajectory feedforward, and convert field-relative velocities to robot-relative commands. Restore the reusable ChoreoAutoController implementation with continuous heading input and reset behavior. Change RobotContainerTest to construct one container in CHOREO mode and check that it yields an autonomous command. A second RobotContainer in the same test process would register duplicate WPILib alert IDs, so the focused test exercises the restored mode directly. Verify the complete ./gradlew test suite and git diff --check with the installed WPILib JDK. Co-authored-by: Codex --- src/main/java/frc/robot/RobotContainer.java | 59 +++++++++--------- .../robot/commands/ChoreoAutoController.java | 61 +++++++++++++++++-- .../frc/robot/subsystems/drive/Drive.java | 43 +++++++------ src/test/frc/robot/RobotContainerTest.java | 18 ++++-- vendordeps/ChoreoLib2027Alpha.json | 43 +++++++++++++ 5 files changed, 159 insertions(+), 65 deletions(-) create mode 100644 vendordeps/ChoreoLib2027Alpha.json diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e1c6685..5817e77 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -19,6 +19,9 @@ package frc.robot; +import choreo.auto.AutoFactory; +import choreo.auto.AutoRoutine; +import choreo.auto.AutoTrajectory; import com.pathplanner.lib.auto.AutoBuilder; import frc.robot.Constants.CANBuses; import frc.robot.Constants.Cameras; @@ -117,9 +120,8 @@ public class RobotContainer { // AutoChoosers for both supported path planning types private final LoggedNetworkChooser autoChooserPathPlanner; - // TODO(2027): Re-enable Choreo chooser/factory when ChoreoLib supports 2027 WPILib. - // private final LoggedNetworkChooser autoChooserChoreo; - // private final AutoFactory autoFactoryChoreo; + private final LoggedNetworkChooser autoChooserChoreo; + private final AutoFactory autoFactoryChoreo; private final LoggedNetworkChooser driveStyle = new LoggedNetworkChooser<>("Drive Style"); @@ -234,32 +236,29 @@ public RobotContainer() { // ... // Set the others to null autoChooserPathPlanner = null; - // TODO(2027): Restore Choreo fields when ChoreoLib supports 2027 WPILib. - // autoChooserChoreo = null; - // autoFactoryChoreo = null; + autoChooserChoreo = null; + autoFactoryChoreo = null; break; case PATHPLANNER: autoChooserPathPlanner = new LoggedNetworkChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); // Set the others to null - // TODO(2027): Restore Choreo fields when ChoreoLib supports 2027 WPILib. - // autoChooserChoreo = null; - // autoFactoryChoreo = null; + autoChooserChoreo = null; + autoFactoryChoreo = null; break; case CHOREO: - // TODO(2027): Re-enable this Choreo AutoFactory setup when ChoreoLib supports 2027 WPILib. - // autoFactoryChoreo = - // new AutoFactory( - // m_drivebase::getPose, - // m_drivebase::resetPose, - // m_drivebase::followTrajectory, - // true, - // m_drivebase); - // autoChooserChoreo = new LoggedNetworkChooser<>("Choreo Auto Choices"); - // autoChooserChoreo.addDefault("Nothing", Commands.none()); - // autoChooserChoreo.add("twoPieceAuto", twoPieceAuto().cmd()); + autoFactoryChoreo = + new AutoFactory( + m_drivebase::getPose, + m_drivebase::resetPose, + m_drivebase::followTrajectory, + true, + m_drivebase); + autoChooserChoreo = new LoggedNetworkChooser<>("Choreo Auto Choices"); + autoChooserChoreo.addDefault("Nothing", Commands.none()); + autoChooserChoreo.add("twoPieceAuto", twoPieceAuto().cmd()); autoChooserPathPlanner = null; break; @@ -436,8 +435,7 @@ public Command getAutonomousCommandPathPlanner() { * @return the command to run in autonomous */ public Command getAutonomousCommandChoreo() { - // TODO(2027): Return autoChooserChoreo.get() when ChoreoLib supports 2027 WPILib. - return Commands.none(); + return autoChooserChoreo.get(); } /** Updates the alerts. */ @@ -576,15 +574,14 @@ public void updateInputs(VisionIOInputs inputs) { return ios; } - // TODO(2027): Re-enable this example Choreo auto when ChoreoLib supports 2027 WPILib. - // private AutoRoutine twoPieceAuto() { - // AutoRoutine routine = autoFactoryChoreo.newRoutine("twoPieceAuto"); - // AutoTrajectory pickupTraj = routine.trajectory("pickupGamepiece"); - // AutoTrajectory scoreTraj = routine.trajectory("scoreGamepiece"); - // routine.active().onTrue(Commands.sequence(pickupTraj.resetOdometry(), pickupTraj.cmd())); - // pickupTraj.done().onTrue(scoreTraj.cmd()); - // return routine; - // } + private AutoRoutine twoPieceAuto() { + AutoRoutine routine = autoFactoryChoreo.newRoutine("twoPieceAuto"); + AutoTrajectory pickupTraj = routine.trajectory("pickupGamepiece"); + AutoTrajectory scoreTraj = routine.trajectory("scoreGamepiece"); + routine.active().onTrue(Commands.sequence(pickupTraj.resetOdometry(), pickupTraj.cmd())); + pickupTraj.done().onTrue(scoreTraj.cmd()); + return routine; + } private DriveStyle getSelectedDriveStyle() { DriveStyle selected = driveStyle.get(); diff --git a/src/main/java/frc/robot/commands/ChoreoAutoController.java b/src/main/java/frc/robot/commands/ChoreoAutoController.java index 15c5be0..6e590cb 100644 --- a/src/main/java/frc/robot/commands/ChoreoAutoController.java +++ b/src/main/java/frc/robot/commands/ChoreoAutoController.java @@ -9,10 +9,59 @@ package frc.robot.commands; -/** - * TODO(2027): Restore the Choreo auto controller when ChoreoLib publishes a 2027-compatible - * vendordep. - */ -public final class ChoreoAutoController { - private ChoreoAutoController() {} +import choreo.trajectory.SwerveSample; +import frc.robot.Constants.AutoConstants; +import frc.robot.subsystems.drive.Drive; +import java.util.function.Consumer; +import org.wpilib.math.controller.PIDController; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.kinematics.ChassisVelocities; + +/** Choreo swerve trajectory controller. */ +public final class ChoreoAutoController implements Consumer { + private final Drive drive; + private final PIDController xController = + new PIDController( + AutoConstants.kChoreoDrivePID.kP, + AutoConstants.kChoreoDrivePID.kI, + AutoConstants.kChoreoDrivePID.kD); + private final PIDController yController = + new PIDController( + AutoConstants.kChoreoDrivePID.kP, + AutoConstants.kChoreoDrivePID.kI, + AutoConstants.kChoreoDrivePID.kD); + private final PIDController headingController = + new PIDController( + AutoConstants.kChoreoSteerPID.kP, + AutoConstants.kChoreoSteerPID.kI, + AutoConstants.kChoreoSteerPID.kD); + + public ChoreoAutoController(Drive drive) { + this.drive = drive; + headingController.enableContinuousInput(-Math.PI, Math.PI); + } + + public void reset() { + Pose2d pose = drive.getPose(); + xController.reset(); + yController.reset(); + headingController.reset(); + headingController.setSetpoint(pose.getRotation().getRadians()); + } + + @Override + public void accept(SwerveSample referenceState) { + Pose2d pose = drive.getPose(); + double xFeedback = xController.calculate(pose.getX(), referenceState.x); + double yFeedback = yController.calculate(pose.getY(), referenceState.y); + double rotationFeedback = + headingController.calculate(pose.getRotation().getRadians(), referenceState.heading); + + ChassisVelocities output = + new ChassisVelocities( + referenceState.vx + xFeedback, + referenceState.vy + yFeedback, + referenceState.omega + rotationFeedback); + drive.runVelocity(output.toRobotRelative(pose.getRotation())); + } } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index d1fe5cd..1fa9e16 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -20,6 +20,7 @@ import static frc.robot.subsystems.drive.SwerveConstants.*; import static org.wpilib.units.Units.Volts; +import choreo.trajectory.SwerveSample; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.controllers.PPHolonomicDriveController; @@ -1115,26 +1116,24 @@ public void resetOdometry(Pose2d pose) { AutoConstants.kChoreoSteerPID.kI, AutoConstants.kChoreoSteerPID.kD); - // TODO(2027): Re-enable these Choreo trajectory followers when ChoreoLib supports 2027 WPILib. - // public void choreoController(Pose2d pose, SwerveSample sample) { - // var targetSpeeds = sample.getChassisVelocities(); - // targetSpeeds.vx += m_pathXController.calculate(pose.getX(), sample.x); - // targetSpeeds.vy += m_pathYController.calculate(pose.getY(), sample.y); - // targetSpeeds.omega += - // m_pathThetaController.calculate(pose.getRotation().getRadians(), sample.heading); - // - // runVelocity(targetSpeeds.toRobotRelative(getHeading())); - // } - // - // public void followTrajectory(SwerveSample sample) { - // Pose2d pose = getPose(); - // ChassisVelocities fieldRelativeSpeeds = - // new ChassisVelocities( - // sample.vx + m_pathXController.calculate(pose.getX(), sample.x), - // sample.vy + m_pathYController.calculate(pose.getY(), sample.y), - // sample.omega - // + m_pathThetaController.calculate(pose.getRotation().getRadians(), - // sample.heading)); - // runVelocity(fieldRelativeSpeeds.toRobotRelative(pose.getRotation())); - // } + public void choreoController(Pose2d pose, SwerveSample sample) { + ChassisVelocities targetSpeeds = sample.getChassisSpeeds(); + targetSpeeds.vx += m_pathXController.calculate(pose.getX(), sample.x); + targetSpeeds.vy += m_pathYController.calculate(pose.getY(), sample.y); + targetSpeeds.omega += + m_pathThetaController.calculate(pose.getRotation().getRadians(), sample.heading); + + runVelocity(targetSpeeds.toRobotRelative(getHeading())); + } + + public void followTrajectory(SwerveSample sample) { + Pose2d pose = getPose(); + ChassisVelocities fieldRelativeSpeeds = + new ChassisVelocities( + sample.vx + m_pathXController.calculate(pose.getX(), sample.x), + sample.vy + m_pathYController.calculate(pose.getY(), sample.y), + sample.omega + + m_pathThetaController.calculate(pose.getRotation().getRadians(), sample.heading)); + runVelocity(fieldRelativeSpeeds.toRobotRelative(pose.getRotation())); + } } diff --git a/src/test/frc/robot/RobotContainerTest.java b/src/test/frc/robot/RobotContainerTest.java index e643216..a3fa571 100644 --- a/src/test/frc/robot/RobotContainerTest.java +++ b/src/test/frc/robot/RobotContainerTest.java @@ -9,19 +9,25 @@ package frc.robot; -import static org.junit.jupiter.api.Assertions.fail; +import static org.junit.jupiter.api.Assertions.assertNotNull; +import frc.robot.util.RBSIEnum.AutoType; +import java.lang.reflect.Field; import org.junit.jupiter.api.Test; public class RobotContainerTest { @Test - public void createRobotContainer() { - // Instantiate RobotContainer + public void createChoreoRobotContainer() throws ReflectiveOperationException { + Field autoTypeField = Constants.class.getDeclaredField("autoType"); + autoTypeField.setAccessible(true); + AutoType previous = (AutoType) autoTypeField.get(null); try { - new RobotContainer(); - } catch (Exception e) { - fail("Failed to instantiate RobotContainer.", e); + autoTypeField.set(null, AutoType.CHOREO); + RobotContainer container = new RobotContainer(); + assertNotNull(container.getAutonomousCommandChoreo()); + } finally { + autoTypeField.set(null, previous); } } } diff --git a/vendordeps/ChoreoLib2027Alpha.json b/vendordeps/ChoreoLib2027Alpha.json new file mode 100644 index 0000000..05e0889 --- /dev/null +++ b/vendordeps/ChoreoLib2027Alpha.json @@ -0,0 +1,43 @@ +{ + "fileName": "ChoreoLib2027Alpha.json", + "name": "ChoreoLib", + "version": "2027.0.0-alpha-3", + "uuid": "b5e23f0a-dac9-4ad2-8dd6-02767c520aca", + "wpilibYear": "2027_alpha7", + "mavenUrls": [ + "https://frcmaven.wpi.edu/artifactory/sleipnirgroup-mvn-release/", + "https://repo1.maven.org/maven2" + ], + "jsonUrl": "https://choreo.autos/lib/ChoreoLib2027Alpha.json", + "javaDependencies": [ + { + "groupId": "choreo", + "artifactId": "ChoreoLib-java", + "version": "2027.0.0-alpha-3" + }, + { + "groupId": "com.google.code.gson", + "artifactId": "gson", + "version": "2.11.0" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "choreo", + "artifactId": "ChoreoLib-cpp", + "version": "2027.0.0-alpha-3", + "libName": "ChoreoLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxarm64", + "linuxsystemcore" + ] + } + ] +} From 63c215e773b85a63ccc8c57c3210370b67543d4c Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Fri, 30 Oct 2026 12:02:35 -0700 Subject: [PATCH 05/10] Update REVLib vendordep to 2027 alpha 9 Advance REVLib from 2027.0.0-alpha-8 to 2027.0.0-alpha-9 in the vendordep metadata. Keep the existing 2027_alpha7 WPILib year, UUID, Maven URLs, platform declarations, and library names. Update every dependency version consistently across REVLib-java, the JNI driver and backend components, REVLib-cpp, and the corresponding C++ driver/backend entries. Keep this dependency refresh separate from the Az-FIRST copyright notice update and the ChoreoLib autonomous reintegration so each change can be reviewed or reverted independently. Validate the vendordep as JSON and confirm all eight dependency declarations use the same alpha-9 version. The complete ./gradlew test suite passed with this vendordep installed and the restored ChoreoLib code present; git diff --check is clean. Co-authored-by: Codex --- vendordeps/REVLib.json | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index 8e9d07b..e092b9a 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2027.0.0-alpha-8", + "version": "2027.0.0-alpha-9", "wpilibYear": "2027_alpha7", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2027.0.0-alpha-8" + "version": "2027.0.0-alpha-9" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2027.0.0-alpha-8", + "version": "2027.0.0-alpha-9", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -33,7 +33,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibBackendDriver", - "version": "2027.0.0-alpha-8", + "version": "2027.0.0-alpha-9", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -47,7 +47,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibWpiBackendDriver", - "version": "2027.0.0-alpha-8", + "version": "2027.0.0-alpha-9", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -63,7 +63,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2027.0.0-alpha-8", + "version": "2027.0.0-alpha-9", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -79,7 +79,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2027.0.0-alpha-8", + "version": "2027.0.0-alpha-9", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, @@ -95,7 +95,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibBackendDriver", - "version": "2027.0.0-alpha-8", + "version": "2027.0.0-alpha-9", "libName": "BackendDriver", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -110,7 +110,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibWpiBackendDriver", - "version": "2027.0.0-alpha-8", + "version": "2027.0.0-alpha-9", "libName": "REVLibWpi", "sharedLibrary": true, "skipInvalidPlatforms": true, From 248162e7bb873895391430ce9f251d750c079d66 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Fri, 30 Oct 2026 14:45:52 -0700 Subject: [PATCH 06/10] Expand BIOCORE VS Code theme colors and select it for this workspace Package version 1.1.0 of the FRC BIOCORE High Contrast theme as a VSIX. Keep the dark teal foundation and FIRST CANOPY season colors while adding more distinct syntax colors for control flow, operators, functions, types, properties, constants, strings, numbers, comments, markup, and regular expressions. Expand semantic token coverage and define workbench colors for UI roles previously left to VS Code defaults. Select the theme in workspace settings and put its extension ID first in the workspace recommendations. Update the README to point at the new VSIX and explain that the project selects the theme after installation. The VSIX remains the only new theme artifact in the repository; it does not auto-install when the project is opened. Validate the VSIX archive and its embedded theme/package JSON, the workspace configuration, and git diff --check. Co-authored-by: Codex --- .vscode/extensions.json | 1 + .vscode/settings.json | 1 + README.md | 5 ++--- .../frc-biocore-high-contrast-theme-1.1.0.vsix | Bin 0 -> 7763 bytes 4 files changed, 4 insertions(+), 3 deletions(-) create mode 100644 themes/frc-biocore-high-contrast-theme-1.1.0.vsix diff --git a/.vscode/extensions.json b/.vscode/extensions.json index 567056a..a1f7c62 100644 --- a/.vscode/extensions.json +++ b/.vscode/extensions.json @@ -1,5 +1,6 @@ { "recommendations": [ + "local.frc-biocore-high-contrast-theme", "richardwillis.vscode-spotless-gradle" ] } diff --git a/.vscode/settings.json b/.vscode/settings.json index e0391a3..15dc1e6 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -1,4 +1,5 @@ { + "workbench.colorTheme": "FRC BIOCORE High Contrast", "java.configuration.updateBuildConfiguration": "automatic", "java.server.launchMode": "Standard", "files.exclude": { diff --git a/README.md b/README.md index 138143d..8282536 100644 --- a/README.md +++ b/README.md @@ -23,9 +23,8 @@ To use the BIOCORE VS Code theme: 1. Open the Extensions view in VS Code. 2. Select `...` > `Install from VSIX...`. 3. Install - [`themes/frc-biocore-high-contrast-theme-1.0.1.vsix`](themes/frc-biocore-high-contrast-theme-1.0.1.vsix). -4. Run `Preferences: Color Theme` from the command palette and select - `FRC BIOCORE High Contrast`. + [`themes/frc-biocore-high-contrast-theme-1.1.0.vsix`](themes/frc-biocore-high-contrast-theme-1.1.0.vsix). +4. Open this project; its workspace settings select `FRC BIOCORE High Contrast`. ## Documentation diff --git a/themes/frc-biocore-high-contrast-theme-1.1.0.vsix b/themes/frc-biocore-high-contrast-theme-1.1.0.vsix new file mode 100644 index 0000000000000000000000000000000000000000..e7b61c2d5f8bb1a92390311b5b0e84de8726fafc GIT binary patch literal 7763 zcmb7}bx<8!)34dc#@*fBc5ruh4NkC)yL)gFG`KqiclY37W5G$V;1F~NCqO>Vd+UBD z=dE-9xHC01Qd3XObg%B!>(?5}aPW9AFfd3kzOJSwzxx9@xZq%5@{nO*2wt9oeLca> z9(FFy9Nr#wzE0-OcGh4I&)(N2F5f_SL1)2{X*M_+eU8_{C#%EN zjkK#pW&Od+$fc=gDQW?d15nX};QXAc^(Uz$dVR+DGnvy}5KUnxww6s^#T7p^=g{PK zQBuhIF&*qW-O>X%7+)OSWfDY_Ln<+0g!FPIFnftaBY2EUxjW=`LXr7VStomC{+=t&^;3x>6fli2jNB$Wh2@Axg1!uyHd`~2LU@I}SFBtXLaeVM)Zm4ldE!4~ z9XYcl9?ZE~Qcr`^$JT6JvMw9tUEfjcI5O(GkM0>OW`v*0KwLl4NIB%=9-)l9CUgX> z50t8JU~>M-NYVZY&{TXIuVfA2EnCfSaBy9N4ASa;9%*DHl2=DX#W=w!c20CHiHon@ z6~vz0NUmlcM4?J6k+IiPKn-ecBk=yW=jhTCELZzM#Ol+Dq!X z0grMKO4%;%<8t$xx<@<4!h+I#8(X97B6E{erw!9sYa!hajW?aaeSLl5_q=NXU#Lr% zYkgTp#V$V(vkrJw!*`}Slk4hDXKI=0!QsbM(Ojg838F1mRa=r84!fx2gzM@;FF%&l zm9l1KKWHhWMCdS-3#AC*mBu*m~NOpx)X8p;+&;nFX1)c*yGgJ?#~9Ldx#<Pjo4G_K9J-c;jT8-=+aal-l*60~Dlw3}sZA zsd5W5NH+KMQDg@-hFDPtk9H86h|8Z2a7LU6Qg8M=Eax6i`%ZT(ii@i|DCJmKsEa*~ z{s>0Y_(zm+Gj#s$e2Fe&01OQ7%h^cU#rY-9Jm2W}xq>}RIDDNPy*@Nz41&;PeEMi0 zY9>TjQclDn;@=T#7T7~a-;=;?ohzD4gRj;_3p^LaIweJI$^ z=|;US1dI3hOjWsi`p2;fD*zb}>gdLcPyyjQN^&K!F$&mh(i`~WrnL|#+i+9rQM7Xv zt(PyQOrBPHfA0vd|I&{ORW)$yC>GE`!yQJ9Z?dHB2@shaG6Y?EIC!I!JZWw&@hihm zCWox)lxcd{qH}O{VSg+0=(bC!oUc^!@%JGnNK)X6s2X=v}HLweLYRI zrkqznXnn`b@A3^l9Y#@juc$Vr$z?hbH_frhkG-})Pr%oQg!McrFgmpZ0I*4q&kBTd z5@&|OzzO{y>dWKg4@ilpxIKYE_rOu|pp$y4C~D(Jya4^X*gROvw7dik_yc)YfWPq9 zb$kqMZn;(ZhJqS|{^&NBkKw9R{%JjCLp-nmaX=|@FeT@dm0L$R z1N>$e@V@Z!^xk%CMP_cbdjRMg*BcrAkl^c^Ei1H&HW@F`drM$!eDWkXXeFI%tW1X7 z(MUc`?&s@*d3Jb)0$;$7O_ZDu$3RtpOcr!-bLDcNUw-NATBKg+A=4`?&jw_fW>sYx zBNUfx82a>Kpy*uixV+mhs;0oU2-;UHk*0i}_?y8k^mJzaU9$J6Cxsp#>vNM+DN6kL z6X*_A*5PSIPzkS(+qz-#~!aE%tW@Gy+oRUhHB> z)VLz@b=jZrph0(8Pk_Hiti{WSwdd<3<9&(CiI?LaGa#p>t+}%e*wMv?!^w(0X~6j< zJfqHoP?U|^ktjlSRV?O5;gRgxvlpD2Kf0xLAZ!F}|`Xw%m$|Q`&?&ln9(HN3u zkOkK48Qd4LsHlvhHQ7Urlu%aSF~hQv>?nsx7afCtf31rvbCU`~3ov)bYelus7>@LI zuM7YX))d&YZ^7C0;2H=ywTPgReX52;u+x_SkA=qG2;A79v;{xo5T`#^^D=kUNloM8U7f^-m zRDYb^KU@UKF5p4Yu~xg!uR|xRgjMlT&_c!+B8Q-h0GoA_D+=Z}RG^rD8`|r|(Dy%G zV$%LH@=Lk#PeZ$!TRNEAfH~|vT%3cnRMmGtSiQG;asCX76JS}Fvf^UILzddt7J5{I z(_hd!8o}z41`EFv#Bn}@Z}Ik~0xm~g_AOySP9xamsF`9$S-PcVZJE5Dwp^7)ZG z=>8B{s^#4{ob{l3gACS=4M)AzpFrnc;Ok!C4K2Em-8kro&o-T^b&ju@jwR)WDtZpdRxJYx3D$ki`5B*fr0dK5f_X1#-@28ViP$P9Ju}W) zHgLF7&>rA-`#2qA4@GmMWm&L#m1UJ)GE<^+ji_G5iH&UWvr$BiV1B=S9^xAs#rerA zji=$Y4eb^{7m7m=rEpNvsdx^M=rxJSp5tYg(l;P7Gv*D;(SXG6>OcPaFux^h_`Ds6 z6qModC>FKGcrecRw~|`bo8WB%FHrtqiy;ITqJhnzk}qPGI*kmaZcm*?e?bv#6nl2 z(T!cs`EN1#FHF2d9c!R!AEL)75GR`$JxgmsFMsxkOf<3D*0h~1RDMKcUZcpzLntyo z2-iuUpbtwdD@2Qj6h-(aV0Q>~-=VdyMz7O4)}5WbeFmEE<^VBJ7npeb6X_jAPq5{c z_Ksd9=E_(0cu5Sj77s+=AR#ks0jZu1LKc67AQZax6@=z6&GF6+(k=*5wM=6m-_GQu z+Y_F9Nh&`6YP=0Dov6zGSm~HX+n*p)=4G-N-Bo5k*rg0e!$N-KEXbNQ)s9owL@;!NK_;-LN0wwG7pr&`M*6^ig%Jea0pIKabiqeDnu6l z7BWUM_(XV4hK?KqmqbcZ;_=Rjz^A>zHLr9A)nb`ZrmvZhT-)WVF_}$>93pX=U77ts z1>}$&pI!se`*o8RwrHOFSW?R=8|UCyMoKlYF&nAxfzhidL)IUH4IMx3R25>4#c_9S z;=wonm8kkTFZ`DB{_M_BaJZV*Mu6nZ7f z)}pBSiP+hiJj@JcS|h-f+Sl(OON3^u*b!eYF*_`>w_Ec)3Y5PM@X1!2^K)}e@)D(1 z)O4Gxk0+o~V^e0j^h@MFueLp_q&IONGWCCu$Coc(W`#=?<}f-U^SmsCa@$onSf@>{ zUn)J$F>TaKGgi4r{Y)=t#McGG*ydSBcXNPpE&*QLIhaJd2)#`vOTC_Pp6V_yIwK_| zcD$ll`nBUg3mH_k%%{Q`VMd*EM&1U4#{N3OnU$iTDk@@MGm+9lN#lc4f>sZQJCSvC zqT*w%XZ$zgFb?*j{-Rtfg~76R{Rh#U+(GqRDbmdgc2dI-fQJeu60-(N52K5I&sD1x#J&{qXr<$MQ3ltoDMjWR3Bn9w>v&#m9s8pGQNX9ohv zH+k@6xvTY$+%4+@rrby!oYK(cQ>rVcR1kf(D=GNwK}9h;-Ji&YQesmkxNQ<_^xyHB z7|^lM<>!qkoC&a!;c5&P2&2l`-T!EDeQglQX7IC?FtgmtQ))yF}v-=zoVpEny) zY*0bYOecrl!6@RB$(5oS-=jV7L@t*#(<5S;gNHbYy)DW}ufDB#uxr(lIc)(yyTE|# zP0xp^jWJHsrjR_W=T)85-LM4k@u+R{7H*|B@0K^>M;?wMdCgI@fu4y?N?%hqRJ`NX z;tAU~W+J5DVmK$4Lg4kr5zd2e;$XV!e21ryzi`r8h{twh+uRrGr6ap%zvVdB&J&25 z*xpJyfK(^iQ5Hvs>`@&)oIdVgPI#);xcd&3`$`679Zy)_GyK~CS5M7r*7h4pP7bvh z(V@1xib@|}L$=*%)-DE)HIksNj{HeZ@;me~((~j3Q)AV-`o}wVRz8h;&F%>9T7HS0 z)CRdW==@#VM(oI=)f}&u%pBG>Vvj682P&!00+94d`uwwK^Dp4M{rkZHYL9qw&xH_p zq5M{Qn(QWen#tTVJ_b*I8~P2JsE+0xe%TG0n9B}4UOGIwT}xf9D}875br0k`X!sLG zh44Ujl04Z^1c{EPIf+ieleaKUF@4rP$j{Lb@DLGn-2m{L0DaupF9AMaPG5G-lYlD+ zH|k2F?n7Q<0YA(Q+xl~f-ws-HzVlal3rFi?A*RmnYvCSU#XNB`Ec8VV9E(sfS&xoz zg6AXJ)0cZkC?V-}%}y{hO9!#9m1!nFmi$o_p-apWh3#6c!5!rLAV-0#^eaw$`0cJ39uG=6LiZk{jpb zCDH|$vZG7}Fnt1eZj7q7k}@T|hABik)t%28*d39XAWX3Z$+ z=!_wWzw&UzWB3Gi&^QF6TUH}q4!k_Z-lmD~^Cd3zA~p>Dn$B!xqP6ixh|xFT*tyE$ zf)o;am=(wC559Eg%`%lxWJ1?2iTxnY?O$un+?!a1&-N8MxN^V@W`+r$ZJ;v zf)dkL6q~H&U=D&Z`O)`qxG%;6+<*tiTwW#$@iANIcy(elU^enuU^BkjT+A!YQ;2GE z`|aYX^-E1u$&{oQj5BT^UwoVFy}+XUhICv+#PGpienVPh>O*RnaOXU)8fC@^;lU?Y zUp}GRLht;{_fLVI)hEt!4n9#{?mlGShxS5Q9#D z_B(!=$;xfF8SkH>QA(ydbY~DB##DJGa5rz+Iq9uUTX?E)wh@0bMuj^&X3XOIdgj_Q z_qF>g=OgXVckJ(jP+x1Ql0Jq)AJvp$ipAtwJ7ENNDDi;?)Az)NTV;W9a#WD@(ts+` z`*xxqIl2Q!^Sla1`xVABF;hUz-vb*4M3l8I^oG0Yx$F{cwo{k`Fx*1Wf~qVPVT~iK zjn9PyoxmUJ!am@ig}n(G%u5v4B*wu)tB3i5p?4#eiPgfN&K?#B?;3csI` zv@d~Aq$;=!Oqb2So}Y1=l3!~v!X~W?-|p9{6AHYw2uePPWMM12M_~YtNN&^Ni3iBq z=U3lo;Dz-&%6pt0e4cYhOs3@!(S+MgES3 zZ0XNZX2M)iSfL9rJv^cG@3A*NLyFNA6)cmVi7X_O^yU&UQX0>2Zg6(a50TN^;vC;8 zh|B@8BJ!Knpxqb7OzL$Mt)*` z?nkyj>pdZA#MgoAhfoEuFS)$KhCavyD(2g#P6u6P{iJ6UvqP|sO-=F7UEuF=9zMN< zqwTa;znU8x^EMA)F6E9e3t4gmUnoi5RLYBSVLu8EsH)-kbOxZ574N)`zrZoQ8tn;E z5QS+E+4+E=uz>q9OCS_wsLW!qRFy-S>Rk9kax+Ss5SlcAjS$dC2ylc%4K2KJ5ecZI zWC_xoaV-l{sh*-%{;*UclP-Z)D|fRmI|DOia}BdM0n}P3q7M~XJT6PNlXVYPIi!;J z)Rp}1uwGQIMj})XIvqNHbGq)vFc!=B%KEf@fe~wB*!;;{WvQM0#>`zpra8!H_cIes z5E)_@9GOd-M0{d5yKK`itA$Q!z?ZPZXr_sWYhm-L?JrM=YdytBHz7QQ*c}@UCm$wJ;3h zH2U+9r;$pvG;lAdD*<+OOH6Nj(P|bNkjHi7_T}sPx`f;_xtvAWX&sK19c}U!ZsQT! zk{-K#ICoe=-s=iNf@YT4Vzm|fBQ9&|ULqSY!3|6h#OIcBV&d_E&fU=RO3pqd`}JxeuJ1tXm97gD zs4yeM>Q`YqeO=SG@411!(3b>(prq7Iag3l7NNwLpVRNS;SM{dO`K`z7CxMQ&%6^fe z47^)g-V3)5b%THg;q!Tv?jrT=$n(&<{CK7e8Gu*q5+yy|4D`qM{8~3h4#15G)|08V z=%D265u8Qh_VcKO@ytr6_&1oYZw$OQUg^Kl{W9o8@AzD+x>zv?hC1jkREI0nO#+ZC zsAVU=IWAYwMaz|t+l2ngMQq>RjS#!xVl;#0hMIO^=Pz)-`g}Y7eZA&ajPBXW4SZNe zpo^ySV2Y~f3Z^Z7d*EAwHM7#MBu5cFp8R1(Yk;@D zj*&RWGuqfD9pX%GMy^2pQJxV#XRU}jyjKdcaC z^w@J!2e!gZM6uA+Ys@2hJ?!9Jo?Z)-M+n;2_jHQ1>3v)qPlY@3Ubiy|>MwYbvR!}U z(fVyo@FdQ2tatFEtIj5M#jjcHk(qX(`Hvzo?YGw>gc-74+;VT7Su~NIMEI?KGh=fS z6Y#u!k5s~+-rp9n-PDR#NuN^ICqG<3l_#a6gq`7%-R4WZMypRf*<$z$X|QQdky1k$0E-9r|863_teO9_g}kVJf5=1sXes_T3i@uE}xtx^5ahxilmZ&L8z2w22F5q}E9e=`0} z<@}rBfb=KhPtEgB#=i+{e=~Sc{$%{G-1aBnuaEd|1> Date: Fri, 30 Oct 2026 15:32:19 -0700 Subject: [PATCH 07/10] Record IMU inputs for replay and log odometry drops on robot thread Change Imu to hold the generated ImuIOInputsAutoLogged type and pass it to Logger.processInputs immediately after the hardware IO update. This records connection state, yaw and yaw rate, acceleration, latency, and the timestamped yaw samples consumed by DriveOdometry. In replay mode, AdvantageKit can now populate those fields from a new recording instead of leaving the no-op IMU at its disconnected, zero-yaw defaults. Existing logs made before this change do not contain the IMU input entry and cannot recover those missing samples. Stop calling AdvantageKit Logger.recordOutput from the Phoenix and Spark odometry sampling threads. Keep each dropped-sample count in a volatile field with a package-local snapshot getter, and publish the same existing log keys from DriveOdometry's main-thread periodic path in real mode. This follows AdvantageKit's single-threaded logging contract without changing queue sampling or the concurrent interpolation buffer. Verify the generated IMU serializer includes the yaw sample arrays. Run the full offline Gradle test suite with the installed WPILib Java 25 toolchain and confirm git diff --check is clean. Co-authored-by: Codex --- .../frc/robot/subsystems/drive/DriveOdometry.java | 8 ++++++++ .../subsystems/drive/PhoenixOdometryThread.java | 13 +++++-------- .../robot/subsystems/drive/SparkOdometryThread.java | 12 +++++------- src/main/java/frc/robot/subsystems/imu/Imu.java | 4 +++- 4 files changed, 21 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveOdometry.java b/src/main/java/frc/robot/subsystems/drive/DriveOdometry.java index 504a266..e443dbf 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveOdometry.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveOdometry.java @@ -349,6 +349,14 @@ public void rbsiPeriodic() { Logger.recordOutput("Odometry/SamplesProcessed", sampleCount); Logger.recordOutput("Odometry/VisionMeasurementsProcessed", visionMeasurementsProcessed); Logger.recordOutput("Odometry/BulkRefreshStatus", bulkRefreshStatus.toString()); + if (Constants.getMode() == Mode.REAL) { + Logger.recordOutput( + "Drive/OdomThread/DroppedSamples", + PhoenixOdometryThread.getInstance().getDroppedSamples()); + Logger.recordOutput( + "Drive/SparkOdomThread/DroppedSamples", + SparkOdometryThread.getInstance().getDroppedSamples()); + } } } } diff --git a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java index e379e50..98d2253 100644 --- a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java @@ -20,7 +20,6 @@ import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; import java.util.function.DoubleSupplier; -import org.littletonrobotics.junction.Logger; import org.wpilib.driverstation.DriverStationErrors; import org.wpilib.system.Timer; import org.wpilib.units.measure.Angle; @@ -48,8 +47,7 @@ public class PhoenixOdometryThread extends Thread { private static PhoenixOdometryThread instance = null; - private long droppedSamples = 0; - private long loopCount = 0; + private volatile long droppedSamples = 0; public static PhoenixOdometryThread getInstance() { if (instance == null) { @@ -58,6 +56,10 @@ public static PhoenixOdometryThread getInstance() { return instance; } + long getDroppedSamples() { + return droppedSamples; + } + private PhoenixOdometryThread() { setName("PhoenixOdometryThread"); setDaemon(true); @@ -179,11 +181,6 @@ public void run() { } finally { Drive.odometryLock.unlock(); } - - // every ~1s - if ((loopCount++ % (int) SwerveConstants.kOdometryFrequency) == 0) { - Logger.recordOutput("Drive/OdomThread/DroppedSamples", droppedSamples); - } } } diff --git a/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java index 5ff4370..47a2e1b 100644 --- a/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java @@ -15,7 +15,6 @@ import java.util.List; import java.util.Queue; import java.util.function.DoubleSupplier; -import org.littletonrobotics.junction.Logger; import org.wpilib.system.Notifier; import org.wpilib.system.Timer; @@ -37,8 +36,7 @@ public class SparkOdometryThread { private static SparkOdometryThread instance = null; private final Notifier notifier = new Notifier(this::run); - private long droppedSamples = 0; - private long loopCount = 0; + private volatile long droppedSamples = 0; public static SparkOdometryThread getInstance() { if (instance == null) { @@ -47,6 +45,10 @@ public static SparkOdometryThread getInstance() { return instance; } + long getDroppedSamples() { + return droppedSamples; + } + private SparkOdometryThread() { notifier.setName("OdometryThread"); } @@ -128,10 +130,6 @@ private void run() { } finally { Drive.odometryLock.unlock(); } - - if ((loopCount++ % (int) SwerveConstants.kOdometryFrequency) == 0) { - Logger.recordOutput("Drive/SparkOdomThread/DroppedSamples", droppedSamples); - } } private static LatestSampleQueue createQueue() { diff --git a/src/main/java/frc/robot/subsystems/imu/Imu.java b/src/main/java/frc/robot/subsystems/imu/Imu.java index b50e8c5..145c162 100644 --- a/src/main/java/frc/robot/subsystems/imu/Imu.java +++ b/src/main/java/frc/robot/subsystems/imu/Imu.java @@ -18,6 +18,7 @@ package frc.robot.subsystems.imu; import frc.robot.util.VirtualSubsystem; +import org.littletonrobotics.junction.Logger; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.geometry.Translation3d; @@ -25,7 +26,7 @@ public class Imu extends VirtualSubsystem { // Declare the io and inputs private final ImuIO io; - private final ImuIO.ImuIOInputs inputs = new ImuIO.ImuIOInputs(); + private final ImuIOInputsAutoLogged inputs = new ImuIOInputsAutoLogged(); // Per-cycle cached objects (to avoid repeated allocations) private long cacheStampNs = -1L; @@ -52,6 +53,7 @@ protected int getPeriodPriority() { /** Periodic function to read inputs */ public void rbsiPeriodic() { io.updateInputs(inputs); + Logger.processInputs("IMU", inputs); } public boolean isConnected() { From 9ee00dceebb8e5c7f9d417292f93a015e540d945 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Fri, 30 Oct 2026 15:42:05 -0700 Subject: [PATCH 08/10] Use Systemcore onboard acceleration and remove legacy thread priorities Remove the inherited roboRIO-era real-time priority changes from Robot construction and each robot loop. Retain the existing loop timing outputs, measuring virtual subsystem and scheduler work directly without priority transition intervals. Replace the disconnected roboRIO accelerometer stub with a controller IO backed by WPILib's Systemcore OnboardIMU. Carry its native meters-per-second- squared readings through the IO contract without multiplying by gravity. Timestamp samples in nanoseconds, mark non-finite readings invalid, and record raw controller inputs through AdvantageKit for replay. Rotate the onboard axes with a renamed Systemcore orientation constant and derive jerk from the actual sample interval rather than an assumed 20 ms loop. Rename acceleration outputs from Accel/Rio to Accel/Systemcore, remove the obsolete fixed sample-rate constant, and update configuration guidance and the no-op/units tests. Verify the full offline Gradle test suite with the installed WPILib Java 25 toolchain and check the diff for whitespace errors. Co-authored-by: Codex --- doc/RBSI-Constants.md | 20 ++--- src/main/java/frc/robot/Constants.java | 12 +-- src/main/java/frc/robot/Robot.java | 30 +------- src/main/java/frc/robot/RobotContainer.java | 4 +- .../accelerometer/Accelerometer.java | 73 +++++++++---------- ...RioAccelIO.java => ControllerAccelIO.java} | 33 +++++---- .../ControllerAccelIOOnboard.java | 43 +++++++++++ .../accelerometer/RioAccelIORoboRIO.java | 27 ------- src/test/frc/robot/RobotConstantsTest.java | 1 - .../accelerometer/ControllerAccelIOTests.java | 58 +++++++++++++++ .../accelerometer/RioAccelIOTests.java | 21 ------ 11 files changed, 163 insertions(+), 159 deletions(-) rename src/main/java/frc/robot/subsystems/accelerometer/{RioAccelIO.java => ControllerAccelIO.java} (53%) create mode 100644 src/main/java/frc/robot/subsystems/accelerometer/ControllerAccelIOOnboard.java delete mode 100644 src/main/java/frc/robot/subsystems/accelerometer/RioAccelIORoboRIO.java create mode 100644 src/test/frc/robot/subsystems/accelerometer/ControllerAccelIOTests.java delete mode 100644 src/test/frc/robot/subsystems/accelerometer/RioAccelIOTests.java diff --git a/doc/RBSI-Constants.md b/doc/RBSI-Constants.md index 12fbfb7..ebcc8ca 100644 --- a/doc/RBSI-Constants.md +++ b/doc/RBSI-Constants.md @@ -47,7 +47,8 @@ General constants are robot-wide support values: - `kLoopPeriodSecs`: main robot loop period. - `kTuningMode`: enables live tunable-number behavior. -- `kGravityMetersPerSecSq`: conversion from g to meters per second squared. +- `kGravityMetersPerSecSq`: conversion for vendor IMU readings reported in g. WPILib's + Systemcore `OnboardIMU` already reports m/s² and is not converted again. Most teams should leave these alone unless they have a specific framework-level reason. @@ -61,7 +62,7 @@ models: - `kMomentOfInertiaKgMetersSq` - `kWheelCoefficientOfFriction` - `kMaxWheelTorqueNm` -- `kRioOrientation` +- `kSystemcoreOrientation` - `kIMUOrientation` Tune these carefully: @@ -69,7 +70,8 @@ Tune these carefully: - `kMass`: measured robot weight including battery and bumpers. - `kMomentOfInertiaKgMetersSq`: estimate from CAD or measurement. - `kWheelCoefficientOfFriction`: carpet/wheel traction estimate. -- `kRioOrientation` and `kIMUOrientation`: must match the physical mounting orientation. +- `kSystemcoreOrientation` and `kIMUOrientation`: must match the physical mounting orientation. + The former rotates raw onboard acceleration into the robot frame. Incorrect orientation constants can corrupt acceleration logging, heading interpretation, and any logic that rotates sensor values into robot coordinates. @@ -154,16 +156,6 @@ Teams should tune this section carefully: Bad entries here cause misleading power logs and can make CAN health debugging much harder. -## SensorConstants - -`SensorConstants` holds configuration values for robot-wide sensors that do not naturally belong to -one mechanism: - -- `kRioAccelerometerSampleRateHz` - -Most teams can leave this alone. Increase it only if you have a specific acceleration or jerk -logging need and have verified that the extra sampling work is worth it. - ## OperatorConstants `OperatorConstants` configures driver feel and command-level control preferences. The physical @@ -424,7 +416,7 @@ Most teams should not change these unless they intentionally reorganize deploy f 1. Select `robotType`, `swerveType`, `autoType`, `visionType`, and `phoenixPro`. 2. Configure `CANBuses` and `RobotDevices`. 3. Set `RobotConstants` mass, moment of inertia, wheel friction, and sensor orientations. -4. Set `SensorConstants` and `PowerConstants`. +4. Set `PowerConstants` and verify the Systemcore accelerometer orientation. 5. Tune `OperatorConstants` with driver feedback. 6. Characterize and tune `DrivebaseConstants`. 7. Configure and validate cameras in `Cameras`. diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 83737d0..e6d2ef8 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -127,10 +127,10 @@ public static final class RobotConstants { // Kraken X60 stall torque ~7.09 Nm; MK4i L3 gear ratio 6.12:1 public static final double kMaxWheelTorqueNm = 43.4; - // Insert here the orientation (CCW == +) of the Rio and IMU from the robot + // Insert here the orientation (CCW == +) of the Systemcore and drive IMU from the robot // An angle of "0." means the x-y-z markings on the device match the robot's intrinsic reference // frame. - public static final Rotation3d kRioOrientation = + public static final Rotation3d kSystemcoreOrientation = switch (getRobot()) { case COMPBOT -> new Rotation3d(0, 0, Units.degreesToRadians(-90)); case DEVBOT1, DEVBOT2 -> Rotation3d.ZERO; @@ -261,14 +261,6 @@ public static class RobotDevices { // public static final int INTAKE_SERVO = 0; } - /************************************************************************* */ - /** Sensor Constants ***************************************************** */ - public static final class SensorConstants { - - // RoboRIO accelerometer sampler rate. - public static final double kRioAccelerometerSampleRateHz = 200.0; - } - /************************************************************************* */ /** Operator Constants *************************************************** */ public static class OperatorConstants { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4d206d7..1d245d6 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -36,7 +36,6 @@ import org.wpilib.driverstation.Alliance; import org.wpilib.driverstation.MatchState; import org.wpilib.math.geometry.Pose2d; -import org.wpilib.system.Threads; import org.wpilib.system.Timer; /** @@ -113,11 +112,6 @@ public Robot() { // Create a timer to disable motor brake a few seconds after disable. This will let the robot // stop immediately when disabled, but then also let it be pushed more m_disabledTimer = new Timer(); - - // Switch thread to high priority to improve loop timing - if (isReal()) { - setCurrentThreadPriorityFor2027Migration(99); - } } // /** This function is called periodically during all modes. */ @@ -125,12 +119,6 @@ public Robot() { public void robotPeriodic() { final long t0 = System.nanoTime(); - if (isReal()) { - // Switch thread to high priority to improve loop timing - setCurrentThreadPriorityFor2027Migration(99); - } - final long t1 = System.nanoTime(); - // Run all virtual subsystems each time through the loop VirtualSubsystem.periodicAll(); final long t2 = System.nanoTime(); @@ -143,16 +131,10 @@ public void robotPeriodic() { CommandScheduler.getInstance().run(); final long t3 = System.nanoTime(); - if (isReal()) { - // Return thread to normal priority - setCurrentThreadPriorityFor2027Migration(10); - } - final long t4 = System.nanoTime(); - if (++timingLogLoops >= TIMING_LOG_PERIOD_LOOPS) { timingLogLoops = 0; - Logger.recordOutput("LogPeriodic/CodeLoop/RobotPeriodicMS", (t4 - t0) / 1e6); - Logger.recordOutput("LogPeriodic/CodeLoop/VirtualMS", (t2 - t1) / 1e6); + Logger.recordOutput("LogPeriodic/CodeLoop/RobotPeriodicMS", (t3 - t0) / 1e6); + Logger.recordOutput("LogPeriodic/CodeLoop/VirtualMS", (t2 - t0) / 1e6); Logger.recordOutput("LogPeriodic/CodeLoop/SchedulerMS", (t3 - t2) / 1e6); } } @@ -275,12 +257,4 @@ public void utilityInit() { /** This function is called periodically during utility mode. */ @Override public void utilityPeriodic() {} - - @SuppressWarnings("deprecation") - private static void setCurrentThreadPriorityFor2027Migration(int priority) { - // TODO(2027): Revisit this as WPILib 2027 moves through beta and release. The current - // Threads API still works in alpha-6, but is deprecated because real-time priority misuse can - // lock up the system. Replace with the final WPILib guidance or remove the priority adjustment. - Threads.setCurrentThreadPriority(priority); - } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5817e77..ce6a579 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -30,7 +30,7 @@ import frc.robot.commands.AutopilotCommands; import frc.robot.commands.DriveCommands; import frc.robot.subsystems.accelerometer.Accelerometer; -import frc.robot.subsystems.accelerometer.RioAccelIO; +import frc.robot.subsystems.accelerometer.ControllerAccelIO; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.DriveOdometry; import frc.robot.subsystems.drive.SwerveConstants; @@ -213,7 +213,7 @@ public RobotContainer() { m_drivebase, m_drivebase::addVisionMeasurement, buildVisionIOsReplay(m_drivebase)); m_flywheel = new Flywheel(new FlywheelIO() {}); - m_accel = new Accelerometer(m_imu, RioAccelIO.noop()); + m_accel = new Accelerometer(m_imu, ControllerAccelIO.noop()); sweep = null; break; } diff --git a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java index 7ea75a2..2e7ab1b 100644 --- a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java +++ b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java @@ -13,9 +13,7 @@ package frc.robot.subsystems.accelerometer; -import frc.robot.Constants; import frc.robot.Constants.RobotConstants; -import frc.robot.Constants.SensorConstants; import frc.robot.subsystems.imu.Imu; import frc.robot.util.VirtualSubsystem; import org.littletonrobotics.junction.Logger; @@ -25,36 +23,31 @@ /** * Accelerometer subsystem (VirtualSubsystem) * - *

This virtual subsystem pulls the acceleration values from both the RoboRIO and the swerve's - * IMU (either Pigeon2 or NavX) and logs them to both AdvantageKitd. In addition to the - * accelerations, the jerk (a-dot or x-tripple-dot) is computed from the delta accelerations. + *

Reads the Systemcore's onboard accelerometer and the swerve IMU, rotates both into the robot + * frame, and logs acceleration and jerk through AdvantageKit. */ public class Accelerometer extends VirtualSubsystem { // Define hardware interfaces - private final RioAccelIO rio; - private final RioAccelIO.Inputs rioInputs = new RioAccelIO.Inputs(); + private final ControllerAccelIO controller; + private final ControllerAccelIOInputsAutoLogged controllerInputs = + new ControllerAccelIOInputsAutoLogged(); private final Imu imu; - // Variables needed during the periodic - private Translation3d rawRio, rioAcc, rioJerk, imuAcc, imuJerk; - private Translation3d prevRioAcc = null; + private Translation3d prevControllerAcc; + private long prevControllerTimestampNs = Long.MIN_VALUE; // Log decimation private int loopCount = 0; private static final int LOG_EVERY_N = 5; // 10Hz for heavier logs - // Profiling decimation - private int profileCount = 0; - private static final int PROFILE_EVERY_N = 50; // 1Hz profiling - public Accelerometer(Imu imu) { - this(imu, new RioAccelIORoboRIO(SensorConstants.kRioAccelerometerSampleRateHz)); + this(imu, new ControllerAccelIOOnboard()); } - public Accelerometer(Imu imu, RioAccelIO rio) { + public Accelerometer(Imu imu, ControllerAccelIO controller) { this.imu = imu; - this.rio = rio; + this.controller = controller; } // Priority value for this virtual subsystem @@ -65,41 +58,41 @@ protected int getPeriodPriority() { @Override public void rbsiPeriodic() { - final boolean doProfile = (++profileCount >= PROFILE_EVERY_N); - if (doProfile) profileCount = 0; - - // Fetch the values from the IMU and the RIO - final var imuInputs = imu.getInputs(); // should be primitive ImuIOInputs - rio.updateInputs(rioInputs); + final var imuInputs = imu.getInputs(); + controller.updateInputs(controllerInputs); + Logger.processInputs("Accel/Systemcore", controllerInputs); - // Compute RIO accelerations and jerks - rawRio = + // OnboardIMU returns m/s^2; only rotate the raw sensor axes into the robot frame. + Translation3d controllerAcc = new Translation3d( - rioInputs.xG * Constants.kGravityMetersPerSecSq, - rioInputs.yG * Constants.kGravityMetersPerSecSq, - rioInputs.zG * Constants.kGravityMetersPerSecSq); - rioAcc = rawRio.rotateBy(RobotConstants.kRioOrientation); - - Translation3d rioJerkThisLoop = - prevRioAcc == null - ? Translation3d.ZERO - : rioAcc.minus(prevRioAcc).div(Constants.kLoopPeriodSecs); - prevRioAcc = rioAcc; + controllerInputs.xMetersPerSecondSq, + controllerInputs.yMetersPerSecondSq, + controllerInputs.zMetersPerSecondSq) + .rotateBy(RobotConstants.kSystemcoreOrientation); + + Translation3d controllerJerk = Translation3d.ZERO; + if (controllerInputs.connected + && prevControllerAcc != null + && controllerInputs.timestampNs > prevControllerTimestampNs) { + double dtSecs = (controllerInputs.timestampNs - prevControllerTimestampNs) * 1e-9; + controllerJerk = controllerAcc.minus(prevControllerAcc).div(dtSecs); + } + prevControllerAcc = controllerInputs.connected ? controllerAcc : null; + prevControllerTimestampNs = controllerInputs.timestampNs; // IMU accelerations and jerks - imuAcc = imuInputs.linearAccel.rotateBy(RobotConstants.kIMUOrientation); + Translation3d imuAcc = imuInputs.linearAccel.rotateBy(RobotConstants.kIMUOrientation); // Logging - Logger.recordOutput("Accel/Rio/Accel_mps2", rioAcc); + Logger.recordOutput("Accel/Systemcore/Accel_mps2", controllerAcc); Logger.recordOutput("Accel/IMU/Accel_mps2", imuAcc); // Every N loops, compute and log the Jerk final boolean doHeavyLogs = (++loopCount >= LOG_EVERY_N); if (doHeavyLogs) { loopCount = 0; - rioJerk = rioJerkThisLoop; - imuJerk = imuInputs.linearJerk.rotateBy(RobotConstants.kIMUOrientation); - Logger.recordOutput("Accel/Rio/Jerk_mps3", rioJerk); + Translation3d imuJerk = imuInputs.linearJerk.rotateBy(RobotConstants.kIMUOrientation); + Logger.recordOutput("Accel/Systemcore/Jerk_mps3", controllerJerk); Logger.recordOutput("Accel/IMU/Jerk_mps3", imuJerk); final double[] ts = imuInputs.odometryYawTimestamps; diff --git a/src/main/java/frc/robot/subsystems/accelerometer/RioAccelIO.java b/src/main/java/frc/robot/subsystems/accelerometer/ControllerAccelIO.java similarity index 53% rename from src/main/java/frc/robot/subsystems/accelerometer/RioAccelIO.java rename to src/main/java/frc/robot/subsystems/accelerometer/ControllerAccelIO.java index 3507fa6..b781878 100644 --- a/src/main/java/frc/robot/subsystems/accelerometer/RioAccelIO.java +++ b/src/main/java/frc/robot/subsystems/accelerometer/ControllerAccelIO.java @@ -13,27 +13,28 @@ package frc.robot.subsystems.accelerometer; -public interface RioAccelIO { - final class Inputs { +import org.littletonrobotics.junction.AutoLog; + +/** Acceleration readings from the robot controller, in SI units. */ +public interface ControllerAccelIO { + @AutoLog + class ControllerAccelIOInputs { public boolean connected; public long timestampNs; - public double xG, yG, zG; // raw in g + public double xMetersPerSecondSq; + public double yMetersPerSecondSq; + public double zMetersPerSecondSq; } - default void updateInputs(Inputs inputs) {} - - default void close() {} + void updateInputs(ControllerAccelIOInputs inputs); - static RioAccelIO noop() { - return new RioAccelIO() { - @Override - public void updateInputs(Inputs inputs) { - inputs.connected = false; - inputs.timestampNs = 0L; - inputs.xG = 0.0; - inputs.yG = 0.0; - inputs.zG = 0.0; - } + static ControllerAccelIO noop() { + return inputs -> { + inputs.connected = false; + inputs.timestampNs = 0L; + inputs.xMetersPerSecondSq = 0.0; + inputs.yMetersPerSecondSq = 0.0; + inputs.zMetersPerSecondSq = 0.0; }; } } diff --git a/src/main/java/frc/robot/subsystems/accelerometer/ControllerAccelIOOnboard.java b/src/main/java/frc/robot/subsystems/accelerometer/ControllerAccelIOOnboard.java new file mode 100644 index 0000000..6ea2fc2 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/accelerometer/ControllerAccelIOOnboard.java @@ -0,0 +1,43 @@ +// Copyright (c) 2024-2027 Az-FIRST +// http://github.com/AZ-First +// +// 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. + +package frc.robot.subsystems.accelerometer; + +import org.wpilib.hardware.imu.OnboardIMU; +import org.wpilib.hardware.imu.OnboardIMU.MountOrientation; +import org.wpilib.system.RobotController; + +/** Reads the Systemcore's onboard IMU accelerometer. */ +public class ControllerAccelIOOnboard implements ControllerAccelIO { + private final OnboardIMU imu; + + public ControllerAccelIOOnboard() { + this(new OnboardIMU(MountOrientation.FLAT)); + } + + ControllerAccelIOOnboard(OnboardIMU imu) { + this.imu = imu; + } + + @Override + public void updateInputs(ControllerAccelIOInputs inputs) { + double x = imu.getAccelX(); + double y = imu.getAccelY(); + double z = imu.getAccelZ(); + inputs.timestampNs = RobotController.getTime(); + inputs.connected = Double.isFinite(x) && Double.isFinite(y) && Double.isFinite(z); + inputs.xMetersPerSecondSq = inputs.connected ? x : 0.0; + inputs.yMetersPerSecondSq = inputs.connected ? y : 0.0; + inputs.zMetersPerSecondSq = inputs.connected ? z : 0.0; + } +} diff --git a/src/main/java/frc/robot/subsystems/accelerometer/RioAccelIORoboRIO.java b/src/main/java/frc/robot/subsystems/accelerometer/RioAccelIORoboRIO.java deleted file mode 100644 index 5945510..0000000 --- a/src/main/java/frc/robot/subsystems/accelerometer/RioAccelIORoboRIO.java +++ /dev/null @@ -1,27 +0,0 @@ -// Copyright (c) 2024-2027 Az-FIRST -// http://github.com/AZ-First -// -// 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. - -package frc.robot.subsystems.accelerometer; - -public class RioAccelIORoboRIO implements RioAccelIO { - public RioAccelIORoboRIO(double sampleHz) {} - - @Override - public void updateInputs(Inputs inputs) { - inputs.connected = false; - inputs.timestampNs = 0L; - inputs.xG = 0.0; - inputs.yG = 0.0; - inputs.zG = 0.0; - } -} diff --git a/src/test/frc/robot/RobotConstantsTest.java b/src/test/frc/robot/RobotConstantsTest.java index da6edc5..afbaac1 100644 --- a/src/test/frc/robot/RobotConstantsTest.java +++ b/src/test/frc/robot/RobotConstantsTest.java @@ -110,7 +110,6 @@ void fieldLayoutConstantsAreInternallyConsistent() { @Test void selectedTunableConstantsAreInUsableRanges() { assertPositive(Constants.OperatorConstants.kRobotRelativeNudgeSpeedMetersPerSec); - assertPositive(Constants.SensorConstants.kRioAccelerometerSampleRateHz); assertPositive(Constants.DrivebaseConstants.kSysIdPreRunStopSecs); assertPositive(Constants.DrivebaseConstants.kFeedforwardCharacterizationStartDelaySecs); diff --git a/src/test/frc/robot/subsystems/accelerometer/ControllerAccelIOTests.java b/src/test/frc/robot/subsystems/accelerometer/ControllerAccelIOTests.java new file mode 100644 index 0000000..ae68d8e --- /dev/null +++ b/src/test/frc/robot/subsystems/accelerometer/ControllerAccelIOTests.java @@ -0,0 +1,58 @@ +package frc.robot.subsystems.accelerometer; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import org.junit.jupiter.api.Test; +import org.wpilib.hardware.hal.HAL; +import org.wpilib.hardware.imu.OnboardIMU; +import org.wpilib.hardware.imu.OnboardIMU.MountOrientation; + +class ControllerAccelIOTests { + @Test + void noopReportsDisconnectedZeroAcceleration() { + ControllerAccelIO.ControllerAccelIOInputs inputs = + new ControllerAccelIO.ControllerAccelIOInputs(); + + ControllerAccelIO.noop().updateInputs(inputs); + + assertFalse(inputs.connected); + assertEquals(0L, inputs.timestampNs); + assertEquals(0.0, inputs.xMetersPerSecondSq); + assertEquals(0.0, inputs.yMetersPerSecondSq); + assertEquals(0.0, inputs.zMetersPerSecondSq); + } + + @Test + void onboardReadingsStayInMetersPerSecondSquared() { + assertTrue(HAL.initialize()); + OnboardIMU imu = + new OnboardIMU(MountOrientation.FLAT) { + @Override + public double getAccelX() { + return 1.25; + } + + @Override + public double getAccelY() { + return -2.5; + } + + @Override + public double getAccelZ() { + return 9.8; + } + }; + ControllerAccelIO.ControllerAccelIOInputs inputs = + new ControllerAccelIO.ControllerAccelIOInputs(); + + new ControllerAccelIOOnboard(imu).updateInputs(inputs); + + assertTrue(inputs.connected); + assertEquals(1.25, inputs.xMetersPerSecondSq); + assertEquals(-2.5, inputs.yMetersPerSecondSq); + assertEquals(9.8, inputs.zMetersPerSecondSq); + assertTrue(inputs.timestampNs >= 0L); + } +} diff --git a/src/test/frc/robot/subsystems/accelerometer/RioAccelIOTests.java b/src/test/frc/robot/subsystems/accelerometer/RioAccelIOTests.java deleted file mode 100644 index 89d69cb..0000000 --- a/src/test/frc/robot/subsystems/accelerometer/RioAccelIOTests.java +++ /dev/null @@ -1,21 +0,0 @@ -package frc.robot.subsystems.accelerometer; - -import static org.junit.jupiter.api.Assertions.assertEquals; -import static org.junit.jupiter.api.Assertions.assertFalse; - -import org.junit.jupiter.api.Test; - -class RioAccelIOTests { - @Test - void noopReportsDisconnectedZeroAcceleration() { - RioAccelIO.Inputs inputs = new RioAccelIO.Inputs(); - - RioAccelIO.noop().updateInputs(inputs); - - assertFalse(inputs.connected); - assertEquals(0L, inputs.timestampNs); - assertEquals(0.0, inputs.xG); - assertEquals(0.0, inputs.yG); - assertEquals(0.0, inputs.zG); - } -} From 004965e45f87a609a799c57a541d2968673cf61b Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Fri, 30 Oct 2026 15:46:00 -0700 Subject: [PATCH 09/10] Document alpha-8 supported paths and isolate the example flywheel Make the 2027 Systemcore configuration explicit in a supported-configurations guide linked from the project front door and documentation index. Record the checked-in COMPBOT, Phoenix/Pigeon, manual-auto, and PhotonVision selections, along with the limits of each branch. Explain why YAGSL remains deferred despite the V3 vendordep, why V3 PathPlanner/Choreo are unavailable, and what remains to be supplied and tuned for the V2 autonomous integrations. Point to the temporary PhotonVision compatibility note. Separate the template flywheel from active robot hardware. Add an explicit ExampleFlywheelHardware selector that defaults to DISABLED. The REAL container now constructs no flywheel motor controllers until TALON_FX or SPARK_MAX is deliberately chosen; simulation retains the simulated IO, and replay retains the no-op IO. Register the right-bumper flywheel command and flywheel SysId chooser entries only when the example has a simulated or selected physical IO. Keep the sample CAN and power-port IDs visibly labeled as examples. Update the constants, autonomous, and SysId guides so the physical bring-up steps match the runtime defaults. This avoids publishing a characterization option for an unconnected example mechanism on a real robot while retaining the example for simulation and deliberate hardware adoption. Verification: spotlessApply and the complete offline Gradle test suite passed with the bundled WPILib Java 25 JDK. Co-authored-by: Codex --- README.md | 1 + doc/RBSI-Autonomous.md | 2 +- doc/RBSI-Constants.md | 7 ++ doc/RBSI-SysId.md | 6 +- doc/README.md | 3 +- doc/SUPPORTED-CONFIGURATIONS.md | 39 ++++++++++ src/main/java/frc/robot/Constants.java | 18 +++-- src/main/java/frc/robot/RobotContainer.java | 86 +++++++++++++-------- 8 files changed, 119 insertions(+), 43 deletions(-) create mode 100644 doc/SUPPORTED-CONFIGURATIONS.md diff --git a/README.md b/README.md index 8282536..be85f8f 100644 --- a/README.md +++ b/README.md @@ -34,6 +34,7 @@ Start with the full documentation index: Quick links: +- [2027 alpha-8 supported configurations](doc/SUPPORTED-CONFIGURATIONS.md) - [Install and project setup](doc/INSTALL.md) - [Getting started guide](doc/RBSI-GSG.md) - [Constants guide](doc/RBSI-Constants.md) diff --git a/doc/RBSI-Autonomous.md b/doc/RBSI-Autonomous.md index 3e025ab..021e9a2 100644 --- a/doc/RBSI-Autonomous.md +++ b/doc/RBSI-Autonomous.md @@ -193,7 +193,7 @@ When using PathPlanner mode, the auto chooser includes: - drive wheel radius characterization, - drive feedforward characterization, - drive SysId routines, -- flywheel SysId routines. +- flywheel SysId routines when the example mechanism is enabled. Run these with the robot in a safe area and the Driver Station ready to disable. Characterization commands intentionally command motors in ways that do not feel diff --git a/doc/RBSI-Constants.md b/doc/RBSI-Constants.md index ebcc8ca..3dcd5e8 100644 --- a/doc/RBSI-Constants.md +++ b/doc/RBSI-Constants.md @@ -311,6 +311,13 @@ Drive characterization helpers also use constants here: - `kWheelRadiusCharacterizationRampRateRadPerSecSq`: spin-speed slew rate during wheel radius characterization. +## Example flywheel hardware selection + +`Constants.kExampleFlywheelHardware` defaults to `DISABLED`. The example CAN IDs below it do not +activate real motors by themselves. Choose `TALON_FX` or `SPARK_MAX` only after replacing and +checking the sample device IDs, power ports, and mechanism limits. Simulation uses `FlywheelIOSim` +regardless of this real-hardware selector. See [supported configurations](SUPPORTED-CONFIGURATIONS.md). + ## FlywheelConstants `FlywheelConstants` configures the example flywheel subsystem: diff --git a/doc/RBSI-SysId.md b/doc/RBSI-SysId.md index 434cfce..88fb4bd 100644 --- a/doc/RBSI-SysId.md +++ b/doc/RBSI-SysId.md @@ -64,7 +64,9 @@ controllers. ## Chooser Entries The routines are published in the PathPlanner auto chooser when `Constants.getAutoType()` is -`PATHPLANNER`. +`PATHPLANNER` and the example flywheel is enabled for simulation or real hardware. Real hardware +defaults to `DISABLED` in `Constants.kExampleFlywheelHardware`; see +`SUPPORTED-CONFIGURATIONS.md` before selecting a physical IO. Run all four voltage routines: @@ -86,7 +88,7 @@ Optional comparison routines: mechanism that is not safe in reverse. 2. Put the robot in a safe open area, with the flywheel pointed away from people and loose objects. 3. Use a charged battery. -4. Confirm the example flywheel IO selected in `RobotContainer` matches the hardware being tested. +4. Confirm the example flywheel hardware selected in `Constants` matches the hardware being tested. 5. Confirm gear ratio and encoder units in `Constants.FlywheelConstants.kGearRatio`. 6. Confirm the follower motor is physically and logically correct. A missing or inverted follower will poison the characterization data. diff --git a/doc/README.md b/doc/README.md index 471d9ff..67ec663 100644 --- a/doc/README.md +++ b/doc/README.md @@ -10,6 +10,7 @@ project front door; this page is the working table of contents. | Page | Use It For | | --- | --- | +| [SUPPORTED-CONFIGURATIONS.md](SUPPORTED-CONFIGURATIONS.md) | Current 2027 branch support, deferred paths, and example hardware activation. | | [INSTALL.md](INSTALL.md) | Creating a project, setting team number, generating Phoenix Tuner X constants, and first setup tasks. | | [RBSI-GSG.md](RBSI-GSG.md) | First robot-code changes after installation. | | [RBSI-Constants.md](RBSI-Constants.md) | Understanding `Constants.java` and the tuning order for a new robot. | @@ -29,7 +30,7 @@ project front door; this page is the working table of contents. ## New User Bring-Up Path -1. [Install the project](INSTALL.md). +1. [Check the supported configuration](SUPPORTED-CONFIGURATIONS.md), then [install the project](INSTALL.md). 2. [Make first code changes](RBSI-GSG.md). 3. [Select robot, swerve, vision, and auto modes](RBSI-Constants.md). 4. [Generate or configure drivetrain constants](RBSI-Drive.md). diff --git a/doc/SUPPORTED-CONFIGURATIONS.md b/doc/SUPPORTED-CONFIGURATIONS.md new file mode 100644 index 0000000..e8343b9 --- /dev/null +++ b/doc/SUPPORTED-CONFIGURATIONS.md @@ -0,0 +1,39 @@ +# 2027 alpha-8 configuration status + +This page describes the checked-in setup. A successful build or simulation does not certify a +physical robot. Confirm CAN IDs, bus routing, geometry, camera transforms, and mechanism limits on +your own hardware before driving. + +| Area | `alpha8` (Commands V2) | `commands_v3` (Commands V3) | +| --- | --- | --- | +| Robot and controller | `COMPBOT` on Systemcore; desktop runs replay | Same | +| Drive | Phoenix 6 and Pigeon, configured path | Same | +| YAGSL | Runtime deferred; selecting it throws | V3 vendordep builds, but runtime remains deferred | +| Autonomous | `MANUAL` selected; PathPlanner and Choreo integrations compile but need paths and robot tuning | `MANUAL` selected; PathPlanner and Choreo disabled pending V3 libraries | +| Vision | PhotonVision selected with a temporary 2027 field-layout compatibility shim | Same | +| Example flywheel | Disabled on real hardware by default; simulation model available | Same | + +The selectors live in `Constants.java`. The default `SwerveType.PHOENIX6` is the only wired 2027 +drive path. The deploy YAGSL JSON uses an older format and a roboRIO `navx_spi` gyro. Regenerate it +for 2027 and select a Systemcore-compatible gyro before restoring the YAGSL mapping. On V3 the +vendordep alone does not enable that mapping. See `Commands-V3-Branch.md` on the V3 branch. + +The V2 PathPlanner and Choreo code is an integration example. Supply valid paths/trajectories, +check the field and pose conventions, and tune the robot before selecting those modes. On V3, +keep `AutoType.MANUAL` until V3-compatible libraries and adapters are available. The PhotonVision +shim is tracked in `PHOTONVISION-COMPAT.md` and should be removed after a compatible release. + +## Example flywheel + +`Constants.kExampleFlywheelHardware` defaults to `DISABLED`. On a real Systemcore, this creates +no flywheel motor controllers and registers neither the right-bumper flywheel binding nor flywheel +SysId choices. `FLYWHEEL_LEADER` and `FLYWHEEL_FOLLOWER` are sample CAN and power-port numbers, +not an installed mechanism. In simulation, `FlywheelIOSim` and its controls remain available. +Replay uses no hardware IO or flywheel controls. + +To install a physical example, first replace those device IDs and ports, verify the gearing, +motor direction, current limits and SysId limits, then select `TALON_FX` or `SPARK_MAX` in +`Constants.kExampleFlywheelHardware`. This opts the real robot into the matching IO, bumper +binding, and flywheel SysId choices (PathPlanner chooser on V2, manual chooser on V3). See +`RBSI-SysId.md` before characterization. For a competition mechanism, replace the example with +its own IO, commands, controls, and power monitoring configuration. diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e6d2ef8..a508a75 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -69,15 +69,23 @@ public final class Constants { private static RobotType robotType = RobotType.COMPBOT; // Define swerve, auto, and vision types being used - // NOTE: Only PHOENIX6 swerve base has been tested at this point!!! - // If you have a swerve base with non-CTRE components, use YAGSL - // under strict caveat emptor -- and submit any error and bugfixes - // via GitHub issues. + // 2027: PHOENIX6 is the configured path. YAGSL runtime is deferred; see + // doc/SUPPORTED-CONFIGURATIONS.md before selecting another value. private static SwerveType swerveType = SwerveType.PHOENIX6; // PHOENIX6, YAGSL private static CTREPro phoenixPro = CTREPro.LICENSED; // LICENSED, UNLICENSED private static AutoType autoType = AutoType.MANUAL; // MANUAL, PATHPLANNER, CHOREO private static VisionType visionType = VisionType.PHOTON; // PHOTON, LIMELIGHT, NONE + /** Example mechanism only. DISABLED leaves real CAN devices untouched. */ + public static final ExampleFlywheelHardware kExampleFlywheelHardware = + ExampleFlywheelHardware.DISABLED; + + public enum ExampleFlywheelHardware { + DISABLED, + TALON_FX, + SPARK_MAX + } + /** Enumerate the robot types (name your robots here) */ public static enum RobotType { DEVBOT1, // Development / Alpha / Practice Bot @@ -245,7 +253,7 @@ public static class RobotDevices { /* SUBSYSTEM CAN DEVICE IDS */ // This is where mechanism subsystem devices are defined (Including ID, bus, and power port) - // Example: + // Example only: these IDs are not active unless kExampleFlywheelHardware is selected. public static final RobotDeviceId FLYWHEEL_LEADER = new RobotDeviceId(3, CANBuses.RIO, 8); public static final RobotDeviceId FLYWHEEL_FOLLOWER = new RobotDeviceId(4, CANBuses.RIO, 9); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ce6a579..7eb28b6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -37,6 +37,8 @@ import frc.robot.subsystems.flywheel_example.Flywheel; import frc.robot.subsystems.flywheel_example.FlywheelIO; import frc.robot.subsystems.flywheel_example.FlywheelIOSim; +import frc.robot.subsystems.flywheel_example.FlywheelIOSpark; +import frc.robot.subsystems.flywheel_example.FlywheelIOTalonFX; import frc.robot.subsystems.imu.Imu; import frc.robot.subsystems.imu.ImuIO; import frc.robot.subsystems.imu.ImuIOSim; @@ -97,6 +99,10 @@ public class RobotContainer { private final Drive m_drivebase; private final Flywheel m_flywheel; + private final boolean flywheelExampleEnabled = + Constants.getMode() == Mode.SIM + || (Constants.getMode() == Mode.REAL + && Constants.kExampleFlywheelHardware != Constants.ExampleFlywheelHardware.DISABLED); // ... Add additional subsystems here (e.g., elevator, arm, etc.) @@ -164,7 +170,7 @@ public RobotContainer() { m_vision = new Vision( m_drivebase, m_drivebase::addVisionMeasurement, buildVisionIOsReal(m_drivebase)); - m_flywheel = new Flywheel(new FlywheelIOSim()); // new Flywheel(new FlywheelIOTalonFX()); + m_flywheel = new Flywheel(createRealExampleFlywheelIO()); m_accel = new Accelerometer(m_imu); sweep = null; break; @@ -322,14 +328,16 @@ private void configureBindings() { // Press Y / Triangle button --> Manually Re-Zero the Gyro driverController.northFaceButton().onTrue(DriveCommands.zeroHeadingForAlliance(m_drivebase)); - // Press RIGHT BUMPER / R1 --> Run the example flywheel - driverController - .rightBumper() - .whileTrue( - Commands.startEnd( - () -> m_flywheel.runVelocity(flywheelSpeedInput.getAsDouble()), - m_flywheel::stop, - m_flywheel)); + // Example mechanism controls are available in simulation or with an explicit real IO choice. + if (flywheelExampleEnabled) { + driverController + .rightBumper() + .whileTrue( + Commands.startEnd( + () -> m_flywheel.runVelocity(flywheelSpeedInput.getAsDouble()), + m_flywheel::stop, + m_flywheel)); + } // Press LEFT BUMPER / L1 --> Drive to a demo pose offset defined in OperatorConstants driverController @@ -489,34 +497,44 @@ private void definesysIdRoutines() { "Drive SysId (Dynamic Reverse)", m_drivebase.sysIdDynamic(SysIdRoutine.Direction.REVERSE)); - // Example Flywheel SysId Characterization - autoChooserPathPlanner.add( - "Flywheel SysId Voltage (Quasistatic Forward)", - m_flywheel.sysIdVoltageQuasistatic(SysIdRoutine.Direction.FORWARD)); - autoChooserPathPlanner.add( - "Flywheel SysId Voltage (Quasistatic Reverse)", - m_flywheel.sysIdVoltageQuasistatic(SysIdRoutine.Direction.REVERSE)); - autoChooserPathPlanner.add( - "Flywheel SysId Voltage (Dynamic Forward)", - m_flywheel.sysIdVoltageDynamic(SysIdRoutine.Direction.FORWARD)); - autoChooserPathPlanner.add( - "Flywheel SysId Voltage (Dynamic Reverse)", - m_flywheel.sysIdVoltageDynamic(SysIdRoutine.Direction.REVERSE)); - autoChooserPathPlanner.add( - "Flywheel SysId Duty Cycle (Quasistatic Forward)", - m_flywheel.sysIdDutyCycleQuasistatic(SysIdRoutine.Direction.FORWARD)); - autoChooserPathPlanner.add( - "Flywheel SysId Duty Cycle (Quasistatic Reverse)", - m_flywheel.sysIdDutyCycleQuasistatic(SysIdRoutine.Direction.REVERSE)); - autoChooserPathPlanner.add( - "Flywheel SysId Duty Cycle (Dynamic Forward)", - m_flywheel.sysIdDutyCycleDynamic(SysIdRoutine.Direction.FORWARD)); - autoChooserPathPlanner.add( - "Flywheel SysId Duty Cycle (Dynamic Reverse)", - m_flywheel.sysIdDutyCycleDynamic(SysIdRoutine.Direction.REVERSE)); + if (flywheelExampleEnabled) { + // Example Flywheel SysId Characterization + autoChooserPathPlanner.add( + "Flywheel SysId Voltage (Quasistatic Forward)", + m_flywheel.sysIdVoltageQuasistatic(SysIdRoutine.Direction.FORWARD)); + autoChooserPathPlanner.add( + "Flywheel SysId Voltage (Quasistatic Reverse)", + m_flywheel.sysIdVoltageQuasistatic(SysIdRoutine.Direction.REVERSE)); + autoChooserPathPlanner.add( + "Flywheel SysId Voltage (Dynamic Forward)", + m_flywheel.sysIdVoltageDynamic(SysIdRoutine.Direction.FORWARD)); + autoChooserPathPlanner.add( + "Flywheel SysId Voltage (Dynamic Reverse)", + m_flywheel.sysIdVoltageDynamic(SysIdRoutine.Direction.REVERSE)); + autoChooserPathPlanner.add( + "Flywheel SysId Duty Cycle (Quasistatic Forward)", + m_flywheel.sysIdDutyCycleQuasistatic(SysIdRoutine.Direction.FORWARD)); + autoChooserPathPlanner.add( + "Flywheel SysId Duty Cycle (Quasistatic Reverse)", + m_flywheel.sysIdDutyCycleQuasistatic(SysIdRoutine.Direction.REVERSE)); + autoChooserPathPlanner.add( + "Flywheel SysId Duty Cycle (Dynamic Forward)", + m_flywheel.sysIdDutyCycleDynamic(SysIdRoutine.Direction.FORWARD)); + autoChooserPathPlanner.add( + "Flywheel SysId Duty Cycle (Dynamic Reverse)", + m_flywheel.sysIdDutyCycleDynamic(SysIdRoutine.Direction.REVERSE)); + } } } + private static FlywheelIO createRealExampleFlywheelIO() { + return switch (Constants.kExampleFlywheelHardware) { + case DISABLED -> new FlywheelIO() {}; + case TALON_FX -> new FlywheelIOTalonFX(); + case SPARK_MAX -> new FlywheelIOSpark(); + }; + } + // Vision Factories // Vision Factories (REAL) private VisionIO[] buildVisionIOsReal(Drive drive) { From 4c5f2a3130303c9b46ccb7f46807c92c85237a82 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Fri, 30 Oct 2026 15:13:59 -0700 Subject: [PATCH 10/10] Log Commands V2 lifecycle and active state with AdvantageKit Register the Commands V2 scheduler's initialize, finish, and interrupt callbacks after starting AdvantageKit and before constructing RobotContainer. Track active commands from those callbacks and publish the active list to Commands/Active once per robot loop. Collect lifecycle transitions in order and write them as a Commands/Events string array after each scheduler run. Keeping all transitions from a loop in one array preserves commands that start and finish within a single loop and avoids overwriting earlier events at the same AdvantageKit key. Include the interrupting command when the scheduler provides one. Assign stable per-instance command labels so multiple instances with the same name can be distinguished without relying on VM identity hash values that change between runs. Retain the existing Field2d telemetry publication for dashboard compatibility; this commit changes only command logging. Verify the full offline Gradle test suite with the installed WPILib Java 25 toolchain and confirm git diff --check is clean. Co-authored-by: Codex --- src/main/java/frc/robot/Robot.java | 40 ++++++++++++++++++++++++++++++ 1 file changed, 40 insertions(+) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1d245d6..7bc8ab8 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -23,6 +23,9 @@ import com.revrobotics.util.StatusLogger; import frc.robot.Constants.PowerConstants; import frc.robot.util.VirtualSubsystem; +import java.util.ArrayList; +import java.util.IdentityHashMap; +import java.util.LinkedHashSet; import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedPowerDistribution; import org.littletonrobotics.junction.LoggedRobot; @@ -51,6 +54,10 @@ public class Robot extends LoggedRobot { private RobotContainer m_robotContainer; private Timer m_disabledTimer; private int timingLogLoops = 0; + private final ArrayList commandEvents = new ArrayList<>(); + private final LinkedHashSet activeCommands = new LinkedHashSet<>(); + private final IdentityHashMap commandIds = new IdentityHashMap<>(); + private int nextCommandId = 1; /** * This function is run when the robot is first started up and should be used for any @@ -104,6 +111,7 @@ public Robot() { // Start AdvantageKit logger Logger.start(); + configureCommandLogging(); // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. @@ -129,6 +137,10 @@ public void robotPeriodic() { // This must be called from the robot's periodic block in order for anything in // the Command-based framework to work. CommandScheduler.getInstance().run(); + Logger.recordOutput( + "Commands/Active", activeCommands.stream().map(this::commandLabel).toArray(String[]::new)); + Logger.recordOutput("Commands/Events", commandEvents.toArray(String[]::new)); + commandEvents.clear(); final long t3 = System.nanoTime(); if (++timingLogLoops >= TIMING_LOG_PERIOD_LOOPS) { @@ -139,6 +151,34 @@ public void robotPeriodic() { } } + private void configureCommandLogging() { + CommandScheduler scheduler = CommandScheduler.getInstance(); + scheduler.onCommandInitialize( + command -> { + activeCommands.add(command); + commandEvents.add("Initialize " + commandLabel(command)); + }); + scheduler.onCommandFinish( + command -> { + activeCommands.remove(command); + commandEvents.add("Finish " + commandLabel(command)); + }); + scheduler.onCommandInterrupt( + (command, interruptor) -> { + activeCommands.remove(command); + commandEvents.add( + "Interrupt " + + commandLabel(command) + + interruptor.map(value -> " by " + commandLabel(value)).orElse("")); + }); + } + + private String commandLabel(Command command) { + return command.getName() + + "#" + + commandIds.computeIfAbsent(command, ignored -> nextCommandId++); + } + /** This function is called once when the robot is disabled. */ @Override public void disabledInit() {