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] Rework function signatures for Pose Estimation / Odometry (#4642)

Browse Source 
Forcing the use of SwerveModulePostition[] and wpi::array<SwerveModulePosition, len> in place of variadic function signatures.
This commit is contained in:
Jordan McMichael
2022-11-17 13:36:33 -05:00
committed by GitHub
parent 0401597d3b
commit ce4c45df13
9 changed files with 84 additions and 104 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -8,6 +8,7 @@ import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.wpilibj.AnalogGyro;
/** Represents a swerve drive style drivetrain. */
@@ -32,7 +33,15 @@ public class Drivetrain {
m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);
private final SwerveDriveOdometry m_odometry =
new SwerveDriveOdometry(m_kinematics, m_gyro.getRotation2d());
new SwerveDriveOdometry(
m_kinematics,
m_gyro.getRotation2d(),
new SwerveModulePosition[] {
new SwerveModulePosition(),
new SwerveModulePosition(),
new SwerveModulePosition(),
new SwerveModulePosition()
});
public Drivetrain() {
m_gyro.reset();
@@ -63,9 +72,11 @@ public class Drivetrain {
public void updateOdometry() {
m_odometry.update(
m_gyro.getRotation2d(),
m_frontLeft.getPosition(),
m_frontRight.getPosition(),
m_backLeft.getPosition(),
m_backRight.getPosition());
new SwerveModulePosition[] {
m_frontLeft.getPosition(),
m_frontRight.getPosition(),
m_backLeft.getPosition(),
m_backRight.getPosition()
});
}
}

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

@@ -8,6 +8,7 @@ import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants;
@@ -57,7 +58,15 @@ public class DriveSubsystem extends SubsystemBase {
// Odometry class for tracking robot pose
SwerveDriveOdometry m_odometry =
new SwerveDriveOdometry(DriveConstants.kDriveKinematics, m_gyro.getRotation2d());
new SwerveDriveOdometry(
DriveConstants.kDriveKinematics,
m_gyro.getRotation2d(),
new SwerveModulePosition[] {
new SwerveModulePosition(),
new SwerveModulePosition(),
new SwerveModulePosition(),
new SwerveModulePosition()
});
/** Creates a new DriveSubsystem. */
public DriveSubsystem() {}
@@ -67,10 +76,12 @@ public class DriveSubsystem extends SubsystemBase {
// Update the odometry in the periodic block
m_odometry.update(
m_gyro.getRotation2d(),
m_frontLeft.getPosition(),
m_frontRight.getPosition(),
m_rearLeft.getPosition(),
m_rearRight.getPosition());
new SwerveModulePosition[] {
m_frontLeft.getPosition(),
m_frontRight.getPosition(),
m_rearLeft.getPosition(),
m_rearRight.getPosition()
});
}
/**
@@ -88,7 +99,15 @@ public class DriveSubsystem extends SubsystemBase {
* @param pose The pose to which to set the odometry.
*/
public void resetOdometry(Pose2d pose) {
m_odometry.resetPosition(pose, m_gyro.getRotation2d());
m_odometry.resetPosition(
pose,
m_gyro.getRotation2d(),
new SwerveModulePosition[] {
m_frontLeft.getPosition(),
m_frontRight.getPosition(),
m_rearLeft.getPosition(),
m_rearRight.getPosition()
});
}
/**