Updated to wpilib beta-3.

Signed-off-by: thenetworkgrinch <thenetworkgrinch@users.noreply.github.com>

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-2"
+    id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-3"
 }
 
 java {

src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java

@@ -276,10 +276,10 @@ public Command aimAtSpeaker(double tolerance)
     SwerveController controller = swerveDrive.getSwerveController();
     return run(
         () -> {
-          ChassisSpeeds speeds = new ChassisSpeeds(0, 0,
+          ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds(0, 0,
                                                    controller.headingCalculate(getHeading().getRadians(),
-                                                                               getSpeakerYaw().getRadians()));
-          speeds.toRobotRelativeSpeeds(getHeading());
+                                                                               getSpeakerYaw().getRadians()),
+                                                                       getHeading());
           drive(speeds);
         }).until(() -> Math.abs(getSpeakerYaw().minus(getHeading()).getDegrees()) < tolerance);
   }
@@ -386,9 +386,7 @@ public Command driveWithSetpointGeneratorFieldRelative(Supplier<ChassisSpeeds> f
     try
     {
       return driveWithSetpointGenerator(() -> {
-        ChassisSpeeds speeds = fieldRelativeSpeeds.get();
-        speeds.toRobotRelativeSpeeds(getHeading());
-        return speeds;
+        return ChassisSpeeds.fromFieldRelativeSpeeds(fieldRelativeSpeeds.get(), getHeading());
 
       });
     } catch (Exception e)

src/main/java/swervelib/SwerveDrive.java

@@ -452,39 +452,38 @@ public void setHeadingCorrection(boolean state, double deadband)
 
   /**
    * Tertiary method of controlling the drive base given velocity in both field oriented and robot oriented at the same
-   * time. The inputs are added together so this is not intneded to be used to give the driver both methods of control.
+   * time. The inputs are added together so this is not intended to be used to give the driver both methods of control.
    *
    * @param fieldOrientedVelocity The field oriented velocties to use
    * @param robotOrientedVelocity The robot oriented velocties to use
    */
-  public void driveFieldOrientedandRobotOriented(ChassisSpeeds fieldOrientedVelocity,
+  public void driveFieldOrientedAndRobotOriented(ChassisSpeeds fieldOrientedVelocity,
                                                  ChassisSpeeds robotOrientedVelocity)
   {
-    fieldOrientedVelocity.toRobotRelativeSpeeds(getOdometryHeading());
-    drive(fieldOrientedVelocity.plus(robotOrientedVelocity));
+
+    drive(ChassisSpeeds.fromFieldRelativeSpeeds(fieldOrientedVelocity, getOdometryHeading())
+                       .plus(robotOrientedVelocity));
   }
 
   /**
    * Secondary method of controlling the drive base given velocity and adjusting it for field oriented use.
    *
-   * @param velocity Velocity of the robot desired.
+   * @param fieldRelativeSpeeds Velocity of the robot desired.
    */
-  public void driveFieldOriented(ChassisSpeeds velocity)
+  public void driveFieldOriented(ChassisSpeeds fieldRelativeSpeeds)
   {
-    velocity.toRobotRelativeSpeeds(getOdometryHeading());
-    drive(velocity);
+    drive(ChassisSpeeds.fromFieldRelativeSpeeds(fieldRelativeSpeeds, getOdometryHeading()));
   }
 
   /**
    * Secondary method of controlling the drive base given velocity and adjusting it for field oriented use.
    *
-   * @param velocity               Velocity of the robot desired.
+   * @param fieldRelativeSpeeds    Velocity of the robot desired.
    * @param centerOfRotationMeters The center of rotation in meters, 0 is the center of the robot.
    */
-  public void driveFieldOriented(ChassisSpeeds velocity, Translation2d centerOfRotationMeters)
+  public void driveFieldOriented(ChassisSpeeds fieldRelativeSpeeds, Translation2d centerOfRotationMeters)
   {
-    velocity.toRobotRelativeSpeeds(getOdometryHeading());
-    drive(velocity, centerOfRotationMeters);
+    drive(ChassisSpeeds.fromFieldRelativeSpeeds(fieldRelativeSpeeds, getOdometryHeading()), centerOfRotationMeters);
   }
 
   /**
@@ -535,7 +534,7 @@ public void drive(
     ChassisSpeeds velocity = new ChassisSpeeds(translation.getX(), translation.getY(), rotation);
     if (fieldRelative)
     {
-      velocity.toRobotRelativeSpeeds(getOdometryHeading());
+      velocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getOdometryHeading());
     }
     drive(velocity, isOpenLoop, centerOfRotationMeters);
   }
@@ -564,7 +563,7 @@ public void drive(
 
     if (fieldRelative)
     {
-      velocity.toRobotRelativeSpeeds(getOdometryHeading());
+      ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getOdometryHeading());
     }
     drive(velocity, isOpenLoop, new Translation2d());
   }
@@ -844,8 +843,10 @@ public ChassisSpeeds getFieldVelocity()
     // but not the reverse.  However, because this transform is a simple rotation, negating the
     // angle given as the robot angle reverses the direction of rotation, and the conversion is reversed.
     ChassisSpeeds robotRelativeSpeeds = kinematics.toChassisSpeeds(getStates());
-    robotRelativeSpeeds.toFieldRelativeSpeeds(getOdometryHeading());//.unaryMinus());
-    return robotRelativeSpeeds;
+    return ChassisSpeeds.fromRobotRelativeSpeeds(robotRelativeSpeeds, getOdometryHeading());
+    // Might need to be this instead
+    //return ChassisSpeeds.fromFieldRelativeSpeeds(
+    //        kinematics.toChassisSpeeds(getStates()), getOdometryHeading().unaryMinus());
   }
 
   /**
@@ -874,8 +875,7 @@ public void resetOdometry(Pose2d pose)
       mapleSimDrive.setSimulationWorldPose(pose);
     }
     odometryLock.unlock();
-    ChassisSpeeds robotRelativeSpeeds = new ChassisSpeeds();
-    robotRelativeSpeeds.toFieldRelativeSpeeds(getYaw());
+    ChassisSpeeds robotRelativeSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds(new ChassisSpeeds(0, 0, 0), getYaw());
     kinematics.toSwerveModuleStates(robotRelativeSpeeds);
 
   }
@@ -1408,8 +1408,8 @@ public void setCosineCompensator(boolean enabled)
   /**
    * Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction in teleop
    *
-   * @param enable    Enable chassis velocity correction, which will use {@link ChassisSpeeds#discretize(double)} with
-   *                  the following.
+   * @param enable    Enable chassis velocity correction, which will use
+   *                  {@link ChassisSpeeds#discretize(ChassisSpeeds, double)}} with the following.
    * @param dtSeconds The duration of the timestep the speeds should be applied for.
    */
   public void setChassisDiscretization(boolean enable, double dtSeconds)
@@ -1425,10 +1425,10 @@ public void setChassisDiscretization(boolean enable, double dtSeconds)
    * Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction in teleop
    * and/or auto
    *
-   * @param useInTeleop Enable chassis velocity correction, which will use {@link ChassisSpeeds#discretize(double)} with
-   *                    the following in teleop.
-   * @param useInAuto   Enable chassis velocity correction, which will use {@link ChassisSpeeds#discretize(double)} with
-   *                    the following in auto.
+   * @param useInTeleop Enable chassis velocity correction, which will use
+   *                    {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} with the following in teleop.
+   * @param useInAuto   Enable chassis velocity correction, which will use
+   *                    {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} with the following in auto.
    * @param dtSeconds   The duration of the timestep the speeds should be applied for.
    */
   public void setChassisDiscretization(boolean useInTeleop, boolean useInAuto, double dtSeconds)
@@ -1476,8 +1476,10 @@ public ChassisSpeeds angularVelocitySkewCorrection(ChassisSpeeds robotRelativeVe
     var angularVelocity = new Rotation2d(imu.getYawAngularVelocity().in(RadiansPerSecond) * angularVelocityCoefficient);
     if (angularVelocity.getRadians() != 0.0)
     {
-      robotRelativeVelocity.toFieldRelativeSpeeds(getOdometryHeading());
-      robotRelativeVelocity.toRobotRelativeSpeeds(getOdometryHeading().plus(angularVelocity));
+      ChassisSpeeds fieldRelativeVelocity = ChassisSpeeds.fromRobotRelativeSpeeds(robotRelativeVelocity,
+                                                                                  getOdometryHeading());
+      robotRelativeVelocity = ChassisSpeeds.fromFieldRelativeSpeeds(fieldRelativeVelocity,
+                                                                    getOdometryHeading().plus(angularVelocity));
     }
     return robotRelativeVelocity;
   }
@@ -1503,7 +1505,7 @@ private ChassisSpeeds movementOptimizations(ChassisSpeeds robotRelativeVelocity,
     // https://www.chiefdelphi.com/t/whitepaper-swerve-drive-skew-and-second-order-kinematics/416964/5
     if (uesChassisDiscretize)
     {
-      robotRelativeVelocity.discretize(discretizationdtSeconds);
+      robotRelativeVelocity = ChassisSpeeds.discretize(robotRelativeVelocity, discretizationdtSeconds);
     }
 
     return robotRelativeVelocity;

src/main/java/swervelib/SwerveModule.java

@@ -423,7 +423,10 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop,
     LinearVelocity curVelocity = MetersPerSecond.of(lastState.speedMetersPerSecond);
     desiredState.speedMetersPerSecond = nextVelocity.magnitude();
 
-    setDesiredState(desiredState, isOpenLoop, driveMotorFeedforward.calculate(nextVelocity).magnitude());
+    setDesiredState(desiredState,
+                    isOpenLoop,
+                    driveMotorFeedforward.calculateWithVelocities(curVelocity.in(MetersPerSecond),
+                                                                  nextVelocity.in(MetersPerSecond)));
   }
 
   /**