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

[wpimath] Rework odometry APIs to improve feature parity (#4645)

Browse Source 
Co-authored-by: Ryan Blue <ryanzblue@gmail.com>
This commit is contained in:
Jordan McMichael
2022-11-18 23:42:00 -05:00
committed by GitHub
parent e2d49181da
commit 902e8686d3
53 changed files with 266 additions and 157 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -27,24 +27,35 @@ class WPILIB_DLLEXPORT DifferentialDriveOdometry {
/**
* Constructs a DifferentialDriveOdometry object.
*
* IF leftDistance and rightDistance are unspecified,
* You NEED to reset your encoders (to zero).
*
* @param gyroAngle The angle reported by the gyroscope.
* @param leftDistance The distance traveled by the left encoder.
* @param rightDistance The distance traveled by the right encoder.
* @param initialPose The starting position of the robot on the field.
*/
explicit DifferentialDriveOdometry(const Rotation2d& gyroAngle,
units::meter_t leftDistance,
units::meter_t rightDistance,
const Pose2d& initialPose = Pose2d{});
/**
* Resets the robot's position on the field.
*
* You NEED to reset your encoders (to zero) when calling this method.
* IF leftDistance and rightDistance are unspecified,
* You NEED to reset your encoders (to zero).
*
* The gyroscope angle does not need to be reset here on the user's robot
* code. The library automatically takes care of offsetting the gyro angle.
*
* @param pose The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
* @param leftDistance The distance traveled by the left encoder.
* @param rightDistance The distance traveled by the right encoder.
*/
void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
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;

17
wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h
View File

@@ -33,10 +33,10 @@ class WPILIB_DLLEXPORT MecanumDriveOdometry {
* @param wheelPositions The current distances measured by each wheel.
* @param initialPose The starting position of the robot on the field.
*/
explicit MecanumDriveOdometry(MecanumDriveKinematics kinematics,
const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions wheelPositions,
const Pose2d& initialPose = Pose2d{});
explicit MecanumDriveOdometry(
MecanumDriveKinematics kinematics, const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions,
const Pose2d& initialPose = Pose2d{});
/**
* Resets the robot's position on the field.
@@ -44,12 +44,13 @@ class WPILIB_DLLEXPORT MecanumDriveOdometry {
* The gyroscope angle does not need to be reset here on the user's robot
* code. The library automatically takes care of offsetting the gyro angle.
*
* @param pose The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelPositions The current distances measured by each wheel.
* @param pose The position on the field that your robot is at.
*/
void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions wheelPositions) {
void ResetPosition(const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions,
const Pose2d& pose) {
m_pose = pose;
m_previousAngle = pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
@@ -74,7 +75,7 @@ class WPILIB_DLLEXPORT MecanumDriveOdometry {
* @return The new pose of the robot.
*/
const Pose2d& Update(const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions wheelPositions);
const MecanumDriveWheelPositions& wheelPositions);
private:
MecanumDriveKinematics m_kinematics;

11
wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
View File

@@ -40,7 +40,7 @@ class SwerveDriveOdometry {
*/
SwerveDriveOdometry(
SwerveDriveKinematics<NumModules> kinematics, const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules> modulePositions,
const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
const Pose2d& initialPose = Pose2d{});
/**
@@ -49,13 +49,14 @@ class SwerveDriveOdometry {
* The gyroscope angle does not need to be reset here on the user's robot
* code. The library automatically takes care of offsetting the gyro angle.
*
* @param pose The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
* @param modulePositions The wheel positions reported by each module.
* @param pose The position on the field that your robot is at.
*/
void ResetPosition(
const Pose2d& pose, const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules> modulePositions);
const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
const Pose2d& pose);
/**
* Returns the position of the robot on the field.
@@ -80,7 +81,7 @@ class SwerveDriveOdometry {
*/
const Pose2d& Update(
const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules> modulePositions);
const wpi::array<SwerveModulePosition, NumModules>& modulePositions);
private:
SwerveDriveKinematics<NumModules> m_kinematics;

9
wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
View File

@@ -11,7 +11,7 @@ namespace frc {
template <size_t NumModules>
SwerveDriveOdometry<NumModules>::SwerveDriveOdometry(
SwerveDriveKinematics<NumModules> kinematics, const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules> modulePositions,
const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
const Pose2d& initialPose)
: m_kinematics(kinematics),
m_pose(initialPose),
@@ -24,8 +24,9 @@ SwerveDriveOdometry<NumModules>::SwerveDriveOdometry(
template <size_t NumModules>
void SwerveDriveOdometry<NumModules>::ResetPosition(
const Pose2d& pose, const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules> modulePositions) {
const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
const Pose2d& pose) {
m_pose = pose;
m_previousAngle = pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
@@ -35,7 +36,7 @@ void SwerveDriveOdometry<NumModules>::ResetPosition(
template <size_t NumModules>
const Pose2d& frc::SwerveDriveOdometry<NumModules>::Update(
const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules> modulePositions) {
const wpi::array<SwerveModulePosition, NumModules>& modulePositions) {
auto moduleDeltas =
wpi::array<SwerveModulePosition, NumModules>(wpi::empty_array);
for (size_t index = 0; index < modulePositions.size(); index++) {