WPILibC++  2020.3.2
SwerveDriveOdometry.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 <chrono>
11 #include <cstddef>
12 #include <ctime>
13 
14 #include <units/units.h>
15 
16 #include "SwerveDriveKinematics.h"
17 #include "SwerveModuleState.h"
18 #include "frc/geometry/Pose2d.h"
19 #include "frc2/Timer.h"
20 
21 namespace frc {
22 
32 template <size_t NumModules>
34  public:
43  const Rotation2d& gyroAngle,
44  const Pose2d& initialPose = Pose2d());
45 
55  void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
56  m_pose = pose;
57  m_previousAngle = pose.Rotation();
58  m_gyroOffset = m_pose.Rotation() - gyroAngle;
59  }
60 
65  const Pose2d& GetPose() const { return m_pose; }
66 
83  template <typename... ModuleStates>
84  const Pose2d& UpdateWithTime(units::second_t currentTime,
85  const Rotation2d& gyroAngle,
86  ModuleStates&&... moduleStates);
87 
103  template <typename... ModuleStates>
104  const Pose2d& Update(const Rotation2d& gyroAngle,
105  ModuleStates&&... moduleStates) {
106  return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), gyroAngle,
107  moduleStates...);
108  }
109 
110  private:
112  Pose2d m_pose;
113 
114  units::second_t m_previousTime = -1_s;
115  Rotation2d m_previousAngle;
116  Rotation2d m_gyroOffset;
117 };
118 
119 } // namespace frc
120 
121 #include "SwerveDriveOdometry.inc"
frc::SwerveDriveOdometry
Class for swerve drive odometry.
Definition: SwerveDriveOdometry.h:33
frc::SwerveDriveOdometry::GetPose
const Pose2d & GetPose() const
Returns the position of the robot on the field.
Definition: SwerveDriveOdometry.h:65
frc2::Timer::GetFPGATimestamp
static units::second_t GetFPGATimestamp()
Return the FPGA system clock time in seconds.
frc::SwerveDriveOdometry::SwerveDriveOdometry
SwerveDriveOdometry(SwerveDriveKinematics< NumModules > kinematics, const Rotation2d &gyroAngle, const Pose2d &initialPose=Pose2d())
Constructs a SwerveDriveOdometry object.
Definition: SwerveDriveOdometry.inc:14
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::SwerveDriveOdometry::ResetPosition
void ResetPosition(const Pose2d &pose, const Rotation2d &gyroAngle)
Resets the robot's position on the field.
Definition: SwerveDriveOdometry.h:55
frc::SwerveDriveOdometry::UpdateWithTime
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
frc::SwerveDriveKinematics
Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual module ...
Definition: SwerveDriveKinematics.h:47
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::SwerveDriveOdometry::Update
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