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

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.