Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-01 02:41:48 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Refactor kinematics, odometry, and pose estimator (#5355)

Browse Source
This commit is contained in:
Joseph Eng
2023-06-19 17:10:39 -07:00
committed by GitHub
parent 5c2addda0f
commit 25ad5017a9
41 changed files with 1742 additions and 2177 deletions
Download Patch File Download Diff File Expand all files Collapse all files

40
wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
View File

@@ -7,6 +7,10 @@
#include <wpi/SymbolExports.h>
#include "frc/geometry/Pose2d.h"
#include "frc/kinematics/DifferentialDriveKinematics.h"
#include "frc/kinematics/DifferentialDriveWheelPositions.h"
#include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
#include "frc/kinematics/Odometry.h"
#include "units/length.h"
namespace frc {
@@ -22,7 +26,9 @@ namespace frc {
* It is important that you reset your encoders to zero before using this class.
* Any subsequent pose resets also require the encoders to be reset to zero.
*/
class WPILIB_DLLEXPORT DifferentialDriveOdometry {
class WPILIB_DLLEXPORT DifferentialDriveOdometry
: public Odometry<DifferentialDriveWheelSpeeds,
DifferentialDriveWheelPositions> {
public:
/**
* Constructs a DifferentialDriveOdometry object.
@@ -56,20 +62,13 @@ class WPILIB_DLLEXPORT DifferentialDriveOdometry {
*/
void ResetPosition(const Rotation2d& gyroAngle, units::meter_t leftDistance,
units::meter_t rightDistance, const Pose2d& pose) {
m_pose = pose;
m_previousAngle = pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
m_prevLeftDistance = leftDistance;
m_prevRightDistance = rightDistance;
Odometry<DifferentialDriveWheelSpeeds,
DifferentialDriveWheelPositions>::ResetPosition(gyroAngle,
{leftDistance,
rightDistance},
pose);
}
/**
* Returns the position of the robot on the field.
* @return The pose of the robot.
*/
const Pose2d& GetPose() const { return m_pose; }
/**
* Updates the robot position on the field using distance measurements from
* encoders. This method is more numerically accurate than using velocities to
@@ -82,15 +81,14 @@ class WPILIB_DLLEXPORT DifferentialDriveOdometry {
* @return The new pose of the robot.
*/
const Pose2d& Update(const Rotation2d& gyroAngle, units::meter_t leftDistance,
units::meter_t rightDistance);
units::meter_t rightDistance) {
return Odometry<DifferentialDriveWheelSpeeds,
DifferentialDriveWheelPositions>::Update(gyroAngle,
{leftDistance,
rightDistance});
}
private:
Pose2d m_pose;
Rotation2d m_gyroOffset;
Rotation2d m_previousAngle;
units::meter_t m_prevLeftDistance = 0_m;
units::meter_t m_prevRightDistance = 0_m;
DifferentialDriveKinematics m_kinematicsImpl{units::meter_t{1}};
};
} // namespace frc