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

[wpimath] Position Delta Odometry for Swerve (#4493)

Browse Source
This commit is contained in:
Jordan McMichael
2022-10-25 15:28:36 -04:00
committed by GitHub
parent fe400f68c5
commit 4170ec6107
35 changed files with 1508 additions and 277 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -12,7 +12,7 @@
#include <wpi/timestamp.h>
#include "SwerveDriveKinematics.h"
#include "SwerveModuleState.h"
#include "SwerveModulePosition.h"
#include "frc/geometry/Pose2d.h"
#include "units/time.h"
@@ -35,11 +35,13 @@ class SwerveDriveOdometry {
*
* @param kinematics The swerve drive kinematics for your drivetrain.
* @param gyroAngle The angle reported by the gyroscope.
* @param modulePositions The wheel positions reported by each module.
* @param initialPose The starting position of the robot on the field.
*/
SwerveDriveOdometry(SwerveDriveKinematics<NumModules> kinematics,
const Rotation2d& gyroAngle,
const Pose2d& initialPose = Pose2d{});
SwerveDriveOdometry(
SwerveDriveKinematics<NumModules> kinematics, const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules> modulePositions,
const Pose2d& initialPose = Pose2d{});
/**
* Resets the robot's position on the field.
@@ -49,12 +51,25 @@ class SwerveDriveOdometry {
*
* @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.
*/
void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
m_pose = pose;
m_previousAngle = pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
}
template <typename... ModulePositions>
void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle,
ModulePositions&&... wheelPositions);
/**
* 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 modulePositions The wheel positions reported by each module.
*/
void ResetPosition(
const Pose2d& pose, const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules> modulePositions);
/**
* Returns the position of the robot on the field.
@@ -70,47 +85,44 @@ class SwerveDriveOdometry {
* also takes in an angle parameter which is used instead of the
* angular rate that is calculated from forward kinematics.
*
* @param currentTime The current time.
* @param gyroAngle The angle reported by the gyroscope.
* @param moduleStates The current state of all swerve modules. Please provide
* the states in the same order in which you instantiated
* your SwerveDriveKinematics.
* @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... ModuleStates>
const Pose2d& UpdateWithTime(units::second_t currentTime,
const Rotation2d& gyroAngle,
ModuleStates&&... moduleStates);
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 automatically calculates
* the current time 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
* 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 moduleStates The current state of all swerve modules. Please provide
* the states in the same order in which you instantiated
* your SwerveDriveKinematics.
* @param modulePositions 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... ModuleStates>
const Pose2d& Update(const Rotation2d& gyroAngle,
ModuleStates&&... moduleStates) {
return UpdateWithTime(wpi::Now() * 1.0e-6_s, gyroAngle, moduleStates...);
}
const Pose2d& Update(
const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules> modulePositions);
private:
SwerveDriveKinematics<NumModules> m_kinematics;
Pose2d m_pose;
units::second_t m_previousTime = -1_s;
Rotation2d m_previousAngle;
Rotation2d m_gyroOffset;
wpi::array<SwerveModulePosition, NumModules> m_previousModulePositions;
};
extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)