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

9
wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp
View File

@@ -11,6 +11,13 @@ frc::MecanumDriveWheelSpeeds Drivetrain::GetCurrentState() const {
units::meters_per_second_t{m_backRightEncoder.GetRate()}};
}
frc::MecanumDriveWheelPositions Drivetrain::GetCurrentWheelDistances() 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) {
const auto frontLeftFeedforward =
m_feedforward.Calculate(wheelSpeeds.frontLeft);
@@ -52,5 +59,5 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed,
}
void Drivetrain::UpdateOdometry() {
m_odometry.Update(m_gyro.GetRotation2d(), GetCurrentState());
m_odometry.Update(m_gyro.GetRotation2d(), GetCurrentWheelDistances());
}

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

@@ -31,6 +31,7 @@ class Drivetrain {
}
frc::MecanumDriveWheelSpeeds GetCurrentState() const;
frc::MecanumDriveWheelPositions GetCurrentWheelDistances() 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,
@@ -69,7 +70,8 @@ class Drivetrain {
m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation,
m_backRightLocation};
frc::MecanumDriveOdometry m_odometry{m_kinematics, m_gyro.GetRotation2d()};
frc::MecanumDriveOdometry m_odometry{m_kinematics, m_gyro.GetRotation2d(),
GetCurrentWheelDistances()};
// Gains are for example purposes only - must be determined for your own
// robot!