Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Revert "ChassisSpeeds fromRelative and discretize methods made instance methods (#7115)" (#7549)

Browse Source 
This reverts commit a3b12b3bd9.
This commit is contained in:
Peter Johnson
2024-12-14 11:42:49 -07:00
committed by GitHub
parent 68285dae77
commit 4bca79b9af
8 changed files with 52 additions and 127 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java
View File

@@ -129,12 +129,14 @@ public class Drivetrain {
*/
public void drive(
double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) {
var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
if (fieldRelative) {
chassisSpeeds.toRobotRelativeSpeeds(m_gyro.getRotation2d());
}
chassisSpeeds.discretize(periodSeconds);
var mecanumDriveWheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
var mecanumDriveWheelSpeeds =
m_kinematics.toWheelSpeeds(
ChassisSpeeds.discretize(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_gyro.getRotation2d())
: new ChassisSpeeds(xSpeed, ySpeed, rot),
periodSeconds));
mecanumDriveWheelSpeeds.desaturate(kMaxSpeed);
setSpeeds(mecanumDriveWheelSpeeds);
}

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

@@ -141,12 +141,14 @@ public class Drivetrain {
*/
public void drive(
double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) {
var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
if (fieldRelative) {
chassisSpeeds.toRobotRelativeSpeeds(m_poseEstimator.getEstimatedPosition().getRotation());
}
chassisSpeeds.discretize(periodSeconds);
var mecanumDriveWheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
var mecanumDriveWheelSpeeds =
m_kinematics.toWheelSpeeds(
ChassisSpeeds.discretize(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_poseEstimator.getEstimatedPosition().getRotation())
: new ChassisSpeeds(xSpeed, ySpeed, rot),
periodSeconds));
mecanumDriveWheelSpeeds.desaturate(kMaxSpeed);
setSpeeds(mecanumDriveWheelSpeeds);
}

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

@@ -57,12 +57,14 @@ public class Drivetrain {
*/
public void drive(
double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) {
var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
if (fieldRelative) {
chassisSpeeds.toRobotRelativeSpeeds(m_gyro.getRotation2d());
}
chassisSpeeds.discretize(periodSeconds);
var swerveModuleStates = m_kinematics.toWheelSpeeds(chassisSpeeds);
var swerveModuleStates =
m_kinematics.toSwerveModuleStates(
ChassisSpeeds.discretize(
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]);

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

@@ -118,12 +118,14 @@ public class DriveSubsystem extends SubsystemBase {
* @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) {
var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
if (fieldRelative) {
chassisSpeeds.toRobotRelativeSpeeds(m_gyro.getRotation2d());
}
chassisSpeeds.discretize(DriveConstants.kDrivePeriod);
var swerveModuleStates = DriveConstants.kDriveKinematics.toWheelSpeeds(chassisSpeeds);
var swerveModuleStates =
DriveConstants.kDriveKinematics.toSwerveModuleStates(
ChassisSpeeds.discretize(
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]);

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

@@ -36,11 +36,8 @@ public class Drivetrain {
new SwerveDriveKinematics(
m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);
/*
* Here we use SwerveDrivePoseEstimator so that we can fuse odometry readings.
* The numbers used
* below are robot specific, and should be tuned.
*/
/* Here we use SwerveDrivePoseEstimator so that we can fuse odometry readings. The numbers used
below are robot specific, and should be tuned. */
private final SwerveDrivePoseEstimator m_poseEstimator =
new SwerveDrivePoseEstimator(
m_kinematics,
@@ -69,12 +66,14 @@ public class Drivetrain {
*/
public void drive(
double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) {
var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
if (fieldRelative) {
chassisSpeeds.toRobotRelativeSpeeds(m_poseEstimator.getEstimatedPosition().getRotation());
}
chassisSpeeds.discretize(periodSeconds);
var swerveModuleStates = m_kinematics.toWheelSpeeds(chassisSpeeds);
var swerveModuleStates =
m_kinematics.toSwerveModuleStates(
ChassisSpeeds.discretize(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_poseEstimator.getEstimatedPosition().getRotation())
: new ChassisSpeeds(xSpeed, ySpeed, rot),
periodSeconds));
SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, kMaxSpeed);
m_frontLeft.setDesiredState(swerveModuleStates[0]);
m_frontRight.setDesiredState(swerveModuleStates[1]);
@@ -93,8 +92,7 @@ public class Drivetrain {
m_backRight.getPosition()
});
// Also apply vision measurements. We use 0.3 seconds in the past as an example
// -- on
// Also apply vision measurements. We use 0.3 seconds in the past as an example -- on
// a real robot, this must be calculated based either on latency or timestamps.
m_poseEstimator.addVisionMeasurement(
ExampleGlobalMeasurementSensor.getEstimatedGlobalPose(