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] Position Delta Odometry for Swerve (#4493)

Browse Source
This commit is contained in:
Jordan McMichael
2022-10-25 15:28:36 -04:00
committed by GitHub
parent fe400f68c5
commit 4170ec6107
35 changed files with 1508 additions and 277 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -63,9 +63,9 @@ public class Drivetrain {
public void updateOdometry() {
m_odometry.update(
m_gyro.getRotation2d(),
m_frontLeft.getState(),
m_frontRight.getState(),
m_backLeft.getState(),
m_backRight.getState());
m_frontLeft.getPosition(),
m_frontRight.getPosition(),
m_backLeft.getPosition(),
m_backRight.getPosition());
}
}

11
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java
View File

@@ -8,6 +8,7 @@ import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.Encoder;
@@ -91,6 +92,16 @@ public class SwerveModule {
return new SwerveModuleState(m_driveEncoder.getRate(), new Rotation2d(m_turningEncoder.get()));
}
/**
* Returns the current position of the module.
*
* @return The current position of the module.
*/
public SwerveModulePosition getPosition() {
return new SwerveModulePosition(
m_driveEncoder.getDistance(), new Rotation2d(m_turningEncoder.get()));
}
/**
* Sets the desired state for the module.
*

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

@@ -67,10 +67,10 @@ public class DriveSubsystem extends SubsystemBase {
// Update the odometry in the periodic block
m_odometry.update(
m_gyro.getRotation2d(),
m_frontLeft.getState(),
m_frontRight.getState(),
m_rearLeft.getState(),
m_rearRight.getState());
m_frontLeft.getPosition(),
m_frontRight.getPosition(),
m_rearLeft.getPosition(),
m_rearRight.getPosition());
}
/**

11
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java
View File

@@ -7,6 +7,7 @@ package edu.wpi.first.wpilibj.examples.swervecontrollercommand.subsystems;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.Encoder;
@@ -87,6 +88,16 @@ public class SwerveModule {
return new SwerveModuleState(m_driveEncoder.getRate(), new Rotation2d(m_turningEncoder.get()));
}
/**
* Returns the current position of the module.
*
* @return The current position of the module.
*/
public SwerveModulePosition getPosition() {
return new SwerveModulePosition(
m_driveEncoder.getDistance(), new Rotation2d(m_turningEncoder.get()));
}
/**
* Sets the desired state for the module.
*

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

@@ -4,12 +4,17 @@
package edu.wpi.first.wpilibj.examples.swervedriveposeestimator;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
import edu.wpi.first.math.geometry.Pose2d;
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.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.numbers.N5;
import edu.wpi.first.math.numbers.N7;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.Timer;
@@ -37,13 +42,22 @@ public class Drivetrain {
/* 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(
private final SwerveDrivePoseEstimator<N7, N7, N5> m_poseEstimator =
new SwerveDrivePoseEstimator<N7, N7, N5>(
Nat.N7(),
Nat.N7(),
Nat.N5(),
m_gyro.getRotation2d(),
new Pose2d(),
new SwerveModulePosition[] {
m_frontLeft.getPosition(),
m_frontRight.getPosition(),
m_backLeft.getPosition(),
m_backRight.getPosition()
},
m_kinematics,
VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)),
VecBuilder.fill(Units.degreesToRadians(0.01)),
VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5), 0.05, 0.05, 0.05, 0.05),
VecBuilder.fill(Units.degreesToRadians(0.01), 0.01, 0.01, 0.01, 0.01),
VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
public Drivetrain() {
@@ -75,10 +89,18 @@ public class Drivetrain {
public void updateOdometry() {
m_poseEstimator.update(
m_gyro.getRotation2d(),
m_frontLeft.getState(),
m_frontRight.getState(),
m_backLeft.getState(),
m_backRight.getState());
new SwerveModuleState[] {
m_frontLeft.getState(),
m_frontRight.getState(),
m_backLeft.getState(),
m_backRight.getState()
},
new SwerveModulePosition[] {
m_frontLeft.getPosition(),
m_frontRight.getPosition(),
m_backLeft.getPosition(),
m_backRight.getPosition()
});
// 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.

11
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java
View File

@@ -8,6 +8,7 @@ import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.Encoder;
@@ -91,6 +92,16 @@ public class SwerveModule {
return new SwerveModuleState(m_driveEncoder.getRate(), new Rotation2d(m_turningEncoder.get()));
}
/**
* Returns the current position of the module.
*
* @return The current position of the module.
*/
public SwerveModulePosition getPosition() {
return new SwerveModulePosition(
m_driveEncoder.getDistance(), new Rotation2d(m_turningEncoder.get()));
}
/**
* Sets the desired state for the module.
*