Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Make swerve and differential kinematics functions immutable (#8274)

Browse Source 
Originally started with just swerve, but expanded to diff and mecanum
(docs only) for parity across the drivetrains. Return value checks are
applied when possible to make migration easier and to error loudly if
people forget.

---------

Co-authored-by: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com>
This commit is contained in:
Gold856
2026-04-17 00:23:32 -04:00
committed by GitHub
parent 8c80cdcf28
commit 056d7bcbbe
34 changed files with 396 additions and 394 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
wpilibjExamples/src/main/java/org/wpilib/examples/swervebot/Drivetrain.java
View File

@@ -63,13 +63,14 @@ public class Drivetrain {
}
chassisVelocities = chassisVelocities.discretize(period);
var states = m_kinematics.toWheelVelocities(chassisVelocities);
SwerveDriveKinematics.desaturateWheelVelocities(states, kMaxVelocity);
var velocities =
SwerveDriveKinematics.desaturateWheelVelocities(
m_kinematics.toWheelVelocities(chassisVelocities), kMaxVelocity);
m_frontLeft.setDesiredVelocity(states[0]);
m_frontRight.setDesiredVelocity(states[1]);
m_backLeft.setDesiredVelocity(states[2]);
m_backRight.setDesiredVelocity(states[3]);
m_frontLeft.setDesiredVelocity(velocities[0]);
m_frontRight.setDesiredVelocity(velocities[1]);
m_backLeft.setDesiredVelocity(velocities[2]);
m_backRight.setDesiredVelocity(velocities[3]);
}
/** Updates the field relative position of the robot. */

19
wpilibjExamples/src/main/java/org/wpilib/examples/swervebot/SwerveModule.java
View File

@@ -110,26 +110,23 @@ public class SwerveModule {
public void setDesiredVelocity(SwerveModuleVelocity desiredVelocity) {
var encoderRotation = new Rotation2d(m_turningEncoder.getDistance());
// Optimize the desired velocity to avoid spinning further than 90 degrees
desiredVelocity.optimize(encoderRotation);
// Scale velocity 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.
desiredVelocity.cosineScale(encoderRotation);
// Optimize the desired velocity to avoid spinning further than 90 degrees, then scale velocity
// 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.
SwerveModuleVelocity velocity =
desiredVelocity.optimize(encoderRotation).cosineScale(encoderRotation);
// Calculate the drive output from the drive PID controller.
final double driveOutput =
m_drivePIDController.calculate(m_driveEncoder.getRate(), desiredVelocity.velocity);
m_drivePIDController.calculate(m_driveEncoder.getRate(), velocity.velocity);
final double driveFeedforward = m_driveFeedforward.calculate(desiredVelocity.velocity);
final double driveFeedforward = m_driveFeedforward.calculate(velocity.velocity);
// Calculate the turning motor output from the turning PID controller.
final double turnOutput =
m_turningPIDController.calculate(
m_turningEncoder.getDistance(), desiredVelocity.angle.getRadians());
m_turningEncoder.getDistance(), velocity.angle.getRadians());
final double turnFeedforward =
m_turnFeedforward.calculate(m_turningPIDController.getSetpoint().velocity);

13
wpilibjExamples/src/main/java/org/wpilib/examples/swervedriveposeestimator/Drivetrain.java
View File

@@ -74,13 +74,14 @@ public class Drivetrain {
chassisVelocities = chassisVelocities.discretize(period);
var states = m_kinematics.toWheelVelocities(chassisVelocities);
SwerveDriveKinematics.desaturateWheelVelocities(states, kMaxVelocity);
var velocities =
SwerveDriveKinematics.desaturateWheelVelocities(
m_kinematics.toWheelVelocities(chassisVelocities), kMaxVelocity);
m_frontLeft.setDesiredVelocity(states[0]);
m_frontRight.setDesiredVelocity(states[1]);
m_backLeft.setDesiredVelocity(states[2]);
m_backRight.setDesiredVelocity(states[3]);
m_frontLeft.setDesiredVelocity(velocities[0]);
m_frontRight.setDesiredVelocity(velocities[1]);
m_backLeft.setDesiredVelocity(velocities[2]);
m_backRight.setDesiredVelocity(velocities[3]);
}
/** Updates the field relative position of the robot. */

19
wpilibjExamples/src/main/java/org/wpilib/examples/swervedriveposeestimator/SwerveModule.java
View File

@@ -110,25 +110,22 @@ public class SwerveModule {
public void setDesiredVelocity(SwerveModuleVelocity desiredVelocity) {
var encoderRotation = new Rotation2d(m_turningEncoder.getDistance());
// Optimize the reference velocity to avoid spinning further than 90 degrees
desiredVelocity.optimize(encoderRotation);
// Scale velocity 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.
desiredVelocity.cosineScale(encoderRotation);
// Optimize the desired velocity to avoid spinning further than 90 degrees, then scale velocity
// 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.
SwerveModuleVelocity velocity =
desiredVelocity.optimize(encoderRotation).cosineScale(encoderRotation);
// Calculate the drive output from the drive PID controller.
final double driveOutput =
m_drivePIDController.calculate(m_driveEncoder.getRate(), desiredVelocity.velocity);
m_drivePIDController.calculate(m_driveEncoder.getRate(), velocity.velocity);
final double driveFeedforward = m_driveFeedforward.calculate(desiredVelocity.velocity);
final double driveFeedforward = m_driveFeedforward.calculate(velocity.velocity);
// Calculate the turning motor output from the turning PID controller.
final double turnOutput =
m_turningPIDController.calculate(
m_turningEncoder.getDistance(), desiredVelocity.angle.getRadians());
m_turningEncoder.getDistance(), velocity.angle.getRadians());
final double turnFeedforward =
m_turnFeedforward.calculate(m_turningPIDController.getSetpoint().velocity);