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 Mecanum (#4514)

Browse Source
This commit is contained in:
Jordan McMichael
2022-10-25 15:28:59 -04:00
committed by GitHub
parent 4170ec6107
commit 901fc555f4
28 changed files with 1222 additions and 235 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -10,6 +10,7 @@ import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
import edu.wpi.first.math.kinematics.MecanumDriveOdometry;
import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.Encoder;
@@ -48,7 +49,7 @@ public class Drivetrain {
m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);
private final MecanumDriveOdometry m_odometry =
new MecanumDriveOdometry(m_kinematics, m_gyro.getRotation2d());
new MecanumDriveOdometry(m_kinematics, m_gyro.getRotation2d(), getCurrentDistances());
// Gains are for example purposes only - must be determined for your own robot!
private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3);
@@ -76,6 +77,19 @@ public class Drivetrain {
m_backRightEncoder.getRate());
}
/**
* Returns the current distances measured by the drivetrain.
*
* @return The current distances measured by the drivetrain.
*/
public MecanumDriveWheelPositions getCurrentDistances() {
return new MecanumDriveWheelPositions(
m_frontLeftEncoder.getDistance(),
m_frontRightEncoder.getDistance(),
m_backLeftEncoder.getDistance(),
m_backRightEncoder.getDistance());
}
/**
* Set the desired speeds for each wheel.
*
@@ -126,6 +140,6 @@ public class Drivetrain {
/** Updates the field relative position of the robot. */
public void updateOdometry() {
m_odometry.update(m_gyro.getRotation2d(), getCurrentState());
m_odometry.update(m_gyro.getRotation2d(), getCurrentDistances());
}
}

29
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java
View File

@@ -7,6 +7,7 @@ package edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.subsystems;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.kinematics.MecanumDriveMotorVoltages;
import edu.wpi.first.math.kinematics.MecanumDriveOdometry;
import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.Encoder;
@@ -58,7 +59,10 @@ public class DriveSubsystem extends SubsystemBase {
// Odometry class for tracking robot pose
MecanumDriveOdometry m_odometry =
new MecanumDriveOdometry(DriveConstants.kDriveKinematics, m_gyro.getRotation2d());
new MecanumDriveOdometry(
DriveConstants.kDriveKinematics,
m_gyro.getRotation2d(),
new MecanumDriveWheelPositions());
/** Creates a new DriveSubsystem. */
public DriveSubsystem() {
@@ -77,13 +81,7 @@ public class DriveSubsystem extends SubsystemBase {
@Override
public void periodic() {
// Update the odometry in the periodic block
m_odometry.update(
m_gyro.getRotation2d(),
new MecanumDriveWheelSpeeds(
m_frontLeftEncoder.getRate(),
m_rearLeftEncoder.getRate(),
m_frontRightEncoder.getRate(),
m_rearRightEncoder.getRate()));
m_odometry.update(m_gyro.getRotation2d(), getCurrentWheelDistances());
}
/**
@@ -101,7 +99,7 @@ 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(), getCurrentWheelDistances());
}
/**
@@ -186,6 +184,19 @@ public class DriveSubsystem extends SubsystemBase {
m_rearRightEncoder.getRate());
}
/**
* Gets the current wheel distance measurements.
*
* @return the current wheel distance measurements in a MecanumDriveWheelPositions object.
*/
public MecanumDriveWheelPositions getCurrentWheelDistances() {
return new MecanumDriveWheelPositions(
m_frontLeftEncoder.getDistance(),
m_rearLeftEncoder.getDistance(),
m_frontRightEncoder.getDistance(),
m_rearRightEncoder.getDistance());
}
/**
* Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
*

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

@@ -12,6 +12,7 @@ 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.MecanumDriveKinematics;
import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.AnalogGyro;
@@ -57,9 +58,10 @@ public class Drivetrain {
new MecanumDrivePoseEstimator(
m_gyro.getRotation2d(),
new Pose2d(),
getCurrentDistances(),
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)));
// Gains are for example purposes only - must be determined for your own robot!
@@ -88,6 +90,19 @@ public class Drivetrain {
m_backRightEncoder.getRate());
}
/**
* Returns the current distances measured by the drivetrain.
*
* @return The current distances measured by the drivetrain.
*/
public MecanumDriveWheelPositions getCurrentDistances() {
return new MecanumDriveWheelPositions(
m_frontLeftEncoder.getDistance(),
m_frontRightEncoder.getDistance(),
m_backLeftEncoder.getDistance(),
m_backRightEncoder.getDistance());
}
/**
* Set the desired speeds for each wheel.
*
@@ -138,7 +153,7 @@ public class Drivetrain {
/** Updates the field relative position of the robot. */
public void updateOdometry() {
m_poseEstimator.update(m_gyro.getRotation2d(), getCurrentState());
m_poseEstimator.update(m_gyro.getRotation2d(), getCurrentState(), getCurrentDistances());
// 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.