Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +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!

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

@@ -28,7 +28,8 @@ DriveSubsystem::DriveSubsystem()
m_rearRightEncoder{kRearRightEncoderPorts[0], kRearRightEncoderPorts[1],
kRearRightEncoderReversed},
m_odometry{kDriveKinematics, m_gyro.GetRotation2d(), frc::Pose2d{}} {
m_odometry{kDriveKinematics, m_gyro.GetRotation2d(),
getCurrentWheelDistances(), frc::Pose2d{}} {
// Set the distance per pulse for the encoders
m_frontLeftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
m_rearLeftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
@@ -43,13 +44,7 @@ DriveSubsystem::DriveSubsystem()
void DriveSubsystem::Periodic() {
// Implementation of subsystem periodic method goes here.
m_odometry.Update(
m_gyro.GetRotation2d(),
frc::MecanumDriveWheelSpeeds{
units::meters_per_second_t{m_frontLeftEncoder.GetRate()},
units::meters_per_second_t{m_rearLeftEncoder.GetRate()},
units::meters_per_second_t{m_frontRightEncoder.GetRate()},
units::meters_per_second_t{m_rearRightEncoder.GetRate()}});
m_odometry.Update(m_gyro.GetRotation2d(), getCurrentWheelDistances());
}
void DriveSubsystem::Drive(double xSpeed, double ySpeed, double rot,
@@ -102,6 +97,14 @@ frc::MecanumDriveWheelSpeeds DriveSubsystem::getCurrentWheelSpeeds() {
units::meters_per_second_t{m_rearRightEncoder.GetRate()}});
}
frc::MecanumDriveWheelPositions DriveSubsystem::getCurrentWheelDistances() {
return (frc::MecanumDriveWheelPositions{
units::meter_t{m_frontLeftEncoder.GetDistance()},
units::meter_t{m_rearLeftEncoder.GetDistance()},
units::meter_t{m_frontRightEncoder.GetDistance()},
units::meter_t{m_rearRightEncoder.GetDistance()}});
}
void DriveSubsystem::SetMaxOutput(double maxOutput) {
m_drive.SetMaxOutput(maxOutput);
}
@@ -123,5 +126,6 @@ frc::Pose2d DriveSubsystem::GetPose() {
}
void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
m_odometry.ResetPosition(pose, m_gyro.GetRotation2d());
m_odometry.ResetPosition(pose, m_gyro.GetRotation2d(),
getCurrentWheelDistances());
}

7
wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/subsystems/DriveSubsystem.h
View File

@@ -81,6 +81,13 @@ class DriveSubsystem : public frc2::SubsystemBase {
*/
frc::MecanumDriveWheelSpeeds getCurrentWheelSpeeds();
/**
* Gets the distances travelled by each wheel.
*
* @return the distances travelled by each wheel.
*/
frc::MecanumDriveWheelPositions getCurrentWheelDistances();
/**
* Sets the drive MotorControllers to a desired voltage.
*/

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}};
};