diff --git a/.github/workflows/build-pplib-release.yaml b/.github/workflows/build-pplib-release.yaml index d0e00dfd..7a3ca519 100644 --- a/.github/workflows/build-pplib-release.yaml +++ b/.github/workflows/build-pplib-release.yaml @@ -14,16 +14,16 @@ jobs: fail-fast: false matrix: include: - - container: wpilib/roborio-cross-ubuntu:2024-22.04 + - container: wpilib/roborio-cross-ubuntu:2025-22.04 artifact-name: PPLib-Athena build-options: "-Ponlylinuxathena" - container: wpilib/ubuntu-base:22.04 artifact-name: PPLib-Linux build-options: "-Ponlylinuxx86-64" - - container: wpilib/raspbian-cross-ubuntu:bullseye-22.04 + - container: wpilib/raspbian-cross-ubuntu:bookworm-22.04 artifact-name: PPLib-Arm32 build-options: "-Ponlylinuxarm32" - - container: wpilib/aarch64-cross-ubuntu:bullseye-22.04 + - container: wpilib/aarch64-cross-ubuntu:bookworm-22.04 artifact-name: PPLib-Arm64 build-options: "-Ponlylinuxarm64" name: "[PPLib] Build - ${{ matrix.artifact-name }}" @@ -56,6 +56,8 @@ jobs: path: pathplannerlib/build/allOutputs build-host: + env: + MACOSX_DEPLOYMENT_TARGET: 13.3 strategy: fail-fast: false matrix: diff --git a/.github/workflows/pplib-ci.yml b/.github/workflows/pplib-ci.yml index dd2a2457..4021f9f8 100644 --- a/.github/workflows/pplib-ci.yml +++ b/.github/workflows/pplib-ci.yml @@ -117,16 +117,16 @@ jobs: fail-fast: false matrix: include: - - container: wpilib/roborio-cross-ubuntu:2024-22.04 + - container: wpilib/roborio-cross-ubuntu:2025-22.04 artifact-name: PPLib-Athena build-options: "-Ponlylinuxathena" - container: wpilib/ubuntu-base:22.04 artifact-name: PPLib-Linux build-options: "-Ponlylinuxx86-64" - - container: wpilib/raspbian-cross-ubuntu:bullseye-22.04 + - container: wpilib/raspbian-cross-ubuntu:bookworm-22.04 artifact-name: PPLib-Arm32 build-options: "-Ponlylinuxarm32" - - container: wpilib/aarch64-cross-ubuntu:bullseye-22.04 + - container: wpilib/aarch64-cross-ubuntu:bookworm-22.04 artifact-name: PPLib-Arm64 build-options: "-Ponlylinuxarm64" name: "[PPLib] Build - ${{ matrix.artifact-name }}" diff --git a/pathplannerlib/build.gradle b/pathplannerlib/build.gradle index 2e6111e7..6809929c 100644 --- a/pathplannerlib/build.gradle +++ b/pathplannerlib/build.gradle @@ -2,16 +2,21 @@ plugins { id 'cpp' id 'java' id 'google-test' - id 'edu.wpi.first.wpilib.repositories.WPILibRepositoriesPlugin' version '2020.2' - id 'edu.wpi.first.NativeUtils' version '2025.3.0' + id 'edu.wpi.first.wpilib.repositories.WPILibRepositoriesPlugin' version '2025.0' + id 'edu.wpi.first.NativeUtils' version '2025.9.0' id 'edu.wpi.first.GradleJni' version '1.1.0' id 'edu.wpi.first.GradleVsCode' version '2.1.0' id 'com.diffplug.spotless' version '6.11.0' id 'jacoco' } -sourceCompatibility = JavaVersion.VERSION_17 -targetCompatibility = JavaVersion.VERSION_17 +// WPILib Version +ext.wpilibVersion = "2025.+" + +java { + sourceCompatibility = JavaVersion.VERSION_17 + targetCompatibility = JavaVersion.VERSION_17 +} repositories { mavenCentral() @@ -27,20 +32,20 @@ apply from: 'config.gradle' // Apply Java configuration dependencies { - implementation 'edu.wpi.first.cscore:cscore-java:2025.+' - implementation 'edu.wpi.first.cameraserver:cameraserver-java:2025.+' - implementation 'edu.wpi.first.ntcore:ntcore-java:2025.+' - implementation 'edu.wpi.first.wpilibj:wpilibj-java:2025.+' - implementation 'edu.wpi.first.wpiutil:wpiutil-java:2025.+' - implementation 'edu.wpi.first.wpimath:wpimath-java:2025.+' - implementation 'edu.wpi.first.wpilibNewCommands:wpilibNewCommands-java:2025.+' - implementation 'edu.wpi.first.wpiunits:wpiunits-java:2025.+' - implementation 'edu.wpi.first.hal:hal-java:2025.+' + implementation "edu.wpi.first.cscore:cscore-java:$wpilibVersion" + implementation "edu.wpi.first.cameraserver:cameraserver-java:$wpilibVersion" + implementation "edu.wpi.first.ntcore:ntcore-java:$wpilibVersion" + implementation "edu.wpi.first.wpilibj:wpilibj-java:$wpilibVersion" + implementation "edu.wpi.first.wpiutil:wpiutil-java:$wpilibVersion" + implementation "edu.wpi.first.wpimath:wpimath-java:$wpilibVersion" + implementation "edu.wpi.first.wpiunits:wpiunits-java:$wpilibVersion" + implementation "edu.wpi.first.hal:hal-java:$wpilibVersion" + implementation "edu.wpi.first.wpilibNewCommands:wpilibNewCommands-java:$wpilibVersion" implementation "org.ejml:ejml-simple:0.43.1" implementation "com.fasterxml.jackson.core:jackson-annotations:2.15.2" implementation "com.fasterxml.jackson.core:jackson-core:2.15.2" implementation "com.fasterxml.jackson.core:jackson-databind:2.15.2" - implementation 'edu.wpi.first.thirdparty.frc2024.opencv:opencv-java:4.8.0-4' + implementation 'edu.wpi.first.thirdparty.frc2025.opencv:opencv-java:4.10.0-2' testImplementation 'org.junit.jupiter:junit-jupiter-api:5.9.0' testImplementation 'org.junit.jupiter:junit-jupiter-params:5.9.0' @@ -163,7 +168,7 @@ javadoc { apply from: 'publish.gradle' wrapper { - gradleVersion '8.5' + gradleVersion '8.11' } jacocoTestReport { diff --git a/pathplannerlib/config.gradle b/pathplannerlib/config.gradle index 025cf1f3..9dd94815 100644 --- a/pathplannerlib/config.gradle +++ b/pathplannerlib/config.gradle @@ -8,10 +8,10 @@ nativeUtils.withCrossLinuxArm64() nativeUtils { wpi { configureDependencies { - wpiVersion = "2025.+" - opencvYear = "frc2024" + wpiVersion = wpilibVersion + opencvYear = "frc2025" niLibVersion = "2025.0.0" - opencvVersion = "4.8.0-4" + opencvVersion = "4.10.0-2" } } } diff --git a/pathplannerlib/gradle/wrapper/gradle-wrapper.properties b/pathplannerlib/gradle/wrapper/gradle-wrapper.properties index df97d72b..94113f20 100644 --- a/pathplannerlib/gradle/wrapper/gradle-wrapper.properties +++ b/pathplannerlib/gradle/wrapper/gradle-wrapper.properties @@ -1,6 +1,6 @@ distributionBase=GRADLE_USER_HOME distributionPath=wrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-8.10.2-bin.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-8.11-bin.zip networkTimeout=10000 validateDistributionUrl=true zipStoreBase=GRADLE_USER_HOME diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PPHolonomicDriveController.java b/pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PPHolonomicDriveController.java index f797098f..a44b42dc 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PPHolonomicDriveController.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PPHolonomicDriveController.java @@ -5,6 +5,7 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import java.util.Optional; import java.util.function.DoubleSupplier; @@ -16,6 +17,7 @@ public class PPHolonomicDriveController implements PathFollowingController { private final PIDController yController; private final PIDController rotationController; + private Translation2d translationError = new Translation2d(); private boolean isEnabled = true; private static Supplier> rotationTargetOverride = null; @@ -96,10 +98,10 @@ public ChassisSpeeds calculateRobotRelativeSpeeds( double xFF = targetState.fieldSpeeds.vxMetersPerSecond; double yFF = targetState.fieldSpeeds.vyMetersPerSecond; + this.translationError = currentPose.getTranslation().minus(targetState.pose.getTranslation()); + if (!this.isEnabled) { - ChassisSpeeds ret = new ChassisSpeeds(xFF, yFF, 0); - ret.toRobotRelativeSpeeds(currentPose.getRotation()); - return ret; + return ChassisSpeeds.fromFieldRelativeSpeeds(xFF, yFF, 0, currentPose.getRotation()); } double xFeedback = this.xController.calculate(currentPose.getX(), targetState.pose.getX()); @@ -125,10 +127,8 @@ public ChassisSpeeds calculateRobotRelativeSpeeds( rotationFeedback = rotFeedbackOverride.getAsDouble(); } - ChassisSpeeds ret = - new ChassisSpeeds(xFF + xFeedback, yFF + yFeedback, rotationFF + rotationFeedback); - ret.toRobotRelativeSpeeds(currentPose.getRotation()); - return ret; + return ChassisSpeeds.fromFieldRelativeSpeeds( + xFF + xFeedback, yFF + yFeedback, rotationFF + rotationFeedback, currentPose.getRotation()); } /** diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java index 4bfbb13e..0ee5dd0f 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java @@ -650,8 +650,9 @@ public Optional getIdealTrajectory(RobotConfig robotConfi // The ideal starting state is known, generate the ideal trajectory Rotation2d heading = getInitialHeading(); Translation2d fieldSpeeds = new Translation2d(idealStartingState.velocityMPS(), heading); - ChassisSpeeds startingSpeeds = new ChassisSpeeds(fieldSpeeds.getX(), fieldSpeeds.getY(), 0.0); - startingSpeeds.toRobotRelativeSpeeds(idealStartingState.rotation()); + ChassisSpeeds startingSpeeds = + ChassisSpeeds.fromFieldRelativeSpeeds( + fieldSpeeds.getX(), fieldSpeeds.getY(), 0.0, idealStartingState.rotation()); idealTrajectory = Optional.of( generateTrajectory(startingSpeeds, idealStartingState.rotation(), robotConfig)); diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectory.java b/pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectory.java index 1d56a2c0..8f8565e3 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectory.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectory.java @@ -70,7 +70,7 @@ public PathPlannerTrajectory( // Set the initial module velocities ChassisSpeeds fieldStartingSpeeds = - fromRobotRelativeSpeeds(startingSpeeds, states.get(0).pose.getRotation()); + ChassisSpeeds.fromRobotRelativeSpeeds(startingSpeeds, states.get(0).pose.getRotation()); var initialStates = config.toSwerveModuleStates(fieldStartingSpeeds); for (int m = 0; m < config.numModules; m++) { states.get(0).moduleStates[m].speedMetersPerSecond = initialStates[m].speedMetersPerSecond; @@ -91,7 +91,7 @@ public PathPlannerTrajectory( new ChassisSpeeds(endSpeedTrans.getX(), endSpeedTrans.getY(), 0.0); var endStates = config.toSwerveModuleStates( - fromFieldRelativeSpeeds( + ChassisSpeeds.fromFieldRelativeSpeeds( endFieldSpeeds, states.get(states.size() - 1).pose.getRotation())); for (int m = 0; m < config.numModules; m++) { states.get(states.size() - 1).moduleStates[m].speedMetersPerSecond = @@ -136,9 +136,10 @@ public PathPlannerTrajectory( state.timeSeconds = prevState.timeSeconds + dt; ChassisSpeeds prevRobotSpeeds = - fromFieldRelativeSpeeds(prevState.fieldSpeeds, prevState.pose.getRotation()); + ChassisSpeeds.fromFieldRelativeSpeeds( + prevState.fieldSpeeds, prevState.pose.getRotation()); ChassisSpeeds robotSpeeds = - fromFieldRelativeSpeeds(state.fieldSpeeds, state.pose.getRotation()); + ChassisSpeeds.fromFieldRelativeSpeeds(state.fieldSpeeds, state.pose.getRotation()); double chassisAccelX = (robotSpeeds.vxMetersPerSecond - prevRobotSpeeds.vxMetersPerSecond) / dt; double chassisAccelY = @@ -367,8 +368,8 @@ private static void forwardAccelPass( } ChassisSpeeds chassisAccel = - new ChassisSpeeds(accelVec.getX(), accelVec.getY(), angularAccel); - chassisAccel.toRobotRelativeSpeeds(state.pose.getRotation()); + ChassisSpeeds.fromFieldRelativeSpeeds( + accelVec.getX(), accelVec.getY(), angularAccel, state.pose.getRotation()); var accelStates = config.toSwerveModuleStates(chassisAccel); for (int m = 0; m < config.numModules; m++) { double moduleAcceleration = accelStates[m].speedMetersPerSecond; @@ -445,8 +446,9 @@ private static void forwardAccelPass( maxChassisVel, maxChassisAngVel); - state.fieldSpeeds = config.toChassisSpeeds(state.moduleStates); - state.fieldSpeeds.toFieldRelativeSpeeds(state.pose.getRotation()); + state.fieldSpeeds = + ChassisSpeeds.fromRobotRelativeSpeeds( + config.toChassisSpeeds(state.moduleStates), state.pose.getRotation()); state.linearVelocity = Math.hypot(state.fieldSpeeds.vxMetersPerSecond, state.fieldSpeeds.vyMetersPerSecond); } @@ -508,8 +510,9 @@ private static void reverseAccelPass( } ChassisSpeeds chassisAccel = - new ChassisSpeeds(accelVec.getX(), accelVec.getY(), angularAccel); - chassisAccel.toRobotRelativeSpeeds(state.pose.getRotation()); + ChassisSpeeds.fromFieldRelativeSpeeds( + new ChassisSpeeds(accelVec.getX(), accelVec.getY(), angularAccel), + state.pose.getRotation()); var accelStates = config.toSwerveModuleStates(chassisAccel); for (int m = 0; m < config.numModules; m++) { double moduleAcceleration = accelStates[m].speedMetersPerSecond; @@ -576,8 +579,9 @@ private static void reverseAccelPass( maxChassisVel, maxChassisAngVel); - state.fieldSpeeds = config.toChassisSpeeds(state.moduleStates); - state.fieldSpeeds.toFieldRelativeSpeeds(state.pose.getRotation()); + state.fieldSpeeds = + ChassisSpeeds.fromRobotRelativeSpeeds( + config.toChassisSpeeds(state.moduleStates), state.pose.getRotation()); state.linearVelocity = Math.hypot(state.fieldSpeeds.vxMetersPerSecond, state.fieldSpeeds.vyMetersPerSecond); } @@ -767,24 +771,4 @@ private static Rotation2d cosineInterpolate(Rotation2d start, Rotation2d end, do double t2 = (1.0 - Math.cos(t * Math.PI)) / 2.0; return start.interpolate(end, t2); } - - // TODO: Remove if ChassisSpeeds factory methods get un-deprecated - private static ChassisSpeeds fromRobotRelativeSpeeds( - ChassisSpeeds speeds, Rotation2d robotRotation) { - ChassisSpeeds ret = - new ChassisSpeeds( - speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, speeds.omegaRadiansPerSecond); - ret.toFieldRelativeSpeeds(robotRotation); - return ret; - } - - // TODO: Remove if ChassisSpeeds factory methods get un-deprecated - private static ChassisSpeeds fromFieldRelativeSpeeds( - ChassisSpeeds speeds, Rotation2d robotRotation) { - ChassisSpeeds ret = - new ChassisSpeeds( - speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, speeds.omegaRadiansPerSecond); - ret.toRobotRelativeSpeeds(robotRotation); - return ret; - } } diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/util/swerve/SwerveSetpointGenerator.java b/pathplannerlib/src/main/java/com/pathplanner/lib/util/swerve/SwerveSetpointGenerator.java index 60a3378d..bd9e6ca8 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/util/swerve/SwerveSetpointGenerator.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/util/swerve/SwerveSetpointGenerator.java @@ -317,7 +317,7 @@ public SwerveSetpoint generateSetpoint( prevSetpoint.robotRelativeSpeeds().vxMetersPerSecond + min_s * dx, prevSetpoint.robotRelativeSpeeds().vyMetersPerSecond + min_s * dy, prevSetpoint.robotRelativeSpeeds().omegaRadiansPerSecond + min_s * dtheta); - retSpeeds.discretize(dt); + retSpeeds = ChassisSpeeds.discretize(retSpeeds, dt); double prevVelX = prevSetpoint.robotRelativeSpeeds().vxMetersPerSecond; double prevVelY = prevSetpoint.robotRelativeSpeeds().vyMetersPerSecond;