WPILibC++  2020.3.2
DifferentialDriveOdometry.h
1 /*----------------------------------------------------------------------------*/
2 /* Copyright (c) 2019 FIRST. All Rights Reserved. */
3 /* Open Source Software - may be modified and shared by FRC teams. The code */
4 /* must be accompanied by the FIRST BSD license file in the root directory of */
5 /* the project. */
6 /*----------------------------------------------------------------------------*/
7 
8 #pragma once
9 
10 #include <units/units.h>
11 
12 #include "DifferentialDriveKinematics.h"
13 #include "frc/geometry/Pose2d.h"
14 #include "frc2/Timer.h"
15 
16 namespace frc {
30  public:
37  explicit DifferentialDriveOdometry(const Rotation2d& gyroAngle,
38  const Pose2d& initialPose = Pose2d());
39 
51  void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
52  m_pose = pose;
53  m_previousAngle = pose.Rotation();
54  m_gyroOffset = m_pose.Rotation() - gyroAngle;
55 
56  m_prevLeftDistance = 0_m;
57  m_prevRightDistance = 0_m;
58  }
59 
64  const Pose2d& GetPose() const { return m_pose; }
65 
77  const Pose2d& Update(const Rotation2d& gyroAngle, units::meter_t leftDistance,
78  units::meter_t rightDistance);
79 
80  private:
81  Pose2d m_pose;
82 
83  Rotation2d m_gyroOffset;
84  Rotation2d m_previousAngle;
85 
86  units::meter_t m_prevLeftDistance = 0_m;
87  units::meter_t m_prevRightDistance = 0_m;
88 };
89 } // namespace frc
frc::DifferentialDriveOdometry::Update
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.
frc::DifferentialDriveOdometry::ResetPosition
void ResetPosition(const Pose2d &pose, const Rotation2d &gyroAngle)
Resets the robot's position on the field.
Definition: DifferentialDriveOdometry.h:51
frc::DifferentialDriveOdometry::GetPose
const Pose2d & GetPose() const
Returns the position of the robot on the field.
Definition: DifferentialDriveOdometry.h:64
frc::DifferentialDriveOdometry::DifferentialDriveOdometry
DifferentialDriveOdometry(const Rotation2d &gyroAngle, const Pose2d &initialPose=Pose2d())
Constructs a DifferentialDriveOdometry object.
frc::Rotation2d
A rotation in a 2d coordinate frame represented a point on the unit circle (cosine and sine).
Definition: Rotation2d.h:22
frc::Pose2d
Represents a 2d pose containing translational and rotational elements.
Definition: Pose2d.h:23
frc::Pose2d::Rotation
const Rotation2d & Rotation() const
Returns the underlying rotation.
Definition: Pose2d.h:111
frc::DifferentialDriveOdometry
Class for differential drive odometry.
Definition: DifferentialDriveOdometry.h:29
frc
A class that enforces constraints on the differential drive kinematics.
Definition: SPIAccelerometerSim.h:16