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] Use immutable member functions in ChassisSpeeds (#7545)

Browse Source
This commit is contained in:
Tyler Veness
2024-12-15 16:09:34 -08:00
committed by GitHub
parent 945d416d07
commit 03f0fc4dea
24 changed files with 163 additions and 412 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp
View File

@@ -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() {

15
wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp
View File

@@ -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() {

13
wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp
View File

@@ -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);

13
wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp
View File

@@ -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);

15
wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp
View File

@@ -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);