[wpimath] Use immutable member functions in ChassisSpeeds

wpilibsuite · Dec 13, 2024 · f67a7b1 · f67a7b1
1 parent 6ba7189
commit f67a7b1
Show file tree

Hide file tree

Showing 24 changed files with 149 additions and 466 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
@@ -354,7 +354,7 @@ public void execute() {
         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
@@ -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
@@ -4,6 +4,8 @@
 
 #include "Drivetrain.h"
 
+#include <frc/kinematics/ChassisSpeeds.h>
+
 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
@@ -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
@@ -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/...cExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp b/...cExamples/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
@@ -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
@@ -131,12 +131,10 @@ public void drive(
       double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) {
     var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
     if (fieldRelative) {
-      chassisSpeeds.toRobotRelativeSpeeds(m_gyro.getRotation2d());
+      chassisSpeeds = chassisSpeeds.toRobotRelative(m_gyro.getRotation2d());
     }
-    chassisSpeeds.discretize(periodSeconds);
-    var mecanumDriveWheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
-    mecanumDriveWheelSpeeds.desaturate(kMaxSpeed);
-    setSpeeds(mecanumDriveWheelSpeeds);
+    setSpeeds(
+        m_kinematics.toWheelSpeeds(chassisSpeeds.discretize(periodSeconds)).desaturate(kMaxSpeed));
   }
 
   /** Updates the field relative position of the robot. */

diff --git a/...es/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java b/...es/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java
@@ -143,12 +143,11 @@ public void drive(
       double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) {
     var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
     if (fieldRelative) {
-      chassisSpeeds.toRobotRelativeSpeeds(m_poseEstimator.getEstimatedPosition().getRotation());
+      chassisSpeeds =
+          chassisSpeeds.toRobotRelative(m_poseEstimator.getEstimatedPosition().getRotation());
     }
-    chassisSpeeds.discretize(periodSeconds);
-    var mecanumDriveWheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
-    mecanumDriveWheelSpeeds.desaturate(kMaxSpeed);
-    setSpeeds(mecanumDriveWheelSpeeds);
+    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
@@ -59,15 +59,17 @@ public void drive(
       double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) {
     var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
     if (fieldRelative) {
-      chassisSpeeds.toRobotRelativeSpeeds(m_gyro.getRotation2d());
+      chassisSpeeds = chassisSpeeds.toRobotRelative(m_gyro.getRotation2d());
     }
-    chassisSpeeds.discretize(periodSeconds);
-    var swerveModuleStates = m_kinematics.toWheelSpeeds(chassisSpeeds);
-    SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, kMaxSpeed);
-    m_frontLeft.setDesiredState(swerveModuleStates[0]);
-    m_frontRight.setDesiredState(swerveModuleStates[1]);
-    m_backLeft.setDesiredState(swerveModuleStates[2]);
-    m_backRight.setDesiredState(swerveModuleStates[3]);
+    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/...ava/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java b/...ava/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java
@@ -120,16 +120,17 @@ public void resetOdometry(Pose2d pose) {
   public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
     var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
     if (fieldRelative) {
-      chassisSpeeds.toRobotRelativeSpeeds(m_gyro.getRotation2d());
+      chassisSpeeds = chassisSpeeds.toRobotRelative(m_gyro.getRotation2d());
     }
-    chassisSpeeds.discretize(DriveConstants.kDrivePeriod);
-    var swerveModuleStates = DriveConstants.kDriveKinematics.toWheelSpeeds(chassisSpeeds);
-    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]);
+    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/...les/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java b/...les/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java
@@ -71,15 +71,19 @@ public void drive(
       double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) {
     var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
     if (fieldRelative) {
-      chassisSpeeds.toRobotRelativeSpeeds(m_poseEstimator.getEstimatedPosition().getRotation());
+      chassisSpeeds =
+          chassisSpeeds.toRobotRelative(m_poseEstimator.getEstimatedPosition().getRotation());
     }
-    chassisSpeeds.discretize(periodSeconds);
-    var swerveModuleStates = m_kinematics.toWheelSpeeds(chassisSpeeds);
-    SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, kMaxSpeed);
-    m_frontLeft.setDesiredState(swerveModuleStates[0]);
-    m_frontRight.setDesiredState(swerveModuleStates[1]);
-    m_backLeft.setDesiredState(swerveModuleStates[2]);
-    m_backRight.setDesiredState(swerveModuleStates[3]);
+
+    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
@@ -104,7 +104,7 @@ public ChassisSpeeds calculate(
     m_rotationError = desiredHeading.minus(currentPose.getRotation());
     ChassisSpeeds speeds = new ChassisSpeeds(xFF, yFF, thetaFF);
     if (!m_enabled) {
-      speeds.toRobotRelativeSpeeds(currentPose.getRotation());
+      speeds = speeds.toRobotRelative(currentPose.getRotation());
       return speeds;
     }
 
@@ -115,7 +115,7 @@ public ChassisSpeeds calculate(
     // Return next output.
     speeds.vxMetersPerSecond += xFeedback;
     speeds.vyMetersPerSecond += yFeedback;
-    speeds.toRobotRelativeSpeeds(currentPose.getRotation());
+    speeds = speeds.toRobotRelative(currentPose.getRotation());
     return speeds;
   }