From 657338715d30a1f3aff24dd841a256b0f649d4e9 Mon Sep 17 00:00:00 2001 From: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> Date: Tue, 18 Jul 2023 21:19:55 -0700 Subject: [PATCH] [wpimath] Add ChassisSpeeds method to fix drifting during compound swerve drive maneuvers (#5425) --- .../examples/MecanumBot/cpp/Drivetrain.cpp | 13 +++-- .../cpp/examples/MecanumBot/cpp/Robot.cpp | 2 +- .../examples/MecanumBot/include/Drivetrain.h | 2 +- .../cpp/Drivetrain.cpp | 13 +++-- .../MecanumDrivePoseEstimator/cpp/Robot.cpp | 2 +- .../include/Drivetrain.h | 2 +- .../cpp/examples/SwerveBot/cpp/Drivetrain.cpp | 13 +++-- .../main/cpp/examples/SwerveBot/cpp/Robot.cpp | 2 +- .../examples/SwerveBot/include/Drivetrain.h | 2 +- .../cpp/subsystems/DriveSubsystem.cpp | 12 +++-- .../include/Constants.h | 5 ++ .../include/subsystems/DriveSubsystem.h | 3 +- .../cpp/Drivetrain.cpp | 13 +++-- .../SwerveDrivePoseEstimator/cpp/Robot.cpp | 2 +- .../include/Drivetrain.h | 2 +- .../examples/mecanumbot/Drivetrain.java | 12 +++-- .../wpilibj/examples/mecanumbot/Robot.java | 2 +- .../mecanumdriveposeestimator/Drivetrain.java | 12 +++-- .../mecanumdriveposeestimator/Robot.java | 2 +- .../examples/swervebot/Drivetrain.java | 12 +++-- .../wpilibj/examples/swervebot/Robot.java | 2 +- .../swervecontrollercommand/Constants.java | 4 ++ .../subsystems/DriveSubsystem.java | 9 ++-- .../swervedriveposeestimator/Drivetrain.java | 12 +++-- .../swervedriveposeestimator/Robot.java | 2 +- .../first/math/kinematics/ChassisSpeeds.java | 49 ++++++++++++++++++ .../include/frc/kinematics/ChassisSpeeds.h | 50 +++++++++++++++++++ .../math/kinematics/ChassisSpeedsTest.java | 28 +++++++++++ 28 files changed, 227 insertions(+), 57 deletions(-) diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp index a59b758021..cacab1b326 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp @@ -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); } diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Robot.cpp index 4943184f61..18955f2ffd 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Robot.cpp @@ -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()); } }; diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h index fd4ae7ae8e..a6545d35c2 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h @@ -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 = diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp index c1046dd1cf..97e6b128d9 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp @@ -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); } diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Robot.cpp index 4943184f61..18955f2ffd 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Robot.cpp @@ -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()); } }; diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/Drivetrain.h index eeaf7aff8b..d934dc5582 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/Drivetrain.h @@ -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 diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp index 537da53edc..a05c88bdef 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp @@ -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); diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Robot.cpp index 39cfa14639..b8edc6ec5c 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Robot.cpp @@ -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()); } }; diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h index 87233d225d..7f6f9cf4e5 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h @@ -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 = diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp index 4b60372a64..7ff41d097d 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp @@ -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); diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h index cacab876c0..779927d7cd 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h +++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h @@ -4,6 +4,7 @@ #include +#include #include #include #include @@ -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 diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h index 6a29940e50..f3cad50dbd 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h +++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h @@ -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. diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp index 5402e8709c..e5ff7e2f6a 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp @@ -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); diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Robot.cpp index 3f5f6751b3..919dca692a 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Robot.cpp @@ -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()); } }; diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/Drivetrain.h index e042291f63..e71dc43bdf 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/Drivetrain.h @@ -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 diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java index 8849598ee3..35468c5baa 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java @@ -128,12 +128,16 @@ public class Drivetrain { * @param rot Angular rate of the robot. * @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) { + public void drive( + double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) { var mecanumDriveWheelSpeeds = m_kinematics.toWheelSpeeds( - fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, m_gyro.getRotation2d()) - : new ChassisSpeeds(xSpeed, ySpeed, rot)); + ChassisSpeeds.fromDiscreteSpeeds( + fieldRelative + ? ChassisSpeeds.fromFieldRelativeSpeeds( + xSpeed, ySpeed, rot, m_gyro.getRotation2d()) + : new ChassisSpeeds(xSpeed, ySpeed, rot), + periodSeconds)); mecanumDriveWheelSpeeds.desaturate(kMaxSpeed); setSpeeds(mecanumDriveWheelSpeeds); } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Robot.java index e1d9a08702..77f55d6f75 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Robot.java @@ -44,6 +44,6 @@ public class Robot extends TimedRobot { // the right by default. final var rot = -m_rotLimiter.calculate(m_controller.getRightX()) * Drivetrain.kMaxAngularSpeed; - m_mecanum.drive(xSpeed, ySpeed, rot, fieldRelative); + m_mecanum.drive(xSpeed, ySpeed, rot, fieldRelative, getPeriod()); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java index 299fe68c74..87cb01b27a 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java @@ -140,12 +140,16 @@ public class Drivetrain { * @param rot Angular rate of the robot. * @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) { + public void drive( + double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) { var mecanumDriveWheelSpeeds = m_kinematics.toWheelSpeeds( - fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, m_gyro.getRotation2d()) - : new ChassisSpeeds(xSpeed, ySpeed, rot)); + ChassisSpeeds.fromDiscreteSpeeds( + fieldRelative + ? ChassisSpeeds.fromFieldRelativeSpeeds( + xSpeed, ySpeed, rot, m_gyro.getRotation2d()) + : new ChassisSpeeds(xSpeed, ySpeed, rot), + periodSeconds)); mecanumDriveWheelSpeeds.desaturate(kMaxSpeed); setSpeeds(mecanumDriveWheelSpeeds); } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Robot.java index fb22388b36..581f744d8b 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Robot.java @@ -44,6 +44,6 @@ public class Robot extends TimedRobot { // the right by default. final var rot = -m_rotLimiter.calculate(m_controller.getRightX()) * Drivetrain.kMaxAngularSpeed; - m_mecanum.drive(xSpeed, ySpeed, rot, fieldRelative); + m_mecanum.drive(xSpeed, ySpeed, rot, fieldRelative, getPeriod()); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java index 6a45f6a3e9..b45c85b58f 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java @@ -55,12 +55,16 @@ public class Drivetrain { * @param rot Angular rate of the robot. * @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) { + public void drive( + double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) { var swerveModuleStates = m_kinematics.toSwerveModuleStates( - fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, m_gyro.getRotation2d()) - : new ChassisSpeeds(xSpeed, ySpeed, rot)); + ChassisSpeeds.fromDiscreteSpeeds( + 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]); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java index b2d39eff35..774c654ea8 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java @@ -51,6 +51,6 @@ public class Robot extends TimedRobot { -m_rotLimiter.calculate(MathUtil.applyDeadband(m_controller.getRightX(), 0.02)) * Drivetrain.kMaxAngularSpeed; - m_swerve.drive(xSpeed, ySpeed, rot, fieldRelative); + m_swerve.drive(xSpeed, ySpeed, rot, fieldRelative, getPeriod()); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Constants.java index 02c04b8271..0965db376d 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Constants.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Constants.java @@ -7,6 +7,7 @@ package edu.wpi.first.wpilibj.examples.swervecontrollercommand; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj.TimedRobot; /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean @@ -48,6 +49,9 @@ public final class Constants { public static final boolean kFrontRightDriveEncoderReversed = false; public static final boolean kRearRightDriveEncoderReversed = true; + // If you call DriveSubsystem.drive() with a different period make sure to update this. + public static final double kDrivePeriod = TimedRobot.kDefaultPeriod; + public static final double kTrackWidth = 0.5; // Distance between centers of right and left wheels on robot public static final double kWheelBase = 0.7; diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java index 51ec2e3ef6..aa04e560b4 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java @@ -120,9 +120,12 @@ public class DriveSubsystem extends Subsystem { public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates( - fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, m_gyro.getRotation2d()) - : new ChassisSpeeds(xSpeed, ySpeed, rot)); + ChassisSpeeds.fromDiscreteSpeeds( + 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]); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java index 8f4c5ce5ae..df44ed9341 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java @@ -64,12 +64,16 @@ public class Drivetrain { * @param rot Angular rate of the robot. * @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) { + public void drive( + double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) { var swerveModuleStates = m_kinematics.toSwerveModuleStates( - fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, m_gyro.getRotation2d()) - : new ChassisSpeeds(xSpeed, ySpeed, rot)); + ChassisSpeeds.fromDiscreteSpeeds( + 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]); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Robot.java index 7fbee8de91..bb3a596913 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Robot.java @@ -44,6 +44,6 @@ public class Robot extends TimedRobot { // the right by default. final var rot = -m_rotLimiter.calculate(m_controller.getRightX()) * Drivetrain.kMaxAngularSpeed; - m_swerve.drive(xSpeed, ySpeed, rot, fieldRelative); + m_swerve.drive(xSpeed, ySpeed, rot, fieldRelative, getPeriod()); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java index 09f25aa276..5018d1d7d8 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java @@ -4,6 +4,7 @@ package edu.wpi.first.math.kinematics; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; /** @@ -43,6 +44,54 @@ public class ChassisSpeeds { this.omegaRadiansPerSecond = omegaRadiansPerSecond; } + /** + * Converts from a chassis speed for a discrete timestep into chassis speed for continuous time. + * + *

The difference between applying a chassis speed for a discrete timestep vs. continuously is + * that applying for a discrete timestep is just scaling the velocity components by the time and + * adding, while when applying continuously the changes to the heading affect the direction the + * translational components are applied to relative to the field. + * + * @param vxMetersPerSecond Forward velocity. + * @param vyMetersPerSecond Sideways velocity. + * @param omegaRadiansPerSecond Angular velocity. + * @param dtSeconds The duration of the timestep the speeds should be applied for. + * @return ChassisSpeeds that can be applied continuously to produce the discrete chassis speeds. + */ + public static ChassisSpeeds fromDiscreteSpeeds( + double vxMetersPerSecond, + double vyMetersPerSecond, + double omegaRadiansPerSecond, + double dtSeconds) { + var desiredDeltaPose = + new Pose2d( + vxMetersPerSecond * dtSeconds, + vyMetersPerSecond * dtSeconds, + new Rotation2d(omegaRadiansPerSecond * dtSeconds)); + var twist = new Pose2d().log(desiredDeltaPose); + return new ChassisSpeeds(twist.dx / dtSeconds, twist.dy / dtSeconds, twist.dtheta / dtSeconds); + } + + /** + * Converts from a chassis speed for a discrete timestep into chassis speed for continuous time. + * + *

The difference between applying a chassis speed for a discrete timestep vs. continuously is + * that applying for a discrete timestep is just scaling the velocity components by the time and + * adding, while when applying continuously the changes to the heading affect the direction the + * translational components are applied to relative to the field. + * + * @param discreteSpeeds The speeds for a discrete timestep. + * @param dtSeconds The duration of the timestep the speeds should be applied for. + * @return ChassisSpeeds that can be applied continuously to produce the discrete chassis speeds. + */ + public static ChassisSpeeds fromDiscreteSpeeds(ChassisSpeeds discreteSpeeds, double dtSeconds) { + return fromDiscreteSpeeds( + discreteSpeeds.vxMetersPerSecond, + discreteSpeeds.vyMetersPerSecond, + discreteSpeeds.omegaRadiansPerSecond, + dtSeconds); + } + /** * Converts a user provided field-relative set of speeds into a robot-relative ChassisSpeeds * object. diff --git a/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h b/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h index 905bb0a3b7..ab6dd1d042 100644 --- a/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h +++ b/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h @@ -6,6 +6,7 @@ #include +#include "frc/geometry/Pose2d.h" #include "frc/geometry/Rotation2d.h" #include "units/angular_velocity.h" #include "units/velocity.h" @@ -38,6 +39,55 @@ struct WPILIB_DLLEXPORT ChassisSpeeds { */ units::radians_per_second_t omega = 0_rad_per_s; + /** + * Converts from a chassis speed for a discrete timestep into chassis speed + * for continuous time. + * + * The difference between applying a chassis speed for a discrete timestep vs. + * continuously is that applying for a discrete timestep is just scaling the + * velocity components by the time and adding, while when applying + * continuously the changes to the heading affect the direction the + * translational components are applied to relative to the field. + * + * @param vx Forward velocity. + * @param vy Sideways velocity. + * @param omega Angular velocity. + * @param dt The duration of the timestep the speeds should be applied for. + * + * @return ChassisSpeeds that can be applied continuously to produce the + * discrete ChassisSpeeds. + */ + static ChassisSpeeds FromDiscreteSpeeds(units::meters_per_second_t vx, + units::meters_per_second_t vy, + units::radians_per_second_t omega, + units::second_t dt) { + Pose2d desiredDeltaPose{vx * dt, vy * dt, omega * dt}; + auto twist = Pose2d{}.Log(desiredDeltaPose); + return {twist.dx / dt, twist.dy / dt, twist.dtheta / dt}; + } + + /** + * Converts from a chassis speed for a discrete timestep into chassis speed + * for continuous time. + * + * The difference between applying a chassis speed for a discrete timestep vs. + * continuously is that applying for a discrete timestep is just scaling the + * velocity components by the time and adding, while when applying + * continuously the changes to the heading affect the direction the + * translational components are applied to relative to the field. + * + * @param discreteSpeeds The speeds for a discrete timestep. + * @param dt The duration of the timestep the speeds should be applied for. + * + * @return ChassisSpeeds that can be applied continuously to produce the + * discrete ChassisSpeeds. + */ + static ChassisSpeeds FromDiscreteSpeeds(const ChassisSpeeds& discreteSpeeds, + units::second_t dt) { + return FromDiscreteSpeeds(discreteSpeeds.vx, discreteSpeeds.vy, + discreteSpeeds.omega, dt); + } + /** * Converts a user provided field-relative set of speeds into a robot-relative * ChassisSpeeds object. diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java index f572ac8cc5..26a1a7d533 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java @@ -7,12 +7,40 @@ package edu.wpi.first.math.kinematics; import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Twist2d; import org.junit.jupiter.api.Test; class ChassisSpeedsTest { private static final double kEpsilon = 1E-9; + @Test + void testVeeringCorrection() { + final var duration = 1.0; // duration of observation + final var dt = 0.01; // time increment for simulation + final var target = new ChassisSpeeds(1.0, 0.0, 0.5); + final var speeds = ChassisSpeeds.fromDiscreteSpeeds(target, duration); + final var twist = + new Twist2d( + speeds.vxMetersPerSecond * dt, + speeds.vyMetersPerSecond * dt, + speeds.omegaRadiansPerSecond * dt); + var pose = new Pose2d(); + for (double time = 0; time < duration; time += dt) { + pose = pose.exp(twist); + } + final var result = pose; // For lambda capture + assertAll( + () -> assertEquals(target.vxMetersPerSecond * duration, result.getX(), kEpsilon), + () -> assertEquals(target.vyMetersPerSecond * duration, result.getY(), kEpsilon), + () -> + assertEquals( + target.omegaRadiansPerSecond * duration, + result.getRotation().getRadians(), + kEpsilon)); + } + @Test void testFieldRelativeConstruction() { final var chassisSpeeds =