10 #include <units/units.h>
12 #include "DifferentialDriveKinematics.h"
13 #include "frc/geometry/Pose2d.h"
14 #include "frc2/Timer.h"
54 m_gyroOffset = m_pose.
Rotation() - gyroAngle;
56 m_prevLeftDistance = 0_m;
57 m_prevRightDistance = 0_m;
78 units::meter_t rightDistance);
86 units::meter_t m_prevLeftDistance = 0_m;
87 units::meter_t m_prevRightDistance = 0_m;
const Pose2d & Update(const Rotation2d &gyroAngle, units::meter_t leftDistance, units::meter_t rightDistance)
Updates the robot position on the field using distance measurements from encoders.
void ResetPosition(const Pose2d &pose, const Rotation2d &gyroAngle)
Resets the robot's position on the field.
Definition: DifferentialDriveOdometry.h:51
const Pose2d & GetPose() const
Returns the position of the robot on the field.
Definition: DifferentialDriveOdometry.h:64
DifferentialDriveOdometry(const Rotation2d &gyroAngle, const Pose2d &initialPose=Pose2d())
Constructs a DifferentialDriveOdometry object.
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
const Rotation2d & Rotation() const
Returns the underlying rotation.
Definition: Pose2d.h:111
Class for differential drive odometry.
Definition: DifferentialDriveOdometry.h:29
A class that enforces constraints on the differential drive kinematics.
Definition: SPIAccelerometerSim.h:16