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

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.
*/