From 02de6c4982c7fc920fd8ccb92953bdd02bb22784 Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Thu, 19 Dec 2024 14:15:47 -0600 Subject: [PATCH] Updated to wpilib beta-3. Signed-off-by: thenetworkgrinch --- build.gradle | 2 +- .../swervedrive/SwerveSubsystem.java | 10 ++-- src/main/java/swervelib/SwerveDrive.java | 56 ++++++++++--------- src/main/java/swervelib/SwerveModule.java | 5 +- 4 files changed, 38 insertions(+), 35 deletions(-) diff --git a/build.gradle b/build.gradle index 7d42139a..fc5c8f73 100644 --- a/build.gradle +++ b/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 { diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 8356fce9..89a4d78f 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/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 f try { return driveWithSetpointGenerator(() -> { - ChassisSpeeds speeds = fieldRelativeSpeeds.get(); - speeds.toRobotRelativeSpeeds(getHeading()); - return speeds; + return ChassisSpeeds.fromFieldRelativeSpeeds(fieldRelativeSpeeds.get(), getHeading()); }); } catch (Exception e) diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index ad094205..0af6a165 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/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; diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index 0d79f13a..2431d130 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/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))); } /**