WPILibC++  2020.3.2
MecanumDriveOdometry.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 "frc/geometry/Pose2d.h"
13 #include "frc/kinematics/MecanumDriveKinematics.h"
14 #include "frc/kinematics/MecanumDriveWheelSpeeds.h"
15 #include "frc2/Timer.h"
16 
17 namespace frc {
18 
29  public:
37  explicit MecanumDriveOdometry(MecanumDriveKinematics kinematics,
38  const Rotation2d& gyroAngle,
39  const Pose2d& initialPose = Pose2d());
40 
50  void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
51  m_pose = pose;
52  m_previousAngle = pose.Rotation();
53  m_gyroOffset = m_pose.Rotation() - gyroAngle;
54  }
55 
60  const Pose2d& GetPose() const { return m_pose; }
61 
76  const Pose2d& UpdateWithTime(units::second_t currentTime,
77  const Rotation2d& gyroAngle,
78  MecanumDriveWheelSpeeds wheelSpeeds);
79 
93  const Pose2d& Update(const Rotation2d& gyroAngle,
94  MecanumDriveWheelSpeeds wheelSpeeds) {
96  wheelSpeeds);
97  }
98 
99  private:
100  MecanumDriveKinematics m_kinematics;
101  Pose2d m_pose;
102 
103  units::second_t m_previousTime = -1_s;
104  Rotation2d m_previousAngle;
105  Rotation2d m_gyroOffset;
106 };
107 
108 } // namespace frc
frc2::Timer::GetFPGATimestamp
static units::second_t GetFPGATimestamp()
Return the FPGA system clock time in seconds.
frc::MecanumDriveOdometry::MecanumDriveOdometry
MecanumDriveOdometry(MecanumDriveKinematics kinematics, const Rotation2d &gyroAngle, const Pose2d &initialPose=Pose2d())
Constructs a MecanumDriveOdometry object.
frc::MecanumDriveOdometry::Update
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
frc::Rotation2d
A rotation in a 2d coordinate frame represented a point on the unit circle (cosine and sine).
Definition: Rotation2d.h:22
frc::MecanumDriveWheelSpeeds
Represents the wheel speeds for a mecanum drive drivetrain.
Definition: MecanumDriveWheelSpeeds.h:16
frc::MecanumDriveOdometry::ResetPosition
void ResetPosition(const Pose2d &pose, const Rotation2d &gyroAngle)
Resets the robot's position on the field.
Definition: MecanumDriveOdometry.h:50
frc::Pose2d
Represents a 2d pose containing translational and rotational elements.
Definition: Pose2d.h:23
frc::MecanumDriveOdometry::GetPose
const Pose2d & GetPose() const
Returns the position of the robot on the field.
Definition: MecanumDriveOdometry.h:60
frc::MecanumDriveOdometry::UpdateWithTime
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...
frc::Pose2d::Rotation
const Rotation2d & Rotation() const
Returns the underlying rotation.
Definition: Pose2d.h:111
frc
A class that enforces constraints on the differential drive kinematics.
Definition: SPIAccelerometerSim.h:16
frc::MecanumDriveOdometry
Class for mecanum drive odometry.
Definition: MecanumDriveOdometry.h:28
frc::MecanumDriveKinematics
Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual wheel s...
Definition: MecanumDriveKinematics.h:42