Skip to content

Commit

Permalink
[wpimath] Use immutable member functions in ChassisSpeeds
Browse files Browse the repository at this point in the history
  • Loading branch information
@calcmogul
calcmogul committed Dec 14, 2024
1 parent 945d416 commit e9b8f25
Show file tree
Hide file tree
Showing 24 changed files with 164 additions and 413 deletions.
2 changes: 1 addition & 1 deletion wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java
View file
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
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
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
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
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
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
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
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
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
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
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
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
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

0 comments on commit e9b8f25

Please sign in to comment.