14 #include <units/units.h>
16 #include "SwerveDriveKinematics.h"
17 #include "SwerveModuleState.h"
18 #include "frc/geometry/Pose2d.h"
19 #include "frc2/Timer.h"
32 template <
size_t NumModules>
58 m_gyroOffset = m_pose.
Rotation() - gyroAngle;
83 template <
typename... ModuleStates>
86 ModuleStates&&... moduleStates);
103 template <
typename... ModuleStates>
105 ModuleStates&&... moduleStates) {
114 units::second_t m_previousTime = -1_s;
121 #include "SwerveDriveOdometry.inc"
Class for swerve drive odometry.
Definition: SwerveDriveOdometry.h:33
const Pose2d & GetPose() const
Returns the position of the robot on the field.
Definition: SwerveDriveOdometry.h:65
static units::second_t GetFPGATimestamp()
Return the FPGA system clock time in seconds.
SwerveDriveOdometry(SwerveDriveKinematics< NumModules > kinematics, const Rotation2d &gyroAngle, const Pose2d &initialPose=Pose2d())
Constructs a SwerveDriveOdometry object.
Definition: SwerveDriveOdometry.inc:14
A rotation in a 2d coordinate frame represented a point on the unit circle (cosine and sine).
Definition: Rotation2d.h:22
Represents a 2d pose containing translational and rotational elements.
Definition: Pose2d.h:23
void ResetPosition(const Pose2d &pose, const Rotation2d &gyroAngle)
Resets the robot's position on the field.
Definition: SwerveDriveOdometry.h:55
const Pose2d & UpdateWithTime(units::second_t currentTime, const Rotation2d &gyroAngle, ModuleStates &&... moduleStates)
Updates the robot's position on the field using forward kinematics and integration of the pose over t...
Definition: SwerveDriveOdometry.inc:26
Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual module ...
Definition: SwerveDriveKinematics.h:47
const Rotation2d & Rotation() const
Returns the underlying rotation.
Definition: Pose2d.h:111
A class that enforces constraints on the differential drive kinematics.
Definition: SPIAccelerometerSim.h:16
const Pose2d & Update(const Rotation2d &gyroAngle, ModuleStates &&... moduleStates)
Updates the robot's position on the field using forward kinematics and integration of the pose over t...
Definition: SwerveDriveOdometry.h:104