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

[wpimath] Add ChassisSpeeds method to fix drifting during compound swerve drive maneuvers (#5425)

Browse Source
This commit is contained in:
Joseph Eng
2023-07-18 21:19:55 -07:00
committed by GitHub
parent 1af224c21b
commit 657338715d
28 changed files with 227 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -46,11 +46,14 @@ void Drivetrain::SetSpeeds(const frc::MecanumDriveWheelSpeeds& wheelSpeeds) {
void Drivetrain::Drive(units::meters_per_second_t xSpeed,
units::meters_per_second_t ySpeed,
units::radians_per_second_t rot, bool fieldRelative) {
auto wheelSpeeds = m_kinematics.ToWheelSpeeds(
fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_gyro.GetRotation2d())
: frc::ChassisSpeeds{xSpeed, ySpeed, rot});
units::radians_per_second_t rot, bool fieldRelative,
units::second_t period) {
auto wheelSpeeds =
m_kinematics.ToWheelSpeeds(frc::ChassisSpeeds::FromDiscreteSpeeds(
fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_gyro.GetRotation2d())
: frc::ChassisSpeeds{xSpeed, ySpeed, rot},
period));
wheelSpeeds.Desaturate(kMaxSpeed);
SetSpeeds(wheelSpeeds);
}

2
wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Robot.cpp
View File

@@ -46,7 +46,7 @@ class Robot : public frc::TimedRobot {
const auto rot = -m_rotLimiter.Calculate(m_controller.GetRightX()) *
Drivetrain::kMaxAngularSpeed;
m_mecanum.Drive(xSpeed, ySpeed, rot, fieldRelative);
m_mecanum.Drive(xSpeed, ySpeed, rot, fieldRelative, GetPeriod());
}
};

2
wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/Drivetrain.h
View File

@@ -36,7 +36,7 @@ class Drivetrain {
void SetSpeeds(const frc::MecanumDriveWheelSpeeds& wheelSpeeds);
void Drive(units::meters_per_second_t xSpeed,
units::meters_per_second_t ySpeed, units::radians_per_second_t rot,
bool fieldRelative);
bool fieldRelative, units::second_t period);
void UpdateOdometry();
static constexpr auto kMaxSpeed = 3.0_mps; // 3 meters per second