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

10
wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp
View File

@@ -15,6 +15,13 @@ frc::MecanumDriveWheelSpeeds Drivetrain::GetCurrentState() const {
units::meters_per_second_t{m_backRightEncoder.GetRate()}};
}
frc::MecanumDriveWheelPositions Drivetrain::GetCurrentDistances() const {
return {units::meter_t{m_frontLeftEncoder.GetDistance()},
units::meter_t{m_frontRightEncoder.GetDistance()},
units::meter_t{m_backLeftEncoder.GetDistance()},
units::meter_t{m_backRightEncoder.GetDistance()}};
}
void Drivetrain::SetSpeeds(const frc::MecanumDriveWheelSpeeds& wheelSpeeds) {
std::function<void(units::meters_per_second_t, const frc::Encoder&,
frc2::PIDController&, frc::PWMSparkMax&)>
@@ -49,7 +56,8 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed,
}
void Drivetrain::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

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

@@ -32,6 +32,7 @@ class Drivetrain {
}
frc::MecanumDriveWheelSpeeds GetCurrentState() const;
frc::MecanumDriveWheelPositions GetCurrentDistances() const;
void SetSpeeds(const frc::MecanumDriveWheelSpeeds& wheelSpeeds);
void Drive(units::meters_per_second_t xSpeed,
units::meters_per_second_t ySpeed, units::radians_per_second_t rot,
@@ -75,7 +76,12 @@ class Drivetrain {
// Gains are for example purposes only - must be determined for your own
// robot!
frc::MecanumDrivePoseEstimator m_poseEstimator{0_deg, frc::Pose2d{},
m_kinematics, {0.1, 0.1, 0.1},
{0.05}, {0.1, 0.1, 0.1}};
frc::MecanumDrivePoseEstimator m_poseEstimator{
0_deg,
frc::Pose2d{},
GetCurrentDistances(),
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},
{0.1, 0.1, 0.1}};
};