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

@@ -49,11 +49,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/MecanumBot/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/MecanumBot/include/Drivetrain.h
View File

@@ -35,7 +35,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 units::meters_per_second_t kMaxSpeed =

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

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

@@ -6,11 +6,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/SwerveBot/cpp/Robot.cpp
View File

@@ -50,7 +50,7 @@ class Robot : public frc::TimedRobot {
frc::ApplyDeadband(m_controller.GetRightX(), 0.02)) *
Drivetrain::kMaxAngularSpeed;
m_swerve.Drive(xSpeed, ySpeed, rot, fieldRelative);
m_swerve.Drive(xSpeed, ySpeed, rot, fieldRelative, GetPeriod());
}
};

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

@@ -22,7 +22,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 units::meters_per_second_t kMaxSpeed =

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

@@ -51,12 +51,14 @@ void DriveSubsystem::Periodic() {
void DriveSubsystem::Drive(units::meters_per_second_t xSpeed,
units::meters_per_second_t ySpeed,
units::radians_per_second_t rot,
bool fieldRelative) {
units::radians_per_second_t rot, bool fieldRelative,
units::second_t period) {
auto states = kDriveKinematics.ToSwerveModuleStates(
fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_gyro.GetRotation2d())
: frc::ChassisSpeeds{xSpeed, ySpeed, rot});
frc::ChassisSpeeds::FromDiscreteSpeeds(
fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_gyro.GetRotation2d())
: frc::ChassisSpeeds{xSpeed, ySpeed, rot},
period));
kDriveKinematics.DesaturateWheelSpeeds(&states, AutoConstants::kMaxSpeed);

5
wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h
View File

@@ -4,6 +4,7 @@
#include <numbers>
#include <frc/TimedRobot.h>
#include <frc/geometry/Translation2d.h>
#include <frc/kinematics/SwerveDriveKinematics.h>
#include <frc/trajectory/TrapezoidProfile.h>
@@ -58,6 +59,10 @@ constexpr bool kRearLeftDriveEncoderReversed = true;
constexpr bool kFrontRightDriveEncoderReversed = false;
constexpr bool kRearRightDriveEncoderReversed = true;
// If you call DriveSubsystem::Drive with a different period make sure to update
// this.
constexpr units::second_t kDrivePeriod = frc::TimedRobot::kDefaultPeriod;
// These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
// These characterization values MUST be determined either experimentally or
// theoretically for *your* robot's drive. The SysId tool provides a convenient

3
wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h
View File

@@ -43,7 +43,8 @@ class DriveSubsystem : public frc2::Subsystem {
*/
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 = DriveConstants::kDrivePeriod);
/**
* Resets the drive encoders to currently read a position of 0.

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