From dc8f5282746b56ff5865d3bcc7a89fcccf0f224f Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Fri, 20 Sep 2024 22:33:36 -0400 Subject: [PATCH] first draft mutRotation2d and mutable SwerveModuleState --- .../command/SwerveControllerCommandTest.java | 4 +- .../examples/swervebot/SwerveModule.java | 36 ++- .../subsystems/SwerveModule.java | 26 +- .../SwerveModule.java | 28 ++- .../first/math/geometry/MutRotation2d.java | 229 ++++++++++++++++++ .../wpi/first/math/geometry/Rotation2d.java | 6 +- .../kinematics/SwerveDriveKinematics.java | 18 +- .../math/kinematics/SwerveModuleState.java | 90 +++++-- .../proto/SwerveModuleStateProto.java | 4 +- .../struct/SwerveModuleStateStruct.java | 4 +- .../SwerveDrivePoseEstimatorTest.java | 4 +- .../kinematics/SwerveDriveKinematicsTest.java | 132 +++++----- .../kinematics/SwerveDriveOdometryTest.java | 21 +- .../kinematics/SwerveModuleStateTest.java | 28 ++- .../proto/SwerveModuleStateProtoTest.java | 4 +- .../struct/SwerveModuleStateStructTest.java | 4 +- 16 files changed, 479 insertions(+), 159 deletions(-) create mode 100644 wpimath/src/main/java/edu/wpi/first/math/geometry/MutRotation2d.java diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java index 0abe4990486..1e07d55b900 100644 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java @@ -121,8 +121,8 @@ void testReachesReference() { m_angle = trajectory.sample(m_timer.get()).poseMeters.getRotation(); for (int i = 0; i < m_modulePositions.length; i++) { - m_modulePositions[i].distanceMeters += m_moduleStates[i].speedMetersPerSecond * 0.005; - m_modulePositions[i].angle = m_moduleStates[i].angle; + m_modulePositions[i].distanceMeters += m_moduleStates[i].getSpeedMetersPerSecond() * 0.005; + m_modulePositions[i].angle = m_moduleStates[i].getAngle(); } SimHooks.stepTiming(0.005); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java index 32ca8fdbbc6..d46de518894 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java @@ -11,6 +11,7 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.geometry.MutRotation2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; @@ -32,6 +33,10 @@ public class SwerveModule { private final Encoder m_driveEncoder; private final Encoder m_turningEncoder; + private final MutRotation2d m_moduleAngle = new MutRotation2d(); + private final SwerveModuleState m_moduleState = new SwerveModuleState(); + private final MutRotation2d m_encoderRotation = new MutRotation2d(); + // Gains are for example purposes only - must be determined for your own robot! private final PIDController m_drivePIDController = new PIDController(1, 0, 0); @@ -76,7 +81,8 @@ public SwerveModule( // resolution. m_driveEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution); - // Set the distance (in this case, angle) in radians per pulse for the turning encoder. + // Set the distance (in this case, angle) in radians per pulse for the turning + // encoder. // This is the the angle through an entire rotation (2 * pi) divided by the // encoder resolution. m_turningEncoder.setDistancePerPulse(2 * Math.PI / kEncoderResolution); @@ -92,8 +98,9 @@ public SwerveModule( * @return The current state of the module. */ public SwerveModuleState getState() { - return new SwerveModuleState( - m_driveEncoder.getRate(), new Rotation2d(m_turningEncoder.getDistance())); + m_moduleAngle.mut_fromRadians(m_turningEncoder.getDistance()); + m_moduleState.setState(m_driveEncoder.getRate(), m_moduleAngle); + return m_moduleState; } /** @@ -109,30 +116,33 @@ public SwerveModulePosition getPosition() { /** * Sets the desired state for the module. * - * @param desiredState Desired state with speed and angle. + * @param state Desired state with speed and angle. */ - public void setDesiredState(SwerveModuleState desiredState) { - var encoderRotation = new Rotation2d(m_turningEncoder.getDistance()); + public void setDesiredState(SwerveModuleState state) { + m_encoderRotation.mut_fromRadians(m_turningEncoder.getDistance()); // Optimize the reference state to avoid spinning further than 90 degrees - SwerveModuleState state = SwerveModuleState.optimize(desiredState, encoderRotation); + state.optimize(m_encoderRotation); - // Scale speed by cosine of angle error. This scales down movement perpendicular to the desired - // direction of travel that can occur when modules change directions. This results in smoother + // Scale speed by cosine of angle error. This scales down movement perpendicular + // to the desired + // direction of travel that can occur when modules change directions. This + // results in smoother // driving. - state.speedMetersPerSecond *= state.angle.minus(encoderRotation).getCos(); + state.cosineScale(m_encoderRotation); // Calculate the drive output from the drive PID controller. final double driveOutput = - m_drivePIDController.calculate(m_driveEncoder.getRate(), state.speedMetersPerSecond); + m_drivePIDController.calculate(m_driveEncoder.getRate(), state.getSpeedMetersPerSecond()); final double driveFeedforward = - m_driveFeedforward.calculate(MetersPerSecond.of(state.speedMetersPerSecond)).in(Volts); + m_driveFeedforward.calculate(MetersPerSecond.of(state.getSpeedMetersPerSecond())).in(Volts); // Calculate the turning motor output from the turning PID controller. final double turnOutput = - m_turningPIDController.calculate(m_turningEncoder.getDistance(), state.angle.getRadians()); + m_turningPIDController.calculate( + m_turningEncoder.getDistance(), state.getAngle().getRadians()); final double turnFeedforward = m_turnFeedforward diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java index 27e800c649e..0195c0649a0 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.MutRotation2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; @@ -21,6 +22,10 @@ public class SwerveModule { private final Encoder m_driveEncoder; private final Encoder m_turningEncoder; + private final MutRotation2d m_moduleAngle = new MutRotation2d(); + private final SwerveModuleState m_moduleState = new SwerveModuleState(); + private final MutRotation2d m_encoderRotation = new MutRotation2d(); + private final PIDController m_drivePIDController = new PIDController(ModuleConstants.kPModuleDriveController, 0, 0); @@ -85,8 +90,9 @@ public SwerveModule( * @return The current state of the module. */ public SwerveModuleState getState() { - return new SwerveModuleState( - m_driveEncoder.getRate(), new Rotation2d(m_turningEncoder.getDistance())); + m_moduleAngle.mut_fromRadians(m_turningEncoder.getDistance()); + m_moduleState.setState(m_driveEncoder.getRate(), m_moduleAngle); + return m_moduleState; } /** @@ -102,26 +108,28 @@ public SwerveModulePosition getPosition() { /** * Sets the desired state for the module. * - * @param desiredState Desired state with speed and angle. + * @param state Desired state with speed and angle. */ - public void setDesiredState(SwerveModuleState desiredState) { - var encoderRotation = new Rotation2d(m_turningEncoder.getDistance()); + public void setDesiredState(SwerveModuleState state) { + m_encoderRotation.mut_fromRadians(m_turningEncoder.getDistance()); // Optimize the reference state to avoid spinning further than 90 degrees - SwerveModuleState state = SwerveModuleState.optimize(desiredState, encoderRotation); + state.optimize(m_encoderRotation); // Scale speed by cosine of angle error. This scales down movement perpendicular to the desired // direction of travel that can occur when modules change directions. This results in smoother // driving. - state.speedMetersPerSecond *= state.angle.minus(encoderRotation).getCos(); + state.setSpeed( + state.getSpeedMetersPerSecond() * state.getAngle().minus(m_encoderRotation).getCos()); // Calculate the drive output from the drive PID controller. final double driveOutput = - m_drivePIDController.calculate(m_driveEncoder.getRate(), state.speedMetersPerSecond); + m_drivePIDController.calculate(m_driveEncoder.getRate(), state.getSpeedMetersPerSecond()); // Calculate the turning motor output from the turning PID controller. final double turnOutput = - m_turningPIDController.calculate(m_turningEncoder.getDistance(), state.angle.getRadians()); + m_turningPIDController.calculate( + m_turningEncoder.getDistance(), state.getAngle().getRadians()); // Calculate the turning motor output from the turning PID controller. m_driveMotor.set(driveOutput); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java index f42e1f43996..e30dc02731a 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java @@ -11,6 +11,7 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.geometry.MutRotation2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; @@ -32,6 +33,10 @@ public class SwerveModule { private final Encoder m_driveEncoder; private final Encoder m_turningEncoder; + private final MutRotation2d m_moduleAngle = new MutRotation2d(); + private final SwerveModuleState m_moduleState = new SwerveModuleState(); + private final MutRotation2d m_encoderRotation = new MutRotation2d(); + // Gains are for example purposes only - must be determined for your own robot! private final PIDController m_drivePIDController = new PIDController(1, 0, 0); @@ -92,8 +97,9 @@ public SwerveModule( * @return The current state of the module. */ public SwerveModuleState getState() { - return new SwerveModuleState( - m_driveEncoder.getRate(), new Rotation2d(m_turningEncoder.getDistance())); + m_moduleAngle.mut_fromRadians(m_turningEncoder.getDistance()); + m_moduleState.setState(m_driveEncoder.getRate(), m_moduleAngle); + return m_moduleState; } /** @@ -109,29 +115,31 @@ public SwerveModulePosition getPosition() { /** * Sets the desired state for the module. * - * @param desiredState Desired state with speed and angle. + * @param state Desired state with speed and angle. */ - public void setDesiredState(SwerveModuleState desiredState) { - var encoderRotation = new Rotation2d(m_turningEncoder.getDistance()); + public void setDesiredState(SwerveModuleState state) { + m_encoderRotation.mut_fromRadians(m_turningEncoder.getDistance()); // Optimize the reference state to avoid spinning further than 90 degrees - SwerveModuleState state = SwerveModuleState.optimize(desiredState, encoderRotation); + state.optimize(m_encoderRotation); // Scale speed by cosine of angle error. This scales down movement perpendicular to the desired // direction of travel that can occur when modules change directions. This results in smoother // driving. - state.speedMetersPerSecond *= state.angle.minus(encoderRotation).getCos(); + state.setSpeed( + state.getSpeedMetersPerSecond() * state.getAngle().minus(m_encoderRotation).getCos()); // Calculate the drive output from the drive PID controller. final double driveOutput = - m_drivePIDController.calculate(m_driveEncoder.getRate(), state.speedMetersPerSecond); + m_drivePIDController.calculate(m_driveEncoder.getRate(), state.getSpeedMetersPerSecond()); final double driveFeedforward = - m_driveFeedforward.calculate(MetersPerSecond.of(state.speedMetersPerSecond)).in(Volts); + m_driveFeedforward.calculate(MetersPerSecond.of(state.getSpeedMetersPerSecond())).in(Volts); // Calculate the turning motor output from the turning PID controller. final double turnOutput = - m_turningPIDController.calculate(m_turningEncoder.getDistance(), state.angle.getRadians()); + m_turningPIDController.calculate( + m_turningEncoder.getDistance(), state.getAngle().getRadians()); final double turnFeedforward = m_turnFeedforward diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/MutRotation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/MutRotation2d.java new file mode 100644 index 00000000000..449c562fbcc --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/MutRotation2d.java @@ -0,0 +1,229 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.geometry; + +import com.fasterxml.jackson.annotation.JsonAutoDetect; +import com.fasterxml.jackson.annotation.JsonCreator; +import com.fasterxml.jackson.annotation.JsonIgnoreProperties; +import com.fasterxml.jackson.annotation.JsonProperty; +import edu.wpi.first.math.MathSharedStore; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; + +/** + * A rotation in a 2D coordinate frame represented by a point on the unit circle (cosine and sine). + * + *

The angle is continuous, that is if a Rotation2d is constructed with 361 degrees, it will + * return 361 degrees. This allows algorithms that wouldn't want to see a discontinuity in the + * rotations as it sweeps past from 360 to 0 on the second time around. + */ +@JsonIgnoreProperties(ignoreUnknown = true) +@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) +public class MutRotation2d extends Rotation2d { + /** Constructs a MutRotation2d with a default angle of 0 degrees. */ + public MutRotation2d() { + super(); + } + + /** + * Constructs a MutRotation2d with the given radian value. + * + * @param value The value of the angle in radians. + */ + @JsonCreator + public MutRotation2d(@JsonProperty(required = true, value = "radians") double value) { + super(value); + } + + /** + * Constructs a MutRotation2d with the given x and y (cosine and sine) components. + * + * @param x The x component or cosine of the rotation. + * @param y The y component or sine of the rotation. + */ + public MutRotation2d(double x, double y) { + super(x, y); + } + + /** + * Constructs a MutRotation2d with the given angle. + * + * @param angle The angle of the rotation. + */ + public MutRotation2d(Angle angle) { + super(angle); + } + + /** + * sets the value of this MutRotation2d to the given radian value. + * + * @param radians The new value of this MutRotation2d in radians. + */ + public void mut_fromRadians(double radians) { + m_value = radians; + m_cos = Math.cos(radians); + m_sin = Math.sin(radians); + } + + /** + * sets the value of this MutRotation2d to the given degrees value. + * + * @param degrees The new value of this MutRotation2d in degrees. + */ + public void mut_fromDegrees(double degrees) { + mut_fromRadians(Math.toRadians(degrees)); + } + + /** + * sets the value of this MutRotation2d to the given rotations value. + * + * @param rotations The new value of this MutRotation2d in rotations. + */ + public void mut_fromRotations(double rotations) { + mut_fromRadians(Units.rotationsToRadians(rotations)); + } + + /** + * sets the value of this MutRotation2d to the given Rotation2d value. + * + * @param other The new value of this MutRotation2d. + */ + public void mut_Rotation2d(Rotation2d other) { + mut_fromRadians(other.getRadians()); + } + + /** + * Adds the new rotation to this one using a rotation matrix. + * + *

The matrix multiplication is as follows: + * + * 

+   * [cos_new]   [other.cos, -other.sin][cos]
+   * [sin_new] = [other.sin,  other.cos][sin]
+   * value_new = atan2(sin_new, cos_new)
+   *
+ * + * @param other The rotation to rotate by. + */ + public void mut_rotateBy(Rotation2d other) { + double x = m_cos * other.m_cos - m_sin * other.m_sin; + double y = m_cos * other.m_sin + m_sin * other.m_cos; + double magnitude = Math.hypot(x, y); + if (magnitude > 1e-6) { + m_sin = y / magnitude; + m_cos = x / magnitude; + } else { + m_sin = 0.0; + m_cos = 1.0; + MathSharedStore.reportError( + "x and y components of MutRotation2d are zero\n", Thread.currentThread().getStackTrace()); + } + m_value = Math.atan2(m_sin, m_cos); + } + + /** + * Adds the other rotation to this one, with the result being bounded between -pi and pi. + * + * @param other The rotation to add. + */ + public void mut_plus(Rotation2d other) { + mut_rotateBy(other); + } + + /** + * Subtracts the other rotation from this one. + * + * @param other The rotation to subtract. + */ + public void mut_minus(MutRotation2d other) { + double x = m_cos * other.m_cos + m_sin * other.m_sin; + double y = -m_cos * other.m_sin + m_sin * other.m_cos; + double magnitude = Math.hypot(x, y); + if (magnitude > 1e-6) { + m_sin = y / magnitude; + m_cos = x / magnitude; + } else { + m_sin = 0.0; + m_cos = 1.0; + MathSharedStore.reportError( + "x and y components of MutRotation2d are zero\n", Thread.currentThread().getStackTrace()); + } + m_value = Math.atan2(m_sin, m_cos); + } + + /** Inverts this rotation. This is simply the negative of the current angular value. */ + public void mut_unaryMinus() { + m_value *= -1; + m_cos = Math.cos(m_value); + m_sin = Math.sin(m_value); + } + + /** + * Multiplies this rotation by a scalar. + * + * @param scalar The scalar. + */ + public void mut_times(double scalar) { + m_value *= scalar; + m_cos = Math.cos(m_value); + m_sin = Math.sin(m_value); + } + + /** + * Divides this rotation by a scalar. + * + * @param scalar The scalar. + */ + public void mut_div(double scalar) { + mut_times(1.0 / scalar); + } + + /** + * Determines this delta in this rotation and other in radians. + * + * @param other The other rotation. + * @return The delta in radians. + */ + public double deltaRadians(Rotation2d other) { + double x = m_cos * other.m_cos + m_sin * other.m_sin; + double y = -m_cos * other.m_sin + m_sin * other.m_cos; + double magnitude = Math.hypot(x, y); + if (magnitude > 1e-6) { + y /= magnitude; + x /= magnitude; + } else { + y = 0.0; + x = 1.0; + MathSharedStore.reportError( + "x and y components of MutRotation2d are zero\n", Thread.currentThread().getStackTrace()); + } + return Math.atan2(y, x); + } + + /** + * Determines this delta in this rotation and other in degrees. + * + * @param other The other rotation. + * @return The delta in degrees. + */ + public double deltaDegrees(Rotation2d other) { + return Math.toDegrees(deltaRadians(other)); + } + + /** + * Determines this delta in this rotation and other in rotations. + * + * @param other The other rotation. + * @return The delta in rotations. + */ + public double deltaRotations(Rotation2d other) { + return Units.radiansToRotations(deltaRadians(other)); + } + + @Override + public String toString() { + return String.format("MutRotation2d(Rads: %.2f, Deg: %.2f)", m_value, Math.toDegrees(m_value)); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java index 3eeac17a8f8..c83a28f2272 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java @@ -81,9 +81,9 @@ public class Rotation2d */ public static final Rotation2d k180deg = kPi; - private final double m_value; - private final double m_cos; - private final double m_sin; + protected double m_value; + protected double m_cos; + protected double m_sin; /** Constructs a Rotation2d with a default angle of 0 degrees. */ public Rotation2d() { diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java index a5205dcfc74..ce5b4ca5726 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java @@ -213,8 +213,10 @@ public ChassisSpeeds toChassisSpeeds(SwerveModuleState... moduleStates) { for (int i = 0; i < m_numModules; i++) { var module = moduleStates[i]; - moduleStatesMatrix.set(i * 2, 0, module.speedMetersPerSecond * module.angle.getCos()); - moduleStatesMatrix.set(i * 2 + 1, module.speedMetersPerSecond * module.angle.getSin()); + moduleStatesMatrix.set( + i * 2, 0, module.getSpeedMetersPerSecond() * module.getAngle().getCos()); + moduleStatesMatrix.set( + i * 2 + 1, module.getSpeedMetersPerSecond() * module.getAngle().getSin()); } var chassisSpeedsVector = m_forwardKinematics.mult(moduleStatesMatrix); @@ -282,12 +284,14 @@ public static void desaturateWheelSpeeds( SwerveModuleState[] moduleStates, double attainableMaxSpeedMetersPerSecond) { double realMaxSpeed = 0; for (SwerveModuleState moduleState : moduleStates) { - realMaxSpeed = Math.max(realMaxSpeed, Math.abs(moduleState.speedMetersPerSecond)); + realMaxSpeed = Math.max(realMaxSpeed, Math.abs(moduleState.getSpeedMetersPerSecond())); } if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) { for (SwerveModuleState moduleState : moduleStates) { - moduleState.speedMetersPerSecond = - moduleState.speedMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond; + moduleState.setSpeed( + moduleState.getSpeedMetersPerSecond() + / realMaxSpeed + * attainableMaxSpeedMetersPerSecond); } } } @@ -335,7 +339,7 @@ public static void desaturateWheelSpeeds( double attainableMaxRotationalVelocityRadiansPerSecond) { double realMaxSpeed = 0; for (SwerveModuleState moduleState : moduleStates) { - realMaxSpeed = Math.max(realMaxSpeed, Math.abs(moduleState.speedMetersPerSecond)); + realMaxSpeed = Math.max(realMaxSpeed, Math.abs(moduleState.getSpeedMetersPerSecond())); } if (attainableMaxTranslationalSpeedMetersPerSecond == 0 @@ -352,7 +356,7 @@ public static void desaturateWheelSpeeds( double k = Math.max(translationalK, rotationalK); double scale = Math.min(k * attainableMaxModuleSpeedMetersPerSecond / realMaxSpeed, 1); for (SwerveModuleState moduleState : moduleStates) { - moduleState.speedMetersPerSecond *= scale; + moduleState.setSpeed(moduleState.getSpeedMetersPerSecond() * scale); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java index 8d30494d526..8e56ead9ad7 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java @@ -6,6 +6,7 @@ import static edu.wpi.first.units.Units.MetersPerSecond; +import edu.wpi.first.math.geometry.MutRotation2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.proto.SwerveModuleStateProto; import edu.wpi.first.math.kinematics.struct.SwerveModuleStateStruct; @@ -18,10 +19,10 @@ public class SwerveModuleState implements Comparable, ProtobufSerializable, StructSerializable { /** Speed of the wheel of the module. */ - public double speedMetersPerSecond; + private double speedMetersPerSecond; /** Angle of the module. */ - public Rotation2d angle = Rotation2d.kZero; + private final MutRotation2d angle = new MutRotation2d(); /** SwerveModuleState protobuf for serialization. */ public static final SwerveModuleStateProto proto = new SwerveModuleStateProto(); @@ -40,7 +41,7 @@ public SwerveModuleState() {} */ public SwerveModuleState(double speedMetersPerSecond, Rotation2d angle) { this.speedMetersPerSecond = speedMetersPerSecond; - this.angle = angle; + this.angle.mut_Rotation2d(angle); } /** @@ -53,6 +54,53 @@ public SwerveModuleState(LinearVelocity speed, Rotation2d angle) { this(speed.in(MetersPerSecond), angle); } + /** + * Returns the angle of this SwerveModuleState. + * + * @return The angle of this SwerveModuleState. + */ + public Rotation2d getAngle() { + return angle; + } + + /** + * Returns the speed of this SwerveModuleState. + * + * @return The speed of this SwerveModuleState. + */ + public double getSpeedMetersPerSecond() { + return speedMetersPerSecond; + } + + /** + * Sets the angle of this SwerveModuleState. + * + * @param angle The new angle of this state. + */ + public void setAngle(Rotation2d angle) { + this.angle.mut_Rotation2d(angle); + } + + /** + * Sets the speed of this SwerveModuleState. + * + * @param speedMetersPerSecond The new speed of this state. + */ + public void setSpeed(double speedMetersPerSecond) { + this.speedMetersPerSecond = speedMetersPerSecond; + } + + /** + * Sets the angle and speed of this SwerveModuleState. + * + * @param speedMetersPerSecond The new speed of this state. + * @param angle The new angle of this state. + */ + public void setState(double speedMetersPerSecond, Rotation2d angle) { + setAngle(angle); + setSpeed(speedMetersPerSecond); + } + @Override public boolean equals(Object obj) { return obj instanceof SwerveModuleState other @@ -84,22 +132,30 @@ public String toString() { } /** - * Minimize the change in heading the desired swerve module state would require by potentially - * reversing the direction the wheel spins. If this is used with the PIDController class's - * continuous input functionality, the furthest a wheel will ever rotate is 90 degrees. + * Minimize the change in heading this swerve module state would require by potentially reversing + * the direction the wheel spins. If this is used with the PIDController class's continuous input + * functionality, the furthest a wheel will ever rotate is 90 degrees. * - * @param desiredState The desired state. - * @param currentAngle The current module angle. - * @return Optimized swerve module state. + * @param currentModuleAngle The current module state's angle. */ - public static SwerveModuleState optimize( - SwerveModuleState desiredState, Rotation2d currentAngle) { - var delta = desiredState.angle.minus(currentAngle); - if (Math.abs(delta.getDegrees()) > 90.0) { - return new SwerveModuleState( - -desiredState.speedMetersPerSecond, desiredState.angle.rotateBy(Rotation2d.kPi)); - } else { - return new SwerveModuleState(desiredState.speedMetersPerSecond, desiredState.angle); + public void optimize(Rotation2d currentModuleAngle) { + var delta = angle.deltaDegrees(currentModuleAngle); + if (Math.abs(delta) > 90.0) { + speedMetersPerSecond *= -1; + angle.mut_rotateBy(Rotation2d.kPi); } } + + /** + * Scales the speed of the module by the cosine of the angle error. This scales down movement + * perpendicular to the desired direction of travel that can occur when modules change directions. + * This results in smoother driver. + * + * @param currentModuleAngle The current module state's angle. + */ + public void cosineScale(Rotation2d currentModuleAngle) { + var delta = angle.deltaRadians(currentModuleAngle); + var cosine = Math.cos(delta); + speedMetersPerSecond *= cosine; + } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveModuleStateProto.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveModuleStateProto.java index cccff4d5910..7741ef7e2be 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveModuleStateProto.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveModuleStateProto.java @@ -34,7 +34,7 @@ public SwerveModuleState unpack(ProtobufSwerveModuleState msg) { @Override public void pack(ProtobufSwerveModuleState msg, SwerveModuleState value) { - msg.setSpeed(value.speedMetersPerSecond); - Rotation2d.proto.pack(msg.getMutableAngle(), value.angle); + msg.setSpeed(value.getSpeedMetersPerSecond()); + Rotation2d.proto.pack(msg.getMutableAngle(), value.getAngle()); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModuleStateStruct.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModuleStateStruct.java index af29a8d628f..218b240f147 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModuleStateStruct.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModuleStateStruct.java @@ -44,7 +44,7 @@ public SwerveModuleState unpack(ByteBuffer bb) { @Override public void pack(ByteBuffer bb, SwerveModuleState value) { - bb.putDouble(value.speedMetersPerSecond); - Rotation2d.struct.pack(bb, value.angle); + bb.putDouble(value.getSpeedMetersPerSecond()); + Rotation2d.struct.pack(bb, value.getAngle()); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java index 54398902f7b..d2f13b7a099 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java @@ -201,9 +201,9 @@ void testFollowTrajectory( for (int i = 0; i < moduleStates.length; i++) { positions[i].distanceMeters += - moduleStates[i].speedMetersPerSecond * (1 - rand.nextGaussian() * 0.05) * dt; + moduleStates[i].getSpeedMetersPerSecond() * (1 - rand.nextGaussian() * 0.05) * dt; positions[i].angle = - moduleStates[i].angle.plus(new Rotation2d(rand.nextGaussian() * 0.005)); + moduleStates[i].getAngle().plus(new Rotation2d(rand.nextGaussian() * 0.005)); } var xHat = diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java index 546db373147..64fdeb245e9 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java @@ -29,14 +29,14 @@ void testStraightLineInverseKinematics() { // test inverse kinematics going in a var moduleStates = m_kinematics.toSwerveModuleStates(speeds); assertAll( - () -> assertEquals(5.0, moduleStates[0].speedMetersPerSecond, kEpsilon), - () -> assertEquals(5.0, moduleStates[1].speedMetersPerSecond, kEpsilon), - () -> assertEquals(5.0, moduleStates[2].speedMetersPerSecond, kEpsilon), - () -> assertEquals(5.0, moduleStates[3].speedMetersPerSecond, kEpsilon), - () -> assertEquals(0.0, moduleStates[0].angle.getRadians(), kEpsilon), - () -> assertEquals(0.0, moduleStates[1].angle.getRadians(), kEpsilon), - () -> assertEquals(0.0, moduleStates[2].angle.getRadians(), kEpsilon), - () -> assertEquals(0.0, moduleStates[3].angle.getRadians(), kEpsilon)); + () -> assertEquals(5.0, moduleStates[0].getSpeedMetersPerSecond(), kEpsilon), + () -> assertEquals(5.0, moduleStates[1].getSpeedMetersPerSecond(), kEpsilon), + () -> assertEquals(5.0, moduleStates[2].getSpeedMetersPerSecond(), kEpsilon), + () -> assertEquals(5.0, moduleStates[3].getSpeedMetersPerSecond(), kEpsilon), + () -> assertEquals(0.0, moduleStates[0].getAngle().getRadians(), kEpsilon), + () -> assertEquals(0.0, moduleStates[1].getAngle().getRadians(), kEpsilon), + () -> assertEquals(0.0, moduleStates[2].getAngle().getRadians(), kEpsilon), + () -> assertEquals(0.0, moduleStates[3].getAngle().getRadians(), kEpsilon)); } @Test @@ -68,14 +68,14 @@ void testStraightStrafeInverseKinematics() { var moduleStates = m_kinematics.toSwerveModuleStates(speeds); assertAll( - () -> assertEquals(5.0, moduleStates[0].speedMetersPerSecond, kEpsilon), - () -> assertEquals(5.0, moduleStates[1].speedMetersPerSecond, kEpsilon), - () -> assertEquals(5.0, moduleStates[2].speedMetersPerSecond, kEpsilon), - () -> assertEquals(5.0, moduleStates[3].speedMetersPerSecond, kEpsilon), - () -> assertEquals(90.0, moduleStates[0].angle.getDegrees(), kEpsilon), - () -> assertEquals(90.0, moduleStates[1].angle.getDegrees(), kEpsilon), - () -> assertEquals(90.0, moduleStates[2].angle.getDegrees(), kEpsilon), - () -> assertEquals(90.0, moduleStates[3].angle.getDegrees(), kEpsilon)); + () -> assertEquals(5.0, moduleStates[0].getSpeedMetersPerSecond(), kEpsilon), + () -> assertEquals(5.0, moduleStates[1].getSpeedMetersPerSecond(), kEpsilon), + () -> assertEquals(5.0, moduleStates[2].getSpeedMetersPerSecond(), kEpsilon), + () -> assertEquals(5.0, moduleStates[3].getSpeedMetersPerSecond(), kEpsilon), + () -> assertEquals(90.0, moduleStates[0].getAngle().getDegrees(), kEpsilon), + () -> assertEquals(90.0, moduleStates[1].getAngle().getDegrees(), kEpsilon), + () -> assertEquals(90.0, moduleStates[2].getAngle().getDegrees(), kEpsilon), + () -> assertEquals(90.0, moduleStates[3].getAngle().getDegrees(), kEpsilon)); } @Test @@ -109,14 +109,14 @@ void testConserveWheelAngle() { // Robot is stationary, but module angles are preserved. assertAll( - () -> assertEquals(0.0, moduleStates[0].speedMetersPerSecond, kEpsilon), - () -> assertEquals(0.0, moduleStates[1].speedMetersPerSecond, kEpsilon), - () -> assertEquals(0.0, moduleStates[2].speedMetersPerSecond, kEpsilon), - () -> assertEquals(0.0, moduleStates[3].speedMetersPerSecond, kEpsilon), - () -> assertEquals(135.0, moduleStates[0].angle.getDegrees(), kEpsilon), - () -> assertEquals(45.0, moduleStates[1].angle.getDegrees(), kEpsilon), - () -> assertEquals(-135.0, moduleStates[2].angle.getDegrees(), kEpsilon), - () -> assertEquals(-45.0, moduleStates[3].angle.getDegrees(), kEpsilon)); + () -> assertEquals(0.0, moduleStates[0].getSpeedMetersPerSecond(), kEpsilon), + () -> assertEquals(0.0, moduleStates[1].getSpeedMetersPerSecond(), kEpsilon), + () -> assertEquals(0.0, moduleStates[2].getSpeedMetersPerSecond(), kEpsilon), + () -> assertEquals(0.0, moduleStates[3].getSpeedMetersPerSecond(), kEpsilon), + () -> assertEquals(135.0, moduleStates[0].getAngle().getDegrees(), kEpsilon), + () -> assertEquals(45.0, moduleStates[1].getAngle().getDegrees(), kEpsilon), + () -> assertEquals(-135.0, moduleStates[2].getAngle().getDegrees(), kEpsilon), + () -> assertEquals(-45.0, moduleStates[3].getAngle().getDegrees(), kEpsilon)); } @Test @@ -131,14 +131,14 @@ void testResetWheelAngle() { // Robot is stationary, but module angles are preserved. assertAll( - () -> assertEquals(0.0, moduleStates[0].speedMetersPerSecond, kEpsilon), - () -> assertEquals(0.0, moduleStates[1].speedMetersPerSecond, kEpsilon), - () -> assertEquals(0.0, moduleStates[2].speedMetersPerSecond, kEpsilon), - () -> assertEquals(0.0, moduleStates[3].speedMetersPerSecond, kEpsilon), - () -> assertEquals(0.0, moduleStates[0].angle.getDegrees(), kEpsilon), - () -> assertEquals(90.0, moduleStates[1].angle.getDegrees(), kEpsilon), - () -> assertEquals(180.0, moduleStates[2].angle.getDegrees(), kEpsilon), - () -> assertEquals(270.0, moduleStates[3].angle.getDegrees(), kEpsilon)); + () -> assertEquals(0.0, moduleStates[0].getSpeedMetersPerSecond(), kEpsilon), + () -> assertEquals(0.0, moduleStates[1].getSpeedMetersPerSecond(), kEpsilon), + () -> assertEquals(0.0, moduleStates[2].getSpeedMetersPerSecond(), kEpsilon), + () -> assertEquals(0.0, moduleStates[3].getSpeedMetersPerSecond(), kEpsilon), + () -> assertEquals(0.0, moduleStates[0].getAngle().getDegrees(), kEpsilon), + () -> assertEquals(90.0, moduleStates[1].getAngle().getDegrees(), kEpsilon), + () -> assertEquals(180.0, moduleStates[2].getAngle().getDegrees(), kEpsilon), + () -> assertEquals(270.0, moduleStates[3].getAngle().getDegrees(), kEpsilon)); } @Test @@ -154,14 +154,14 @@ void testTurnInPlaceInverseKinematics() { */ assertAll( - () -> assertEquals(106.63, moduleStates[0].speedMetersPerSecond, 0.1), - () -> assertEquals(106.63, moduleStates[1].speedMetersPerSecond, 0.1), - () -> assertEquals(106.63, moduleStates[2].speedMetersPerSecond, 0.1), - () -> assertEquals(106.63, moduleStates[3].speedMetersPerSecond, 0.1), - () -> assertEquals(135.0, moduleStates[0].angle.getDegrees(), kEpsilon), - () -> assertEquals(45.0, moduleStates[1].angle.getDegrees(), kEpsilon), - () -> assertEquals(-135.0, moduleStates[2].angle.getDegrees(), kEpsilon), - () -> assertEquals(-45.0, moduleStates[3].angle.getDegrees(), kEpsilon)); + () -> assertEquals(106.63, moduleStates[0].getSpeedMetersPerSecond(), 0.1), + () -> assertEquals(106.63, moduleStates[1].getSpeedMetersPerSecond(), 0.1), + () -> assertEquals(106.63, moduleStates[2].getSpeedMetersPerSecond(), 0.1), + () -> assertEquals(106.63, moduleStates[3].getSpeedMetersPerSecond(), 0.1), + () -> assertEquals(135.0, moduleStates[0].getAngle().getDegrees(), kEpsilon), + () -> assertEquals(45.0, moduleStates[1].getAngle().getDegrees(), kEpsilon), + () -> assertEquals(-135.0, moduleStates[2].getAngle().getDegrees(), kEpsilon), + () -> assertEquals(-45.0, moduleStates[3].getAngle().getDegrees(), kEpsilon)); } @Test @@ -209,14 +209,14 @@ void testOffCenterCORRotationInverseKinematics() { */ assertAll( - () -> assertEquals(0.0, moduleStates[0].speedMetersPerSecond, 0.1), - () -> assertEquals(150.796, moduleStates[1].speedMetersPerSecond, 0.1), - () -> assertEquals(150.796, moduleStates[2].speedMetersPerSecond, 0.1), - () -> assertEquals(213.258, moduleStates[3].speedMetersPerSecond, 0.1), - () -> assertEquals(0.0, moduleStates[0].angle.getDegrees(), kEpsilon), - () -> assertEquals(0.0, moduleStates[1].angle.getDegrees(), kEpsilon), - () -> assertEquals(-90.0, moduleStates[2].angle.getDegrees(), kEpsilon), - () -> assertEquals(-45.0, moduleStates[3].angle.getDegrees(), kEpsilon)); + () -> assertEquals(0.0, moduleStates[0].getSpeedMetersPerSecond(), 0.1), + () -> assertEquals(150.796, moduleStates[1].getSpeedMetersPerSecond(), 0.1), + () -> assertEquals(150.796, moduleStates[2].getSpeedMetersPerSecond(), 0.1), + () -> assertEquals(213.258, moduleStates[3].getSpeedMetersPerSecond(), 0.1), + () -> assertEquals(0.0, moduleStates[0].getAngle().getDegrees(), kEpsilon), + () -> assertEquals(0.0, moduleStates[1].getAngle().getDegrees(), kEpsilon), + () -> assertEquals(-90.0, moduleStates[2].getAngle().getDegrees(), kEpsilon), + () -> assertEquals(-45.0, moduleStates[3].getAngle().getDegrees(), kEpsilon)); } @Test @@ -272,14 +272,14 @@ private void assertModuleState( assertAll( () -> assertEquals( - expected.speedMetersPerSecond, - actual.speedMetersPerSecond, - tolerance.speedMetersPerSecond), + expected.getSpeedMetersPerSecond(), + actual.getSpeedMetersPerSecond(), + tolerance.getSpeedMetersPerSecond()), () -> assertEquals( - expected.angle.getDegrees(), - actual.angle.getDegrees(), - tolerance.angle.getDegrees())); + expected.getAngle().getDegrees(), + actual.getAngle().getDegrees(), + tolerance.getAngle().getDegrees())); } /** @@ -368,10 +368,10 @@ void testDesaturate() { double factor = 5.5 / 7.0; assertAll( - () -> assertEquals(5.0 * factor, arr[0].speedMetersPerSecond, kEpsilon), - () -> assertEquals(6.0 * factor, arr[1].speedMetersPerSecond, kEpsilon), - () -> assertEquals(4.0 * factor, arr[2].speedMetersPerSecond, kEpsilon), - () -> assertEquals(7.0 * factor, arr[3].speedMetersPerSecond, kEpsilon)); + () -> assertEquals(5.0 * factor, arr[0].getSpeedMetersPerSecond(), kEpsilon), + () -> assertEquals(6.0 * factor, arr[1].getSpeedMetersPerSecond(), kEpsilon), + () -> assertEquals(4.0 * factor, arr[2].getSpeedMetersPerSecond(), kEpsilon), + () -> assertEquals(7.0 * factor, arr[3].getSpeedMetersPerSecond(), kEpsilon)); } @Test @@ -388,10 +388,10 @@ void testDesaturateSmooth() { double factor = 5.5 / 7.0; assertAll( - () -> assertEquals(5.0 * factor, arr[0].speedMetersPerSecond, kEpsilon), - () -> assertEquals(6.0 * factor, arr[1].speedMetersPerSecond, kEpsilon), - () -> assertEquals(4.0 * factor, arr[2].speedMetersPerSecond, kEpsilon), - () -> assertEquals(7.0 * factor, arr[3].speedMetersPerSecond, kEpsilon)); + () -> assertEquals(5.0 * factor, arr[0].getSpeedMetersPerSecond(), kEpsilon), + () -> assertEquals(6.0 * factor, arr[1].getSpeedMetersPerSecond(), kEpsilon), + () -> assertEquals(4.0 * factor, arr[2].getSpeedMetersPerSecond(), kEpsilon), + () -> assertEquals(7.0 * factor, arr[3].getSpeedMetersPerSecond(), kEpsilon)); } @Test @@ -405,9 +405,9 @@ void testDesaturateNegativeSpeed() { SwerveDriveKinematics.desaturateWheelSpeeds(arr, 1); assertAll( - () -> assertEquals(0.5, arr[0].speedMetersPerSecond, kEpsilon), - () -> assertEquals(0.5, arr[1].speedMetersPerSecond, kEpsilon), - () -> assertEquals(-1.0, arr[2].speedMetersPerSecond, kEpsilon), - () -> assertEquals(-1.0, arr[3].speedMetersPerSecond, kEpsilon)); + () -> assertEquals(0.5, arr[0].getSpeedMetersPerSecond(), kEpsilon), + () -> assertEquals(0.5, arr[1].getSpeedMetersPerSecond(), kEpsilon), + () -> assertEquals(-1.0, arr[2].getSpeedMetersPerSecond(), kEpsilon), + () -> assertEquals(-1.0, arr[3].getSpeedMetersPerSecond(), kEpsilon)); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.java index 411ba705f47..5b7f573b90c 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.java @@ -146,19 +146,20 @@ void testAccuracyFacingTrajectory() { groundTruthState.velocityMetersPerSecond * groundTruthState.curvatureRadPerMeter)); for (var moduleState : moduleStates) { - moduleState.angle = moduleState.angle.plus(new Rotation2d(rand.nextGaussian() * 0.005)); - moduleState.speedMetersPerSecond += rand.nextGaussian() * 0.1; + moduleState.setAngle( + moduleState.getAngle().plus(new Rotation2d(rand.nextGaussian() * 0.005))); + moduleState.setSpeed(moduleState.getSpeedMetersPerSecond() + rand.nextGaussian() * 0.1); } - fl.distanceMeters += moduleStates[0].speedMetersPerSecond * dt; - fr.distanceMeters += moduleStates[1].speedMetersPerSecond * dt; - bl.distanceMeters += moduleStates[2].speedMetersPerSecond * dt; - br.distanceMeters += moduleStates[3].speedMetersPerSecond * dt; + fl.distanceMeters += moduleStates[0].getSpeedMetersPerSecond() * dt; + fr.distanceMeters += moduleStates[1].getSpeedMetersPerSecond() * dt; + bl.distanceMeters += moduleStates[2].getSpeedMetersPerSecond() * dt; + br.distanceMeters += moduleStates[3].getSpeedMetersPerSecond() * dt; - fl.angle = moduleStates[0].angle; - fr.angle = moduleStates[1].angle; - bl.angle = moduleStates[2].angle; - br.angle = moduleStates[3].angle; + fl.angle = moduleStates[0].getAngle(); + fr.angle = moduleStates[1].getAngle(); + bl.angle = moduleStates[2].getAngle(); + br.angle = moduleStates[3].getAngle(); var xHat = odometry.update( diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveModuleStateTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveModuleStateTest.java index 15ca831f806..d430fb2560c 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveModuleStateTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveModuleStateTest.java @@ -17,37 +17,41 @@ class SwerveModuleStateTest { void testOptimize() { var angleA = Rotation2d.fromDegrees(45); var refA = new SwerveModuleState(-2.0, Rotation2d.kPi); - var optimizedA = SwerveModuleState.optimize(refA, angleA); + refA.optimize(angleA); + var optimizedA = refA; assertAll( - () -> assertEquals(2.0, optimizedA.speedMetersPerSecond, kEpsilon), - () -> assertEquals(0.0, optimizedA.angle.getDegrees(), kEpsilon)); + () -> assertEquals(2.0, optimizedA.getSpeedMetersPerSecond(), kEpsilon), + () -> assertEquals(0.0, optimizedA.getAngle().getDegrees(), kEpsilon)); var angleB = Rotation2d.fromDegrees(-50); var refB = new SwerveModuleState(4.7, Rotation2d.fromDegrees(41)); - var optimizedB = SwerveModuleState.optimize(refB, angleB); + refB.optimize(angleB); + var optimizedB = refB; assertAll( - () -> assertEquals(-4.7, optimizedB.speedMetersPerSecond, kEpsilon), - () -> assertEquals(-139.0, optimizedB.angle.getDegrees(), kEpsilon)); + () -> assertEquals(-4.7, optimizedB.getSpeedMetersPerSecond(), kEpsilon), + () -> assertEquals(-139.0, optimizedB.getAngle().getDegrees(), kEpsilon)); } @Test void testNoOptimize() { var angleA = Rotation2d.kZero; var refA = new SwerveModuleState(2.0, Rotation2d.fromDegrees(89)); - var optimizedA = SwerveModuleState.optimize(refA, angleA); + refA.optimize(angleA); + var optimizedA = refA; assertAll( - () -> assertEquals(2.0, optimizedA.speedMetersPerSecond, kEpsilon), - () -> assertEquals(89.0, optimizedA.angle.getDegrees(), kEpsilon)); + () -> assertEquals(2.0, optimizedA.getSpeedMetersPerSecond(), kEpsilon), + () -> assertEquals(89.0, optimizedA.getAngle().getDegrees(), kEpsilon)); var angleB = Rotation2d.kZero; var refB = new SwerveModuleState(-2.0, Rotation2d.fromDegrees(-2)); - var optimizedB = SwerveModuleState.optimize(refB, angleB); + refB.optimize(angleB); + var optimizedB = refB; assertAll( - () -> assertEquals(-2.0, optimizedB.speedMetersPerSecond, kEpsilon), - () -> assertEquals(-2.0, optimizedB.angle.getDegrees(), kEpsilon)); + () -> assertEquals(-2.0, optimizedB.getSpeedMetersPerSecond(), kEpsilon), + () -> assertEquals(-2.0, optimizedB.getAngle().getDegrees(), kEpsilon)); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/SwerveModuleStateProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/SwerveModuleStateProtoTest.java index 269b7b18b27..a363d1b084a 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/SwerveModuleStateProtoTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/SwerveModuleStateProtoTest.java @@ -20,7 +20,7 @@ void testRoundtrip() { SwerveModuleState.proto.pack(proto, DATA); SwerveModuleState data = SwerveModuleState.proto.unpack(proto); - assertEquals(DATA.speedMetersPerSecond, data.speedMetersPerSecond); - assertEquals(DATA.angle, data.angle); + assertEquals(DATA.getSpeedMetersPerSecond(), data.getSpeedMetersPerSecond()); + assertEquals(DATA.getAngle(), data.getAngle()); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/SwerveModuleStateStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/SwerveModuleStateStructTest.java index 625514c04b9..d7e984db3e0 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/SwerveModuleStateStructTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/SwerveModuleStateStructTest.java @@ -23,7 +23,7 @@ void testRoundtrip() { buffer.rewind(); SwerveModuleState data = SwerveModuleState.struct.unpack(buffer); - assertEquals(DATA.speedMetersPerSecond, data.speedMetersPerSecond); - assertEquals(DATA.angle, data.angle); + assertEquals(DATA.getSpeedMetersPerSecond(), data.getSpeedMetersPerSecond()); + assertEquals(DATA.getAngle(), data.getAngle()); } }