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

[wpimath] Rework function signatures for Pose Estimation / Odometry (#4642)

Browse Source 
Forcing the use of SwerveModulePostition[] and wpi::array<SwerveModulePosition, len> in place of variadic function signatures.
This commit is contained in:
Jordan McMichael
2022-11-17 13:36:33 -05:00
committed by GitHub
parent 0401597d3b
commit ce4c45df13
9 changed files with 84 additions and 104 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -43,20 +43,6 @@ class SwerveDriveOdometry {
const wpi::array<SwerveModulePosition, NumModules> modulePositions,
const Pose2d& initialPose = Pose2d{});
/**
* Resets the robot's position on the field.
*
* 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 wheel positions reported by each module.
*/
template <typename... ModulePositions>
void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle,
ModulePositions&&... wheelPositions);
/**
* Resets the robot's position on the field.
*
@@ -77,25 +63,6 @@ class SwerveDriveOdometry {
*/
const Pose2d& GetPose() const { return m_pose; }
/**
* Updates the robot's position on the field using forward kinematics and
* integration of the pose over time. This method takes in the current time as
* a parameter to calculate period (difference between two timestamps). The
* period is used to calculate the change in distance from a velocity. This
* also takes in an angle parameter which is used instead of the
* angular rate that is calculated from forward kinematics.
*
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelPositions The current position of all swerve modules. Please
* provide the positions in the same order in which you instantiated your
* SwerveDriveKinematics.
*
* @return The new pose of the robot.
*/
template <typename... ModulePositions>
const Pose2d& Update(const Rotation2d& gyroAngle,
ModulePositions&&... wheelPositions);
/**
* Updates the robot's position on the field using forward kinematics and
* integration of the pose over time. This method takes in the current time as

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

@@ -22,20 +22,6 @@ SwerveDriveOdometry<NumModules>::SwerveDriveOdometry(
wpi::math::MathUsageId::kOdometry_SwerveDrive, 1);
}
template <size_t NumModules>
template <typename... ModulePositions>
void SwerveDriveOdometry<NumModules>::ResetPosition(
const Pose2d& pose, const Rotation2d& gyroAngle,
ModulePositions&&... wheelPositions) {
static_assert(sizeof...(wheelPositions) == NumModules,
"Number of modules is not consistent with number of wheel "
"locations provided in constructor.");
wpi::array<SwerveModulePosition, NumModules> modulePositions{
wheelPositions...};
this->ResetPosition(pose, gyroAngle, modulePositions);
}
template <size_t NumModules>
void SwerveDriveOdometry<NumModules>::ResetPosition(
const Pose2d& pose, const Rotation2d& gyroAngle,
@@ -46,20 +32,6 @@ void SwerveDriveOdometry<NumModules>::ResetPosition(
m_previousModulePositions = modulePositions;
}
template <size_t NumModules>
template <typename... ModulePositions>
const Pose2d& frc::SwerveDriveOdometry<NumModules>::Update(
const Rotation2d& gyroAngle, ModulePositions&&... wheelPositions) {
static_assert(sizeof...(wheelPositions) == NumModules,
"Number of modules is not consistent with number of wheel "
"locations provided in constructor.");
wpi::array<SwerveModulePosition, NumModules> modulePositions{
wheelPositions...};
return this->Update(gyroAngle, modulePositions);
}
template <size_t NumModules>
const Pose2d& frc::SwerveDriveOdometry<NumModules>::Update(
const Rotation2d& gyroAngle,