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/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp
View File

@@ -10,11 +10,14 @@
void Drivetrain::Drive(units::meters_per_second_t xSpeed,
units::meters_per_second_t ySpeed,
units::radians_per_second_t rot, bool fieldRelative) {
auto states = m_kinematics.ToSwerveModuleStates(
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 states =
m_kinematics.ToSwerveModuleStates(frc::ChassisSpeeds::FromDiscreteSpeeds(
fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_gyro.GetRotation2d())
: frc::ChassisSpeeds{xSpeed, ySpeed, rot},
period));
m_kinematics.DesaturateWheelSpeeds(&states, kMaxSpeed);

2
wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/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_swerve.Drive(xSpeed, ySpeed, rot, fieldRelative);
m_swerve.Drive(xSpeed, ySpeed, rot, fieldRelative, GetPeriod());
}
};

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

@@ -23,7 +23,7 @@ class Drivetrain {
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