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 odometry APIs to improve feature parity (#4645)

Browse Source 
Co-authored-by: Ryan Blue <ryanzblue@gmail.com>
This commit is contained in:
Jordan McMichael
2022-11-18 23:42:00 -05:00
committed by GitHub
parent e2d49181da
commit 902e8686d3
53 changed files with 266 additions and 157 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h
View File

@@ -75,7 +75,9 @@ class Drivetrain {
frc::AnalogGyro m_gyro{0};
frc::DifferentialDriveKinematics m_kinematics{kTrackWidth};
frc::DifferentialDriveOdometry m_odometry{m_gyro.GetRotation2d()};
frc::DifferentialDriveOdometry m_odometry{
m_gyro.GetRotation2d(), units::meter_t{m_leftEncoder.GetDistance()},
units::meter_t{m_rightEncoder.GetDistance()}};
// Gains are for example purposes only - must be determined for your own
// robot!

4
wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h
View File

@@ -79,7 +79,9 @@ class Drivetrain {
// Gains are for example purposes only - must be determined for your own
// robot!
frc::DifferentialDrivePoseEstimator m_poseEstimator{
frc::Rotation2d{},
m_gyro.GetRotation2d(),
units::meter_t{m_leftEncoder.GetDistance()},
units::meter_t{m_rightEncoder.GetDistance()},
frc::Pose2d{},
{0.01, 0.01, 0.01, 0.01, 0.01},
{0.1, 0.1, 0.1},

4
wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp
View File

@@ -126,6 +126,6 @@ frc::Pose2d DriveSubsystem::GetPose() {
}
void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
m_odometry.ResetPosition(pose, m_gyro.GetRotation2d(),
getCurrentWheelDistances());
m_odometry.ResetPosition(m_gyro.GetRotation2d(), getCurrentWheelDistances(),
pose);
}

4
wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/Drivetrain.h
View File

@@ -77,9 +77,9 @@ class Drivetrain {
// Gains are for example purposes only - must be determined for your own
// robot!
frc::MecanumDrivePoseEstimator m_poseEstimator{
0_deg,
frc::Pose2d{},
m_gyro.GetRotation2d(),
GetCurrentDistances(),
frc::Pose2d{},
m_kinematics,
{0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1},
{0.05, 0.05, 0.05, 0.05, 0.05},

6
wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp
View File

@@ -16,7 +16,7 @@ DriveSubsystem::DriveSubsystem()
m_right2{kRightMotor2Port},
m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]},
m_odometry{m_gyro.GetRotation2d()} {
m_odometry{m_gyro.GetRotation2d(), units::meter_t{0}, units::meter_t{0}} {
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
@@ -86,5 +86,7 @@ frc::DifferentialDriveWheelSpeeds DriveSubsystem::GetWheelSpeeds() {
void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
ResetEncoders();
m_odometry.ResetPosition(pose, m_gyro.GetRotation2d());
m_odometry.ResetPosition(m_gyro.GetRotation2d(),
units::meter_t{m_leftEncoder.GetDistance()},
units::meter_t{m_rightEncoder.GetDistance()}, pose);
}

4
wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Drivetrain.cpp
View File

@@ -28,7 +28,9 @@ void Drivetrain::UpdateOdometry() {
}
void Drivetrain::ResetOdometry(const frc::Pose2d& pose) {
m_odometry.ResetPosition(pose, m_gyro.GetRotation2d());
m_odometry.ResetPosition(m_gyro.GetRotation2d(),
units::meter_t{m_leftEncoder.GetDistance()},
units::meter_t{m_rightEncoder.GetDistance()}, pose);
}
frc::Pose2d Drivetrain::GetPose() const {

4
wpilibcExamples/src/main/cpp/examples/RamseteController/include/Drivetrain.h
View File

@@ -76,7 +76,9 @@ class Drivetrain {
frc::AnalogGyro m_gyro{0};
frc::DifferentialDriveKinematics m_kinematics{kTrackWidth};
frc::DifferentialDriveOdometry m_odometry{m_gyro.GetRotation2d()};
frc::DifferentialDriveOdometry m_odometry{
m_gyro.GetRotation2d(), units::meter_t{m_leftEncoder.GetDistance()},
units::meter_t{m_rightEncoder.GetDistance()}};
// Gains are for example purposes only - must be determined for your own
// robot!

4
wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Drivetrain.cpp
View File

@@ -33,7 +33,9 @@ void Drivetrain::ResetOdometry(const frc::Pose2d& pose) {
m_leftEncoder.Reset();
m_rightEncoder.Reset();
m_drivetrainSimulator.SetPose(pose);
m_odometry.ResetPosition(pose, m_gyro.GetRotation2d());
m_odometry.ResetPosition(m_gyro.GetRotation2d(),
units::meter_t{m_leftEncoder.GetDistance()},
units::meter_t{m_rightEncoder.GetDistance()}, pose);
}
void Drivetrain::SimulationPeriodic() {

4
wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/include/Drivetrain.h
View File

@@ -92,7 +92,9 @@ class Drivetrain {
frc::AnalogGyro m_gyro{0};
frc::DifferentialDriveKinematics m_kinematics{kTrackWidth};
frc::DifferentialDriveOdometry m_odometry{m_gyro.GetRotation2d()};
frc::DifferentialDriveOdometry m_odometry{
m_gyro.GetRotation2d(), units::meter_t{m_leftEncoder.GetDistance()},
units::meter_t{m_rightEncoder.GetDistance()}};
// Gains are for example purposes only - must be determined for your own
// robot!

4
wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp
View File

@@ -109,5 +109,7 @@ frc::DifferentialDriveWheelSpeeds DriveSubsystem::GetWheelSpeeds() {
void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
ResetEncoders();
m_drivetrainSimulator.SetPose(pose);
m_odometry.ResetPosition(pose, m_gyro.GetRotation2d());
m_odometry.ResetPosition(m_gyro.GetRotation2d(),
units::meter_t{m_leftEncoder.GetDistance()},
units::meter_t{m_rightEncoder.GetDistance()}, pose);
}

4
wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/subsystems/DriveSubsystem.h
View File

@@ -154,7 +154,9 @@ class DriveSubsystem : public frc2::SubsystemBase {
frc::ADXRS450_Gyro m_gyro;
// Odometry class for tracking robot pose
frc::DifferentialDriveOdometry m_odometry{m_gyro.GetRotation2d()};
frc::DifferentialDriveOdometry m_odometry{
m_gyro.GetRotation2d(), units::meter_t{m_leftEncoder.GetDistance()},
units::meter_t{m_rightEncoder.GetDistance()}};
// These classes help simulate our drivetrain.
frc::sim::DifferentialDrivetrainSim m_drivetrainSimulator{

5
wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp
View File

@@ -103,7 +103,8 @@ frc::Pose2d DriveSubsystem::GetPose() {
void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
m_odometry.ResetPosition(
pose, units::degree_t{GetHeading()},
GetHeading(),
{m_frontLeft.GetPosition(), m_frontRight.GetPosition(),
m_rearLeft.GetPosition(), m_rearRight.GetPosition()});
m_rearLeft.GetPosition(), m_rearRight.GetPosition()},
pose);
}

2
wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/Drivetrain.h
View File

@@ -51,9 +51,9 @@ class Drivetrain {
// robot!
frc::SwerveDrivePoseEstimator<4> m_poseEstimator{
frc::Rotation2d{},
frc::Pose2d{},
{m_frontLeft.GetPosition(), m_frontRight.GetPosition(),
m_backLeft.GetPosition(), m_backRight.GetPosition()},
frc::Pose2d{},
m_kinematics,
{0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1},
{0.05, 0.05, 0.05, 0.05, 0.05},