From 03f0fc4dea4703ceea4b4cff07142e47806b0373 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sun, 15 Dec 2024 16:09:34 -0800 Subject: [PATCH] [wpimath] Use immutable member functions in ChassisSpeeds (#7545) --- .../command/MecanumControllerCommand.java | 2 +- .../frc2/command/MecanumControllerCommand.cpp | 2 +- .../examples/MecanumBot/cpp/Drivetrain.cpp | 15 +- .../cpp/Drivetrain.cpp | 15 +- .../cpp/examples/SwerveBot/cpp/Drivetrain.cpp | 13 +- .../cpp/subsystems/DriveSubsystem.cpp | 13 +- .../cpp/Drivetrain.cpp | 15 +- .../examples/mecanumbot/Drivetrain.java | 16 +- .../mecanumdriveposeestimator/Drivetrain.java | 17 +- .../examples/swervebot/Drivetrain.java | 26 +-- .../subsystems/DriveSubsystem.java | 27 ++- .../swervedriveposeestimator/Drivetrain.java | 28 +-- .../controller/HolonomicDriveController.java | 6 +- .../first/math/kinematics/ChassisSpeeds.java | 171 +----------------- .../kinematics/MecanumDriveWheelSpeeds.java | 27 +-- .../MecanumDriveKinematicsConstraint.java | 4 +- .../frc/controller/HolonomicDriveController.h | 8 +- .../include/frc/kinematics/ChassisSpeeds.h | 112 ++---------- .../frc/kinematics/MecanumDriveWheelSpeeds.h | 12 +- .../MecanumDriveKinematicsConstraint.h | 6 +- .../math/kinematics/ChassisSpeedsTest.java | 11 +- .../MecanumDriveKinematicsTest.java | 6 +- .../cpp/kinematics/ChassisSpeedsTest.cpp | 15 +- .../kinematics/MecanumDriveKinematicsTest.cpp | 8 +- 24 files changed, 163 insertions(+), 412 deletions(-) diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java index 36a5989983..a3d6c92d79 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java @@ -488,7 +488,7 @@ public class MecanumControllerCommand extends Command { m_controller.calculate(m_pose.get(), desiredState, m_desiredRotation.get()); var targetWheelSpeeds = m_kinematics.toWheelSpeeds(targetChassisSpeeds); - targetWheelSpeeds.desaturate(m_maxWheelVelocityMetersPerSecond); + targetWheelSpeeds = targetWheelSpeeds.desaturate(m_maxWheelVelocityMetersPerSecond); double frontLeftSpeedSetpoint = targetWheelSpeeds.frontLeftMetersPerSecond; double rearLeftSpeedSetpoint = targetWheelSpeeds.rearLeftMetersPerSecond; diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp index 742a37d959..f08bab80e9 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp @@ -167,7 +167,7 @@ void MecanumControllerCommand::Execute() { m_controller.Calculate(m_pose(), m_desiredState, m_desiredRotation()); auto targetWheelSpeeds = m_kinematics.ToWheelSpeeds(targetChassisSpeeds); - targetWheelSpeeds.Desaturate(m_maxWheelVelocity); + targetWheelSpeeds = targetWheelSpeeds.Desaturate(m_maxWheelVelocity); auto frontLeftSpeedSetpoint = targetWheelSpeeds.frontLeft; auto rearLeftSpeedSetpoint = targetWheelSpeeds.rearLeft; diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp index 94b5feb78d..456a6cc5b5 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp @@ -4,6 +4,8 @@ #include "Drivetrain.h" +#include + frc::MecanumDriveWheelSpeeds Drivetrain::GetCurrentState() const { return {units::meters_per_second_t{m_frontLeftEncoder.GetRate()}, units::meters_per_second_t{m_frontRightEncoder.GetRate()}, @@ -51,13 +53,12 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed, units::meters_per_second_t ySpeed, units::radians_per_second_t rot, bool fieldRelative, units::second_t period) { - auto wheelSpeeds = m_kinematics.ToWheelSpeeds(frc::ChassisSpeeds::Discretize( - fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds( - xSpeed, ySpeed, rot, m_gyro.GetRotation2d()) - : frc::ChassisSpeeds{xSpeed, ySpeed, rot}, - period)); - wheelSpeeds.Desaturate(kMaxSpeed); - SetSpeeds(wheelSpeeds); + frc::ChassisSpeeds chassisSpeeds{xSpeed, ySpeed, rot}; + if (fieldRelative) { + chassisSpeeds = chassisSpeeds.ToRobotRelative(m_gyro.GetRotation2d()); + } + SetSpeeds(m_kinematics.ToWheelSpeeds(chassisSpeeds.Discretize(period)) + .Desaturate(kMaxSpeed)); } void Drivetrain::UpdateOdometry() { diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp index 7ef5e4629a..8e6369cf3a 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp @@ -48,14 +48,13 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed, units::meters_per_second_t ySpeed, units::radians_per_second_t rot, bool fieldRelative, units::second_t period) { - auto wheelSpeeds = m_kinematics.ToWheelSpeeds(frc::ChassisSpeeds::Discretize( - fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds( - xSpeed, ySpeed, rot, - m_poseEstimator.GetEstimatedPosition().Rotation()) - : frc::ChassisSpeeds{xSpeed, ySpeed, rot}, - period)); - wheelSpeeds.Desaturate(kMaxSpeed); - SetSpeeds(wheelSpeeds); + frc::ChassisSpeeds chassisSpeeds{xSpeed, ySpeed, rot}; + if (fieldRelative) { + chassisSpeeds = chassisSpeeds.ToRobotRelative( + m_poseEstimator.GetEstimatedPosition().Rotation()); + } + SetSpeeds(m_kinematics.ToWheelSpeeds(chassisSpeeds.Discretize(period)) + .Desaturate(kMaxSpeed)); } void Drivetrain::UpdateOdometry() { diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp index 03cc93b66b..39d711b622 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp @@ -8,17 +8,16 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed, units::meters_per_second_t ySpeed, units::radians_per_second_t rot, bool fieldRelative, units::second_t period) { - auto states = - m_kinematics.ToSwerveModuleStates(frc::ChassisSpeeds::Discretize( - fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds( - xSpeed, ySpeed, rot, m_gyro.GetRotation2d()) - : frc::ChassisSpeeds{xSpeed, ySpeed, rot}, - period)); + frc::ChassisSpeeds chassisSpeeds{xSpeed, ySpeed, rot}; + if (fieldRelative) { + chassisSpeeds = chassisSpeeds.ToRobotRelative(m_gyro.GetRotation2d()); + } + chassisSpeeds = chassisSpeeds.Discretize(period); + auto states = m_kinematics.ToSwerveModuleStates(chassisSpeeds); m_kinematics.DesaturateWheelSpeeds(&states, kMaxSpeed); auto [fl, fr, bl, br] = states; - m_frontLeft.SetDesiredState(fl); m_frontRight.SetDesiredState(fr); m_backLeft.SetDesiredState(bl); diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp index a6b810a386..ae3cd7c8be 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp @@ -53,17 +53,16 @@ void DriveSubsystem::Drive(units::meters_per_second_t xSpeed, units::meters_per_second_t ySpeed, units::radians_per_second_t rot, bool fieldRelative, units::second_t period) { - auto states = - kDriveKinematics.ToSwerveModuleStates(frc::ChassisSpeeds::Discretize( - fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds( - xSpeed, ySpeed, rot, m_gyro.GetRotation2d()) - : frc::ChassisSpeeds{xSpeed, ySpeed, rot}, - period)); + frc::ChassisSpeeds chassisSpeeds{xSpeed, ySpeed, rot}; + if (fieldRelative) { + chassisSpeeds = chassisSpeeds.ToRobotRelative(m_gyro.GetRotation2d()); + } + chassisSpeeds = chassisSpeeds.Discretize(period); + auto states = kDriveKinematics.ToSwerveModuleStates(chassisSpeeds); kDriveKinematics.DesaturateWheelSpeeds(&states, AutoConstants::kMaxSpeed); auto [fl, fr, bl, br] = states; - m_frontLeft.SetDesiredState(fl); m_frontRight.SetDesiredState(fr); m_rearLeft.SetDesiredState(bl); diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp index e834d3d271..be7f3592f5 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp @@ -12,18 +12,17 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed, units::meters_per_second_t ySpeed, units::radians_per_second_t rot, bool fieldRelative, units::second_t period) { - auto states = - m_kinematics.ToSwerveModuleStates(frc::ChassisSpeeds::Discretize( - fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds( - xSpeed, ySpeed, rot, - m_poseEstimator.GetEstimatedPosition().Rotation()) - : frc::ChassisSpeeds{xSpeed, ySpeed, rot}, - period)); + frc::ChassisSpeeds chassisSpeeds{xSpeed, ySpeed, rot}; + if (fieldRelative) { + chassisSpeeds = chassisSpeeds.ToRobotRelative( + m_poseEstimator.GetEstimatedPosition().Rotation()); + } + chassisSpeeds = chassisSpeeds.Discretize(period); + auto states = m_kinematics.ToSwerveModuleStates(chassisSpeeds); m_kinematics.DesaturateWheelSpeeds(&states, kMaxSpeed); auto [fl, fr, bl, br] = states; - m_frontLeft.SetDesiredState(fl); m_frontRight.SetDesiredState(fr); m_backLeft.SetDesiredState(bl); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java index c64c9b3958..8f69237db6 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java @@ -129,16 +129,12 @@ public class Drivetrain { */ public void drive( double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) { - var mecanumDriveWheelSpeeds = - m_kinematics.toWheelSpeeds( - ChassisSpeeds.discretize( - fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds( - xSpeed, ySpeed, rot, m_gyro.getRotation2d()) - : new ChassisSpeeds(xSpeed, ySpeed, rot), - periodSeconds)); - mecanumDriveWheelSpeeds.desaturate(kMaxSpeed); - setSpeeds(mecanumDriveWheelSpeeds); + var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot); + if (fieldRelative) { + chassisSpeeds = chassisSpeeds.toRobotRelative(m_gyro.getRotation2d()); + } + setSpeeds( + m_kinematics.toWheelSpeeds(chassisSpeeds.discretize(periodSeconds)).desaturate(kMaxSpeed)); } /** Updates the field relative position of the robot. */ diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java index 256824c03b..f1b0e5003c 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java @@ -141,16 +141,13 @@ public class Drivetrain { */ public void drive( double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) { - var mecanumDriveWheelSpeeds = - m_kinematics.toWheelSpeeds( - ChassisSpeeds.discretize( - fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds( - xSpeed, ySpeed, rot, m_poseEstimator.getEstimatedPosition().getRotation()) - : new ChassisSpeeds(xSpeed, ySpeed, rot), - periodSeconds)); - mecanumDriveWheelSpeeds.desaturate(kMaxSpeed); - setSpeeds(mecanumDriveWheelSpeeds); + var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot); + if (fieldRelative) { + chassisSpeeds = + chassisSpeeds.toRobotRelative(m_poseEstimator.getEstimatedPosition().getRotation()); + } + setSpeeds( + m_kinematics.toWheelSpeeds(chassisSpeeds.discretize(periodSeconds)).desaturate(kMaxSpeed)); } /** Updates the field relative position of the robot. */ diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java index 958e5a3e61..0784b623b0 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java @@ -57,19 +57,19 @@ public class Drivetrain { */ public void drive( double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) { - var swerveModuleStates = - m_kinematics.toSwerveModuleStates( - ChassisSpeeds.discretize( - fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds( - xSpeed, ySpeed, rot, m_gyro.getRotation2d()) - : new ChassisSpeeds(xSpeed, ySpeed, rot), - periodSeconds)); - SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, kMaxSpeed); - m_frontLeft.setDesiredState(swerveModuleStates[0]); - m_frontRight.setDesiredState(swerveModuleStates[1]); - m_backLeft.setDesiredState(swerveModuleStates[2]); - m_backRight.setDesiredState(swerveModuleStates[3]); + var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot); + if (fieldRelative) { + chassisSpeeds = chassisSpeeds.toRobotRelative(m_gyro.getRotation2d()); + } + chassisSpeeds = chassisSpeeds.discretize(periodSeconds); + + var states = m_kinematics.toWheelSpeeds(chassisSpeeds); + SwerveDriveKinematics.desaturateWheelSpeeds(states, kMaxSpeed); + + m_frontLeft.setDesiredState(states[0]); + m_frontRight.setDesiredState(states[1]); + m_backLeft.setDesiredState(states[2]); + m_backRight.setDesiredState(states[3]); } /** Updates the field relative position of the robot. */ diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java index 04ee2998df..23d04a1aac 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java @@ -118,20 +118,19 @@ public class DriveSubsystem extends SubsystemBase { * @param fieldRelative Whether the provided x and y speeds are relative to the field. */ public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { - var swerveModuleStates = - DriveConstants.kDriveKinematics.toSwerveModuleStates( - ChassisSpeeds.discretize( - fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds( - xSpeed, ySpeed, rot, m_gyro.getRotation2d()) - : new ChassisSpeeds(xSpeed, ySpeed, rot), - DriveConstants.kDrivePeriod)); - SwerveDriveKinematics.desaturateWheelSpeeds( - swerveModuleStates, DriveConstants.kMaxSpeedMetersPerSecond); - m_frontLeft.setDesiredState(swerveModuleStates[0]); - m_frontRight.setDesiredState(swerveModuleStates[1]); - m_rearLeft.setDesiredState(swerveModuleStates[2]); - m_rearRight.setDesiredState(swerveModuleStates[3]); + var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot); + if (fieldRelative) { + chassisSpeeds = chassisSpeeds.toRobotRelative(m_gyro.getRotation2d()); + } + chassisSpeeds = chassisSpeeds.discretize(DriveConstants.kDrivePeriod); + + var states = DriveConstants.kDriveKinematics.toWheelSpeeds(chassisSpeeds); + SwerveDriveKinematics.desaturateWheelSpeeds(states, DriveConstants.kMaxSpeedMetersPerSecond); + + m_frontLeft.setDesiredState(states[0]); + m_frontRight.setDesiredState(states[1]); + m_rearLeft.setDesiredState(states[2]); + m_rearRight.setDesiredState(states[3]); } /** diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java index c232bb19a7..f754f08bed 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java @@ -66,19 +66,21 @@ public class Drivetrain { */ public void drive( double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) { - var swerveModuleStates = - m_kinematics.toSwerveModuleStates( - ChassisSpeeds.discretize( - fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds( - xSpeed, ySpeed, rot, m_poseEstimator.getEstimatedPosition().getRotation()) - : new ChassisSpeeds(xSpeed, ySpeed, rot), - periodSeconds)); - SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, kMaxSpeed); - m_frontLeft.setDesiredState(swerveModuleStates[0]); - m_frontRight.setDesiredState(swerveModuleStates[1]); - m_backLeft.setDesiredState(swerveModuleStates[2]); - m_backRight.setDesiredState(swerveModuleStates[3]); + var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot); + if (fieldRelative) { + chassisSpeeds = + chassisSpeeds.toRobotRelative(m_poseEstimator.getEstimatedPosition().getRotation()); + } + + chassisSpeeds = chassisSpeeds.discretize(periodSeconds); + + var states = m_kinematics.toWheelSpeeds(chassisSpeeds); + SwerveDriveKinematics.desaturateWheelSpeeds(states, kMaxSpeed); + + m_frontLeft.setDesiredState(states[0]); + m_frontRight.setDesiredState(states[1]); + m_backLeft.setDesiredState(states[2]); + m_backRight.setDesiredState(states[3]); } /** Updates the field relative position of the robot. */ diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java index a243a52d85..cc5b1438c5 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java @@ -104,7 +104,7 @@ public class HolonomicDriveController { m_rotationError = desiredHeading.minus(currentPose.getRotation()); if (!m_enabled) { - return ChassisSpeeds.fromFieldRelativeSpeeds(xFF, yFF, thetaFF, currentPose.getRotation()); + return new ChassisSpeeds(xFF, yFF, thetaFF).toRobotRelative(currentPose.getRotation()); } // Calculate feedback velocities (based on position error). @@ -112,8 +112,8 @@ public class HolonomicDriveController { double yFeedback = m_yController.calculate(currentPose.getY(), trajectoryPose.getY()); // Return next output. - return ChassisSpeeds.fromFieldRelativeSpeeds( - xFF + xFeedback, yFF + yFeedback, thetaFF, currentPose.getRotation()); + return new ChassisSpeeds(xFF + xFeedback, yFF + yFeedback, thetaFF) + .toRobotRelative(currentPose.getRotation()); } /** diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java index 4c806a52cb..de6c5fa767 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java @@ -6,7 +6,6 @@ package edu.wpi.first.math.kinematics; import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.Seconds; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -16,7 +15,6 @@ import edu.wpi.first.math.kinematics.proto.ChassisSpeedsProto; import edu.wpi.first.math.kinematics.struct.ChassisSpeedsStruct; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.LinearVelocity; -import edu.wpi.first.units.measure.Time; import edu.wpi.first.util.protobuf.ProtobufSerializable; import edu.wpi.first.util.struct.StructSerializable; import java.util.Objects; @@ -90,7 +88,7 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable { /** * Discretizes a continuous-time chassis speed. * - *

This function converts a continuous-time chassis speed into a discrete-time one such that + *

This function converts this continuous-time chassis speed into a discrete-time one such that * when the discrete-time chassis speed is applied for one timestep, the robot moves as if the * velocity components are independent (i.e., the robot moves v_x * dt along the x-axis, v_y * dt * along the y-axis, and omega * dt around the z-axis). @@ -98,19 +96,10 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable { *

This is useful for compensating for translational skew when translating and rotating a * swerve drivetrain. * - * @param vxMetersPerSecond Forward velocity. - * @param vyMetersPerSecond Sideways velocity. - * @param omegaRadiansPerSecond Angular velocity. * @param dtSeconds The duration of the timestep the speeds should be applied for. * @return Discretized ChassisSpeeds. */ - public static ChassisSpeeds discretize( - double vxMetersPerSecond, - double vyMetersPerSecond, - double omegaRadiansPerSecond, - double dtSeconds) { - // Construct the desired pose after a timestep, relative to the current pose. The desired pose - // has decoupled translation and rotation. + public ChassisSpeeds discretize(double dtSeconds) { var desiredDeltaPose = new Pose2d( vxMetersPerSecond * dtSeconds, @@ -126,70 +115,14 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable { } /** - * Discretizes a continuous-time chassis speed. + * Converts this field-relative set of speeds into a robot-relative ChassisSpeeds object. * - *

This function converts a continuous-time chassis speed into a discrete-time one such that - * when the discrete-time chassis speed is applied for one timestep, the robot moves as if the - * velocity components are independent (i.e., the robot moves v_x * dt along the x-axis, v_y * dt - * along the y-axis, and omega * dt around the z-axis). - * - *

This is useful for compensating for translational skew when translating and rotating a - * swerve drivetrain. - * - * @param vx Forward velocity. - * @param vy Sideways velocity. - * @param omega Angular velocity. - * @param dt The duration of the timestep the speeds should be applied for. - * @return Discretized ChassisSpeeds. - */ - public static ChassisSpeeds discretize( - LinearVelocity vx, LinearVelocity vy, AngularVelocity omega, Time dt) { - return discretize( - vx.in(MetersPerSecond), vy.in(MetersPerSecond), omega.in(RadiansPerSecond), dt.in(Seconds)); - } - - /** - * Discretizes a continuous-time chassis speed. - * - *

This function converts a continuous-time chassis speed into a discrete-time one such that - * when the discrete-time chassis speed is applied for one timestep, the robot moves as if the - * velocity components are independent (i.e., the robot moves v_x * dt along the x-axis, v_y * dt - * along the y-axis, and omega * dt around the z-axis). - * - *

This is useful for compensating for translational skew when translating and rotating a - * swerve drivetrain. - * - * @param continuousSpeeds The continuous speeds. - * @param dtSeconds The duration of the timestep the speeds should be applied for. - * @return Discretized ChassisSpeeds. - */ - public static ChassisSpeeds discretize(ChassisSpeeds continuousSpeeds, double dtSeconds) { - return discretize( - continuousSpeeds.vxMetersPerSecond, - continuousSpeeds.vyMetersPerSecond, - continuousSpeeds.omegaRadiansPerSecond, - dtSeconds); - } - - /** - * Converts a user provided field-relative set of speeds into a robot-relative ChassisSpeeds - * object. - * - * @param vxMetersPerSecond The component of speed in the x direction relative to the field. - * Positive x is away from your alliance wall. - * @param vyMetersPerSecond The component of speed in the y direction relative to the field. - * Positive y is to your left when standing behind the alliance wall. - * @param omegaRadiansPerSecond The angular rate of the robot. * @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is * considered to be zero when it is facing directly away from your alliance station wall. * Remember that this should be CCW positive. * @return ChassisSpeeds object representing the speeds in the robot's frame of reference. */ - public static ChassisSpeeds fromFieldRelativeSpeeds( - double vxMetersPerSecond, - double vyMetersPerSecond, - double omegaRadiansPerSecond, - Rotation2d robotAngle) { + public ChassisSpeeds toRobotRelative(Rotation2d robotAngle) { // CW rotation into chassis frame var rotated = new Translation2d(vxMetersPerSecond, vyMetersPerSecond).rotateBy(robotAngle.unaryMinus()); @@ -197,111 +130,19 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable { } /** - * Converts a user provided field-relative set of speeds into a robot-relative ChassisSpeeds - * object. + * Converts this robot-relative set of speeds into a field-relative ChassisSpeeds object. * - * @param vx The component of speed in the x direction relative to the field. Positive x is away - * from your alliance wall. - * @param vy The component of speed in the y direction relative to the field. Positive y is to - * your left when standing behind the alliance wall. - * @param omega The angular rate of the robot. - * @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is - * considered to be zero when it is facing directly away from your alliance station wall. - * Remember that this should be CCW positive. - * @return ChassisSpeeds object representing the speeds in the robot's frame of reference. - */ - public static ChassisSpeeds fromFieldRelativeSpeeds( - LinearVelocity vx, LinearVelocity vy, AngularVelocity omega, Rotation2d robotAngle) { - return fromFieldRelativeSpeeds( - vx.in(MetersPerSecond), vy.in(MetersPerSecond), omega.in(RadiansPerSecond), robotAngle); - } - - /** - * Converts a user provided field-relative ChassisSpeeds object into a robot-relative - * ChassisSpeeds object. - * - * @param fieldRelativeSpeeds The ChassisSpeeds object representing the speeds in the field frame - * of reference. Positive x is away from your alliance wall. Positive y is to your left when - * standing behind the alliance wall. - * @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is - * considered to be zero when it is facing directly away from your alliance station wall. - * Remember that this should be CCW positive. - * @return ChassisSpeeds object representing the speeds in the robot's frame of reference. - */ - public static ChassisSpeeds fromFieldRelativeSpeeds( - ChassisSpeeds fieldRelativeSpeeds, Rotation2d robotAngle) { - return fromFieldRelativeSpeeds( - fieldRelativeSpeeds.vxMetersPerSecond, - fieldRelativeSpeeds.vyMetersPerSecond, - fieldRelativeSpeeds.omegaRadiansPerSecond, - robotAngle); - } - - /** - * Converts a user provided robot-relative set of speeds into a field-relative ChassisSpeeds - * object. - * - * @param vxMetersPerSecond The component of speed in the x direction relative to the robot. - * Positive x is towards the robot's front. - * @param vyMetersPerSecond The component of speed in the y direction relative to the robot. - * Positive y is towards the robot's left. - * @param omegaRadiansPerSecond The angular rate of the robot. * @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is * considered to be zero when it is facing directly away from your alliance station wall. * Remember that this should be CCW positive. * @return ChassisSpeeds object representing the speeds in the field's frame of reference. */ - public static ChassisSpeeds fromRobotRelativeSpeeds( - double vxMetersPerSecond, - double vyMetersPerSecond, - double omegaRadiansPerSecond, - Rotation2d robotAngle) { + public ChassisSpeeds toFieldRelative(Rotation2d robotAngle) { // CCW rotation out of chassis frame var rotated = new Translation2d(vxMetersPerSecond, vyMetersPerSecond).rotateBy(robotAngle); return new ChassisSpeeds(rotated.getX(), rotated.getY(), omegaRadiansPerSecond); } - /** - * Converts a user provided robot-relative set of speeds into a field-relative ChassisSpeeds - * object. - * - * @param vx The component of speed in the x direction relative to the robot. Positive x is - * towards the robot's front. - * @param vy The component of speed in the y direction relative to the robot. Positive y is - * towards the robot's left. - * @param omega The angular rate of the robot. - * @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is - * considered to be zero when it is facing directly away from your alliance station wall. - * Remember that this should be CCW positive. - * @return ChassisSpeeds object representing the speeds in the field's frame of reference. - */ - public static ChassisSpeeds fromRobotRelativeSpeeds( - LinearVelocity vx, LinearVelocity vy, AngularVelocity omega, Rotation2d robotAngle) { - return fromRobotRelativeSpeeds( - vx.in(MetersPerSecond), vy.in(MetersPerSecond), omega.in(RadiansPerSecond), robotAngle); - } - - /** - * Converts a user provided robot-relative ChassisSpeeds object into a field-relative - * ChassisSpeeds object. - * - * @param robotRelativeSpeeds The ChassisSpeeds object representing the speeds in the robot frame - * of reference. Positive x is towards the robot's front. Positive y is towards the robot's - * left. - * @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is - * considered to be zero when it is facing directly away from your alliance station wall. - * Remember that this should be CCW positive. - * @return ChassisSpeeds object representing the speeds in the field's frame of reference. - */ - public static ChassisSpeeds fromRobotRelativeSpeeds( - ChassisSpeeds robotRelativeSpeeds, Rotation2d robotAngle) { - return fromRobotRelativeSpeeds( - robotRelativeSpeeds.vxMetersPerSecond, - robotRelativeSpeeds.vyMetersPerSecond, - robotRelativeSpeeds.omegaRadiansPerSecond, - robotAngle); - } - /** * Adds two ChassisSpeeds and returns the sum. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java index 4240b9f539..e85adf8fd6 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java @@ -83,23 +83,27 @@ public class MecanumDriveWheelSpeeds implements ProtobufSerializable, StructSeri * absolute threshold, while maintaining the ratio of speeds between wheels. * * @param attainableMaxSpeedMetersPerSecond The absolute max speed that a wheel can reach. + * @return Desaturated MecanumDriveWheelSpeeds. */ - public void desaturate(double attainableMaxSpeedMetersPerSecond) { + public MecanumDriveWheelSpeeds desaturate(double attainableMaxSpeedMetersPerSecond) { double realMaxSpeed = Math.max(Math.abs(frontLeftMetersPerSecond), Math.abs(frontRightMetersPerSecond)); realMaxSpeed = Math.max(realMaxSpeed, Math.abs(rearLeftMetersPerSecond)); realMaxSpeed = Math.max(realMaxSpeed, Math.abs(rearRightMetersPerSecond)); if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) { - frontLeftMetersPerSecond = - frontLeftMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond; - frontRightMetersPerSecond = - frontRightMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond; - rearLeftMetersPerSecond = - rearLeftMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond; - rearRightMetersPerSecond = - rearRightMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond; + return new MecanumDriveWheelSpeeds( + frontLeftMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond, + frontRightMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond, + rearLeftMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond, + rearRightMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond); } + + return new MecanumDriveWheelSpeeds( + frontLeftMetersPerSecond, + frontRightMetersPerSecond, + rearLeftMetersPerSecond, + rearRightMetersPerSecond); } /** @@ -111,9 +115,10 @@ public class MecanumDriveWheelSpeeds implements ProtobufSerializable, StructSeri * absolute threshold, while maintaining the ratio of speeds between wheels. * * @param attainableMaxSpeed The absolute max speed that a wheel can reach. + * @return Desaturated MecanumDriveWheelSpeeds. */ - public void desaturate(LinearVelocity attainableMaxSpeed) { - desaturate(attainableMaxSpeed.in(MetersPerSecond)); + public MecanumDriveWheelSpeeds desaturate(LinearVelocity attainableMaxSpeed) { + return desaturate(attainableMaxSpeed.in(MetersPerSecond)); } /** diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/MecanumDriveKinematicsConstraint.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/MecanumDriveKinematicsConstraint.java index dab3699650..35224cf395 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/MecanumDriveKinematicsConstraint.java +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/MecanumDriveKinematicsConstraint.java @@ -52,8 +52,8 @@ public class MecanumDriveKinematicsConstraint implements TrajectoryConstraint { new ChassisSpeeds(xdVelocity, ydVelocity, velocityMetersPerSecond * curvatureRadPerMeter); // Get the wheel speeds and normalize them to within the max velocity. - var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds); - wheelSpeeds.desaturate(m_maxSpeedMetersPerSecond); + var wheelSpeeds = + m_kinematics.toWheelSpeeds(chassisSpeeds).desaturate(m_maxSpeedMetersPerSecond); // Convert normalized wheel speeds back to chassis speeds var normSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds); diff --git a/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h b/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h index a78fdd85c1..2bf42cc093 100644 --- a/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h +++ b/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h @@ -115,8 +115,8 @@ class WPILIB_DLLEXPORT HolonomicDriveController { m_rotationError = desiredHeading - currentPose.Rotation(); if (!m_enabled) { - return ChassisSpeeds::FromFieldRelativeSpeeds(xFF, yFF, thetaFF, - currentPose.Rotation()); + return ChassisSpeeds{xFF, yFF, thetaFF}.ToRobotRelative( + currentPose.Rotation()); } // Calculate feedback velocities (based on position error). @@ -126,8 +126,8 @@ class WPILIB_DLLEXPORT HolonomicDriveController { currentPose.Y().value(), trajectoryPose.Y().value())}; // Return next output. - return ChassisSpeeds::FromFieldRelativeSpeeds( - xFF + xFeedback, yFF + yFeedback, thetaFF, currentPose.Rotation()); + return ChassisSpeeds{xFF + xFeedback, yFF + yFeedback, thetaFF} + .ToRobotRelative(currentPose.Rotation()); } /** diff --git a/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h b/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h index 396b33a618..9c3a3c88df 100644 --- a/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h +++ b/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h @@ -61,17 +61,10 @@ struct WPILIB_DLLEXPORT ChassisSpeeds { * This is useful for compensating for translational skew when translating and * rotating a swerve drivetrain. * - * @param vx Forward velocity. - * @param vy Sideways velocity. - * @param omega Angular velocity. * @param dt The duration of the timestep the speeds should be applied for. - * * @return Discretized ChassisSpeeds. */ - static constexpr ChassisSpeeds Discretize(units::meters_per_second_t vx, - units::meters_per_second_t vy, - units::radians_per_second_t omega, - units::second_t dt) { + constexpr ChassisSpeeds Discretize(units::second_t dt) const { // Construct the desired pose after a timestep, relative to the current // pose. The desired pose has decoupled translation and rotation. Pose2d desiredDeltaPose{vx * dt, vy * dt, omega * dt}; @@ -85,47 +78,17 @@ struct WPILIB_DLLEXPORT ChassisSpeeds { } /** - * Discretizes a continuous-time chassis speed. - * - * This function converts a continuous-time chassis speed into a discrete-time - * one such that when the discrete-time chassis speed is applied for one - * timestep, the robot moves as if the velocity components are independent - * (i.e., the robot moves v_x * dt along the x-axis, v_y * dt along the - * y-axis, and omega * dt around the z-axis). - * - * This is useful for compensating for translational skew when translating and - * rotating a swerve drivetrain. - * - * @param continuousSpeeds The continuous speeds. - * @param dt The duration of the timestep the speeds should be applied for. - * - * @return Discretized ChassisSpeeds. - */ - static constexpr ChassisSpeeds Discretize( - const ChassisSpeeds& continuousSpeeds, units::second_t dt) { - return Discretize(continuousSpeeds.vx, continuousSpeeds.vy, - continuousSpeeds.omega, dt); - } - - /** - * Converts a user provided field-relative set of speeds into a robot-relative + * Converts this field-relative set of speeds into a robot-relative * ChassisSpeeds object. * - * @param vx The component of speed in the x direction relative to the field. - * Positive x is away from your alliance wall. - * @param vy The component of speed in the y direction relative to the field. - * Positive y is to your left when standing behind the alliance wall. - * @param omega The angular rate of the robot. * @param robotAngle The angle of the robot as measured by a gyroscope. The - * robot's angle is considered to be zero when it is facing directly away from - * your alliance station wall. Remember that this should be CCW positive. - * + * robot's angle is considered to be zero when it is facing directly away + * from your alliance station wall. Remember that this should be CCW + * positive. * @return ChassisSpeeds object representing the speeds in the robot's frame - * of reference. + * of reference. */ - static constexpr ChassisSpeeds FromFieldRelativeSpeeds( - units::meters_per_second_t vx, units::meters_per_second_t vy, - units::radians_per_second_t omega, const Rotation2d& robotAngle) { + constexpr ChassisSpeeds ToRobotRelative(const Rotation2d& robotAngle) const { // CW rotation into chassis frame auto rotated = Translation2d{units::meter_t{vx.value()}, units::meter_t{vy.value()}} @@ -135,45 +98,17 @@ struct WPILIB_DLLEXPORT ChassisSpeeds { } /** - * Converts a user provided field-relative ChassisSpeeds object into a - * robot-relative ChassisSpeeds object. - * - * @param fieldRelativeSpeeds The ChassisSpeeds object representing the speeds - * in the field frame of reference. Positive x is away from your alliance - * wall. Positive y is to your left when standing behind the alliance wall. - * @param robotAngle The angle of the robot as measured by a gyroscope. The - * robot's angle is considered to be zero when it is facing directly away - * from your alliance station wall. Remember that this should be CCW - * positive. - * @return ChassisSpeeds object representing the speeds in the robot's frame - * of reference. - */ - static constexpr ChassisSpeeds FromFieldRelativeSpeeds( - const ChassisSpeeds& fieldRelativeSpeeds, const Rotation2d& robotAngle) { - return FromFieldRelativeSpeeds(fieldRelativeSpeeds.vx, - fieldRelativeSpeeds.vy, - fieldRelativeSpeeds.omega, robotAngle); - } - - /** - * Converts a user provided robot-relative set of speeds into a field-relative + * Converts this robot-relative set of speeds into a field-relative * ChassisSpeeds object. * - * @param vx The component of speed in the x direction relative to the robot. - * Positive x is towards the robot's front. - * @param vy The component of speed in the y direction relative to the robot. - * Positive y is towards the robot's left. - * @param omega The angular rate of the robot. * @param robotAngle The angle of the robot as measured by a gyroscope. The - * robot's angle is considered to be zero when it is facing directly away from - * your alliance station wall. Remember that this should be CCW positive. - * + * robot's angle is considered to be zero when it is facing directly away + * from your alliance station wall. Remember that this should be CCW + * positive. * @return ChassisSpeeds object representing the speeds in the field's frame - * of reference. + * of reference. */ - static constexpr ChassisSpeeds FromRobotRelativeSpeeds( - units::meters_per_second_t vx, units::meters_per_second_t vy, - units::radians_per_second_t omega, const Rotation2d& robotAngle) { + constexpr ChassisSpeeds ToFieldRelative(const Rotation2d& robotAngle) const { // CCW rotation out of chassis frame auto rotated = Translation2d{units::meter_t{vx.value()}, units::meter_t{vy.value()}} @@ -182,27 +117,6 @@ struct WPILIB_DLLEXPORT ChassisSpeeds { units::meters_per_second_t{rotated.Y().value()}, omega}; } - /** - * Converts a user provided robot-relative ChassisSpeeds object into a - * field-relative ChassisSpeeds object. - * - * @param robotRelativeSpeeds The ChassisSpeeds object representing the speeds - * in the robot frame of reference. Positive x is the towards robot's - * front. Positive y is towards the robot's left. - * @param robotAngle The angle of the robot as measured by a gyroscope. The - * robot's angle is considered to be zero when it is facing directly away - * from your alliance station wall. Remember that this should be CCW - * positive. - * @return ChassisSpeeds object representing the speeds in the field's frame - * of reference. - */ - static constexpr ChassisSpeeds FromRobotRelativeSpeeds( - const ChassisSpeeds& robotRelativeSpeeds, const Rotation2d& robotAngle) { - return FromRobotRelativeSpeeds(robotRelativeSpeeds.vx, - robotRelativeSpeeds.vy, - robotRelativeSpeeds.omega, robotAngle); - } - /** * Adds two ChassisSpeeds and returns the sum. * diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h index 62208e5d50..df923483d0 100644 --- a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h +++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h @@ -49,8 +49,10 @@ struct WPILIB_DLLEXPORT MecanumDriveWheelSpeeds { * threshold, while maintaining the ratio of speeds between wheels. * * @param attainableMaxSpeed The absolute max speed that a wheel can reach. + * @return Desaturated MecanumDriveWheelSpeeds. */ - constexpr void Desaturate(units::meters_per_second_t attainableMaxSpeed) { + constexpr MecanumDriveWheelSpeeds Desaturate( + units::meters_per_second_t attainableMaxSpeed) const { std::array wheelSpeeds{frontLeft, frontRight, rearLeft, rearRight}; units::meters_per_second_t realMaxSpeed = units::math::abs( @@ -63,11 +65,11 @@ struct WPILIB_DLLEXPORT MecanumDriveWheelSpeeds { for (int i = 0; i < 4; ++i) { wheelSpeeds[i] = wheelSpeeds[i] / realMaxSpeed * attainableMaxSpeed; } - frontLeft = wheelSpeeds[0]; - frontRight = wheelSpeeds[1]; - rearLeft = wheelSpeeds[2]; - rearRight = wheelSpeeds[3]; + return MecanumDriveWheelSpeeds{wheelSpeeds[0], wheelSpeeds[1], + wheelSpeeds[2], wheelSpeeds[3]}; } + + return *this; } /** diff --git a/wpimath/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h index 572d478877..fc7b7526f9 100644 --- a/wpimath/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h +++ b/wpimath/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h @@ -32,9 +32,9 @@ class WPILIB_DLLEXPORT MecanumDriveKinematicsConstraint units::meters_per_second_t velocity) const override { auto xVelocity = velocity * pose.Rotation().Cos(); auto yVelocity = velocity * pose.Rotation().Sin(); - auto wheelSpeeds = m_kinematics.ToWheelSpeeds( - {xVelocity, yVelocity, velocity * curvature}); - wheelSpeeds.Desaturate(m_maxSpeed); + auto wheelSpeeds = + m_kinematics.ToWheelSpeeds({xVelocity, yVelocity, velocity * curvature}) + .Desaturate(m_maxSpeed); auto normSpeeds = m_kinematics.ToChassisSpeeds(wheelSpeeds); diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java index 8f7bb6fd12..361f42520d 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java @@ -23,7 +23,7 @@ class ChassisSpeedsTest { final var duration = 1.0; final var dt = 0.01; - final var speeds = ChassisSpeeds.discretize(target, duration); + final var speeds = target.discretize(duration); final var twist = new Twist2d( speeds.vxMetersPerSecond * dt, @@ -60,9 +60,8 @@ class ChassisSpeedsTest { } @Test - void testFromFieldRelativeSpeeds() { - final var chassisSpeeds = - ChassisSpeeds.fromFieldRelativeSpeeds(1.0, 0.0, 0.5, Rotation2d.kCW_Pi_2); + void testToRobotRelative() { + final var chassisSpeeds = new ChassisSpeeds(1.0, 0.0, 0.5).toRobotRelative(Rotation2d.kCW_Pi_2); assertAll( () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon), @@ -71,9 +70,9 @@ class ChassisSpeedsTest { } @Test - void testFromRobotRelativeSpeeds() { + void testToFieldRelative() { final var chassisSpeeds = - ChassisSpeeds.fromRobotRelativeSpeeds(1.0, 0.0, 0.5, Rotation2d.fromDegrees(45.0)); + new ChassisSpeeds(1.0, 0.0, 0.5).toFieldRelative(Rotation2d.fromDegrees(45.0)); assertAll( () -> assertEquals(1.0 / Math.sqrt(2.0), chassisSpeeds.vxMetersPerSecond, kEpsilon), diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java index 28fbfd8e83..ad0214072f 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java @@ -227,8 +227,7 @@ class MecanumDriveKinematicsTest { @Test void testDesaturate() { - var wheelSpeeds = new MecanumDriveWheelSpeeds(5, 6, 4, 7); - wheelSpeeds.desaturate(5.5); + var wheelSpeeds = new MecanumDriveWheelSpeeds(5, 6, 4, 7).desaturate(5.5); double factor = 5.5 / 7.0; @@ -241,8 +240,7 @@ class MecanumDriveKinematicsTest { @Test void testDesaturateNegativeSpeeds() { - var wheelSpeeds = new MecanumDriveWheelSpeeds(-5, 6, 4, -7); - wheelSpeeds.desaturate(5.5); + var wheelSpeeds = new MecanumDriveWheelSpeeds(-5, 6, 4, -7).desaturate(5.5); final double kFactor = 5.5 / 7.0; diff --git a/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp b/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp index 97de03bcf6..993ae40654 100644 --- a/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp @@ -15,7 +15,7 @@ TEST(ChassisSpeedsTest, Discretize) { constexpr units::second_t duration = 1_s; constexpr units::second_t dt = 10_ms; - const auto speeds = frc::ChassisSpeeds::Discretize(target, duration); + const auto speeds = target.Discretize(duration); const frc::Twist2d twist{speeds.vx * dt, speeds.vy * dt, speeds.omega * dt}; frc::Pose2d pose; @@ -29,18 +29,19 @@ TEST(ChassisSpeedsTest, Discretize) { pose.Rotation().Radians().value(), kEpsilon); } -TEST(ChassisSpeedsTest, FromFieldRelativeSpeeds) { - const auto chassisSpeeds = frc::ChassisSpeeds::FromFieldRelativeSpeeds( - 1.0_mps, 0.0_mps, 0.5_rad_per_s, -90.0_deg); +TEST(ChassisSpeedsTest, ToRobotRelative) { + const auto chassisSpeeds = + frc::ChassisSpeeds{1_mps, 0_mps, 0.5_rad_per_s}.ToRobotRelative( + -90.0_deg); EXPECT_NEAR(0.0, chassisSpeeds.vx.value(), kEpsilon); EXPECT_NEAR(1.0, chassisSpeeds.vy.value(), kEpsilon); EXPECT_NEAR(0.5, chassisSpeeds.omega.value(), kEpsilon); } -TEST(ChassisSpeedsTest, FromRobotRelativeSpeeds) { - const auto chassisSpeeds = frc::ChassisSpeeds::FromRobotRelativeSpeeds( - 1.0_mps, 0.0_mps, 0.5_rad_per_s, 45.0_deg); +TEST(ChassisSpeedsTest, ToFieldRelative) { + const auto chassisSpeeds = + frc::ChassisSpeeds{1_mps, 0_mps, 0.5_rad_per_s}.ToFieldRelative(45.0_deg); EXPECT_NEAR(1.0 / std::sqrt(2.0), chassisSpeeds.vx.value(), kEpsilon); EXPECT_NEAR(1.0 / std::sqrt(2.0), chassisSpeeds.vy.value(), kEpsilon); diff --git a/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp index 3fe1471222..38496ba710 100644 --- a/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp @@ -205,8 +205,8 @@ TEST_F(MecanumDriveKinematicsTest, } TEST_F(MecanumDriveKinematicsTest, Desaturate) { - MecanumDriveWheelSpeeds wheelSpeeds{5_mps, 6_mps, 4_mps, 7_mps}; - wheelSpeeds.Desaturate(5.5_mps); + auto wheelSpeeds = + MecanumDriveWheelSpeeds{5_mps, 6_mps, 4_mps, 7_mps}.Desaturate(5.5_mps); double kFactor = 5.5 / 7.0; @@ -217,8 +217,8 @@ TEST_F(MecanumDriveKinematicsTest, Desaturate) { } TEST_F(MecanumDriveKinematicsTest, DesaturateNegativeSpeeds) { - MecanumDriveWheelSpeeds wheelSpeeds{-5_mps, 6_mps, 4_mps, -7_mps}; - wheelSpeeds.Desaturate(5.5_mps); + auto wheelSpeeds = + MecanumDriveWheelSpeeds{-5_mps, 6_mps, 4_mps, -7_mps}.Desaturate(5.5_mps); constexpr double kFactor = 5.5 / 7.0;