Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[wpimath] Use immutable member functions in ChassisSpeeds #7545

Merged
merged 1 commit into from
Dec 16, 2024
+164 −413
Merged

[wpimath] Use immutable member functions in ChassisSpeeds #7545

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -488,7 +488,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;
Expand Down
2 changes: 1 addition & 1 deletion wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
15 changes: 8 additions & 7 deletions wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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()},
Expand Down Expand Up @@ -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() {
Expand Down
15 changes: 7 additions & 8 deletions wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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() {
Expand Down
13 changes: 6 additions & 7 deletions wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
13 changes: 6 additions & 7 deletions ...cExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
17 changes: 8 additions & 9 deletions wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
16 changes: 6 additions & 10 deletions wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -129,16 +129,12 @@ public void setSpeeds(MecanumDriveWheelSpeeds speeds) {
*/
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. */
Expand Down
17 changes: 7 additions & 10 deletions ...es/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -141,16 +141,13 @@ public void setSpeeds(MecanumDriveWheelSpeeds speeds) {
*/
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. */
Expand Down
26 changes: 13 additions & 13 deletions wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -57,19 +57,19 @@ public 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. */
Expand Down
27 changes: 13 additions & 14 deletions ...ava/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -118,20 +118,19 @@ public void resetOdometry(Pose2d pose) {
* @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]);
}

/**
Expand Down
28 changes: 15 additions & 13 deletions ...les/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -66,19 +66,21 @@ public 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. */
Expand Down
6 changes: 3 additions & 3 deletions wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -104,16 +104,16 @@ public ChassisSpeeds calculate(
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).
double xFeedback = m_xController.calculate(currentPose.getX(), trajectoryPose.getX());
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());
}

/**
Expand Down
Loading
Loading