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/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..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": { @@ -74,5 +75,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/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/README.md b/README.md index 138143d..be85f8f 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 @@ -35,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/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/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 12fbfb7..3dcd5e8 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 @@ -319,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: @@ -424,7 +423,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/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/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..a508a75 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 @@ -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 @@ -68,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 @@ -126,21 +135,21 @@ 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.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 +198,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); }; @@ -244,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); @@ -260,14 +269,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 { @@ -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/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 f4ce0e9..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 @@ -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/Robot.java b/src/main/java/frc/robot/Robot.java index 56df2fe..7bc8ab8 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 @@ -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; @@ -36,7 +39,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; /** @@ -52,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 @@ -105,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. @@ -113,11 +120,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 +127,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(); @@ -141,22 +137,48 @@ 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 (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); } } + 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() { @@ -275,12 +297,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 8a4a71c..7eb28b6 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 @@ -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; @@ -27,13 +30,15 @@ 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; 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; @@ -43,9 +48,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; import frc.robot.util.RBSICANHealth; @@ -57,7 +59,9 @@ import java.util.Arrays; import java.util.List; import java.util.Set; -import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; +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; @@ -72,6 +76,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 { @@ -93,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.) @@ -114,24 +124,28 @@ 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 AutoFactory autoFactoryChoreo; + 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 = - 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("", AlertType.INFO); + private final Alert aprilTagLayoutAlert = new Alert("aprilTagLayout", "", Level.LOW); /** * Constructor for the Robot Container. This container holds subsystems, operator interface @@ -156,13 +170,13 @@ 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; case SIM: - RBSICANBusRegistry.initSim(CANBuses.ALL); + RBSICANBusRegistry.initSim(); m_imu = new Imu(new ImuIOSim()); m_drivebase = new Drive(m_imu); @@ -196,7 +210,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()); @@ -205,7 +219,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; } @@ -228,32 +242,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 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; - // 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 LoggedDashboardChooser<>("Choreo Auto Choices"); - // autoChooserChoreo.addDefaultOption("Nothing", Commands.none()); - // autoChooserChoreo.addOption("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; @@ -265,11 +276,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); } } @@ -318,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.get()), - 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 @@ -341,7 +353,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 +392,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( @@ -430,8 +443,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. */ @@ -466,53 +478,63 @@ 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)); - - // Example Flywheel SysId Characterization - autoChooserPathPlanner.addOption( - "Flywheel SysId Voltage (Quasistatic Forward)", - m_flywheel.sysIdVoltageQuasistatic(SysIdRoutine.Direction.kForward)); - autoChooserPathPlanner.addOption( - "Flywheel SysId Voltage (Quasistatic Reverse)", - m_flywheel.sysIdVoltageQuasistatic(SysIdRoutine.Direction.kReverse)); - autoChooserPathPlanner.addOption( - "Flywheel SysId Voltage (Dynamic Forward)", - m_flywheel.sysIdVoltageDynamic(SysIdRoutine.Direction.kForward)); - autoChooserPathPlanner.addOption( - "Flywheel SysId Voltage (Dynamic Reverse)", - m_flywheel.sysIdVoltageDynamic(SysIdRoutine.Direction.kReverse)); - autoChooserPathPlanner.addOption( - "Flywheel SysId Duty Cycle (Quasistatic Forward)", - m_flywheel.sysIdDutyCycleQuasistatic(SysIdRoutine.Direction.kForward)); - autoChooserPathPlanner.addOption( - "Flywheel SysId Duty Cycle (Quasistatic Reverse)", - m_flywheel.sysIdDutyCycleQuasistatic(SysIdRoutine.Direction.kReverse)); - autoChooserPathPlanner.addOption( - "Flywheel SysId Duty Cycle (Dynamic Forward)", - m_flywheel.sysIdDutyCycleDynamic(SysIdRoutine.Direction.kForward)); - autoChooserPathPlanner.addOption( - "Flywheel SysId Duty Cycle (Dynamic Reverse)", - m_flywheel.sysIdDutyCycleDynamic(SysIdRoutine.Direction.kReverse)); + m_drivebase.sysIdDynamic(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) { @@ -570,15 +592,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/AutopilotCommands.java b/src/main/java/frc/robot/commands/AutopilotCommands.java index 02e5252..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 @@ -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/ChoreoAutoController.java b/src/main/java/frc/robot/commands/ChoreoAutoController.java index c7bb2b4..6e590cb 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/ @@ -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/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 1483e91..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 @@ -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; @@ -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..2e7ab1b 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 @@ -13,48 +13,41 @@ 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.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) * - *

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,46 +58,46 @@ 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.kZero - : 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; 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/accelerometer/RioAccelIO.java b/src/main/java/frc/robot/subsystems/accelerometer/ControllerAccelIO.java similarity index 50% rename from src/main/java/frc/robot/subsystems/accelerometer/RioAccelIO.java rename to src/main/java/frc/robot/subsystems/accelerometer/ControllerAccelIO.java index 2a60a89..b781878 100644 --- a/src/main/java/frc/robot/subsystems/accelerometer/RioAccelIO.java +++ b/src/main/java/frc/robot/subsystems/accelerometer/ControllerAccelIO.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 @@ -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 5d5ff88..0000000 --- a/src/main/java/frc/robot/subsystems/accelerometer/RioAccelIORoboRIO.java +++ /dev/null @@ -1,27 +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.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/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index d559585..1fa9e16 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 @@ -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; @@ -30,18 +31,15 @@ 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; 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; +import java.util.concurrent.atomic.AtomicInteger; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; import org.littletonrobotics.junction.AutoLogOutput; @@ -64,8 +62,12 @@ 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.smartdashboard.SmartDashboard; +import org.wpilib.system.Timer; +import org.wpilib.telemetry.Telemetry; +import org.wpilib.util.Alert; +import org.wpilib.util.Alert.Level; /** * Drive subsystem (RBSISubsystem) @@ -74,6 +76,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 +96,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 +128,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 +246,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 +295,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 +390,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 +420,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 +594,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 +634,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()); } @@ -723,7 +732,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` */ @@ -763,7 +772,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); @@ -778,7 +787,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) @@ -864,17 +873,17 @@ 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()); + markPoseReset(Timer.getTimestamp()); } /** Zeros the gyro regardless of the alliance */ public void zeroHeading() { - imu.zeroYaw(Rotation2d.kZero); + imu.zeroYaw(Rotation2d.ZERO); resetHeadingController(); - markPoseReset(TimeUtil.now()); + markPoseReset(Timer.getTimestamp()); } /** @@ -989,7 +998,7 @@ private void applyVisionMeasurementLocked(TimedPose meas) { private void markVisionMeasurementAccepted(double measurementTimestamp) { lastAcceptedVisionMeasurementTimestamp = measurementTimestamp; - lastAcceptedVisionReceiptTimestamp = TimeUtil.now(); + lastAcceptedVisionReceiptTimestamp = Timer.getTimestamp(); } /** @@ -1107,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.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())); - // } + 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/main/java/frc/robot/subsystems/drive/DriveOdometry.java b/src/main/java/frc/robot/subsystems/drive/DriveOdometry.java index e66b4cc..e443dbf 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 @@ -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]); } @@ -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/DriveSimPhysics.java b/src/main/java/frc/robot/subsystems/drive/DriveSimPhysics.java index c2f05af..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 @@ -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/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 a516e0d..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 @@ -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..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 @@ -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..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 @@ -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..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 @@ -10,14 +10,13 @@ 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; 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 @@ -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); @@ -114,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/ModuleIOSpark.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java index e449848..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 @@ -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..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 @@ -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..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 @@ -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..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 @@ -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/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java index 4dfb496..98d2253 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 @@ -20,9 +20,8 @@ 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.RobotController; +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); @@ -157,7 +159,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(); @@ -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 00e9b4a..47a2e1b 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 @@ -15,9 +15,8 @@ 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.RobotController; +import org.wpilib.system.Timer; /** * Provides an interface for asynchronously reading high-frequency measurements to a set of queues. @@ -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"); } @@ -101,7 +103,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()]; @@ -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/drive/SparkSwerveUnits.java b/src/main/java/frc/robot/subsystems/drive/SparkSwerveUnits.java new file mode 100644 index 0000000..670c826 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/SparkSwerveUnits.java @@ -0,0 +1,44 @@ +package frc.robot.subsystems.drive; + +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 { + 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/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 7c3e99e..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 @@ -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/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 76ebf36..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 @@ -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..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 @@ -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/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 8927ff0..145c162 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 @@ -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,13 +26,13 @@ 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; - 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) { @@ -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() { diff --git a/src/main/java/frc/robot/subsystems/imu/ImuIO.java b/src/main/java/frc/robot/subsystems/imu/ImuIO.java index 8f71b2b..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 @@ -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/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 2fde327..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 @@ -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..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 @@ -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 { @@ -70,10 +70,10 @@ 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); + 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/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 0e0ec9f..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 @@ -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/VisionIOLimelight.java b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java index 0ed6ee6..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 @@ -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/VisionIOPhotonVision.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java index 5923813..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 @@ -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/subsystems/vision/VisionIOPhotonVisionSim.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java index 575b33d..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 @@ -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/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..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 @@ -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); } /** @@ -144,7 +148,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 deleted file mode 100644 index 42c2e3c..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.kZero); - } - - /** - * 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.kZero); - } - - /** - * 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.kZero, 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.kZero); - } - - /** - * 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.kZero, 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/LocalADStarAK.java b/src/main/java/frc/robot/util/LocalADStarAK.java index 0aff129..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 @@ -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/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/OverrideSwitches.java b/src/main/java/frc/robot/util/OverrideSwitches.java index 9fa2553..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 @@ -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/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 d574cd9..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 @@ -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..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 @@ -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/RBSIController.java b/src/main/java/frc/robot/util/RBSIController.java index 505067d..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 @@ -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/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 45c8aea..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 @@ -15,10 +15,12 @@ import frc.robot.Constants.PowerConstants; import frc.robot.Constants.RobotDevices; -import frc.robot.util.Alert.AlertType; +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; /** * Power monitoring virtual subsystem that periodically polls the Power Distribution Module. Each @@ -29,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; @@ -56,22 +60,27 @@ public class RBSIPowerMonitor extends VirtualSubsystem { }; private final Alert totalCurrentAlert = - new Alert("Total current draw exceeds limit!", AlertType.WARNING); - private final Alert[] portAlerts = new Alert[24]; // or pdh.getNumChannels() after construct - private final Alert lowVoltageAlert = new Alert("Low battery voltage!", AlertType.WARNING); + new Alert("totalCurrentHigh", "Total current draw exceeds limit!", Level.MEDIUM); + private final Alert[] portAlerts = new Alert[24]; + 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; // Constructor, including inputs of optional subsystems - public RBSIPowerMonitor(LoggedTunableNumber batteryCapacityAh, RBSISubsystem... subsystems) { + public RBSIPowerMonitor(DoubleSupplier batteryCapacityAh, RBSISubsystem... subsystems) { this.batteryCapacityAh = batteryCapacityAh; 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); } } @@ -108,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/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/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/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/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..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 @@ -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..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 @@ -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..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 @@ -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/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/RobotConstantsTest.java b/src/test/frc/robot/RobotConstantsTest.java index 4950a3d..afbaac1 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")); } @@ -109,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/RobotContainerTest.java b/src/test/frc/robot/RobotContainerTest.java index c400666..a3fa571 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 @@ -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/src/test/frc/robot/commands/DriveCommandsTests.java b/src/test/frc/robot/commands/DriveCommandsTests.java index c0db9fe..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 { @@ -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/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); - } -} 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/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 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/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 3a9e01e..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(500, 0)); - - 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 = 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 0000000..e7b61c2 Binary files /dev/null and b/themes/frc-biocore-high-contrast-theme-1.1.0.vsix differ 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/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" + ] + } + ] +} 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..e092b9a 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-9", + "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-9" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2027.0.0-alpha-4", + "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-4", + "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-4", + "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-4", + "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-4", + "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-4", + "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-4", + "version": "2027.0.0-alpha-9", "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" + } + ] +}