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
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java
View File

@@ -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);
}

2
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Robot.java
View File

@@ -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());
}
}

12
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java
View File

@@ -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);
}

2
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Robot.java
View File

@@ -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());
}
}

12
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java
View File

@@ -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]);

2
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java
View File

@@ -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());
}
}

4
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Constants.java
View File

@@ -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;

9
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java
View File

@@ -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]);

12
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java
View File

@@ -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]);

2
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Robot.java
View File

@@ -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());
}
}