10 #include <units/units.h>
12 #include "frc/geometry/Pose2d.h"
13 #include "frc/kinematics/MecanumDriveKinematics.h"
14 #include "frc/kinematics/MecanumDriveWheelSpeeds.h"
15 #include "frc2/Timer.h"
53 m_gyroOffset = m_pose.
Rotation() - gyroAngle;
103 units::second_t m_previousTime = -1_s;
static units::second_t GetFPGATimestamp()
Return the FPGA system clock time in seconds.
MecanumDriveOdometry(MecanumDriveKinematics kinematics, const Rotation2d &gyroAngle, const Pose2d &initialPose=Pose2d())
Constructs a MecanumDriveOdometry object.
const Pose2d & Update(const Rotation2d &gyroAngle, MecanumDriveWheelSpeeds wheelSpeeds)
Updates the robot's position on the field using forward kinematics and integration of the pose over t...
Definition: MecanumDriveOdometry.h:93
A rotation in a 2d coordinate frame represented a point on the unit circle (cosine and sine).
Definition: Rotation2d.h:22
Represents the wheel speeds for a mecanum drive drivetrain.
Definition: MecanumDriveWheelSpeeds.h:16
void ResetPosition(const Pose2d &pose, const Rotation2d &gyroAngle)
Resets the robot's position on the field.
Definition: MecanumDriveOdometry.h:50
Represents a 2d pose containing translational and rotational elements.
Definition: Pose2d.h:23
const Pose2d & GetPose() const
Returns the position of the robot on the field.
Definition: MecanumDriveOdometry.h:60
const Pose2d & UpdateWithTime(units::second_t currentTime, const Rotation2d &gyroAngle, MecanumDriveWheelSpeeds wheelSpeeds)
Updates the robot's position on the field using forward kinematics and integration of the pose over t...
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
Class for mecanum drive odometry.
Definition: MecanumDriveOdometry.h:28
Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual wheel s...
Definition: MecanumDriveKinematics.h:42