Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-23 01:21:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add reset methods to Odometry (#6702)

Browse Source
This commit is contained in:
Jade
2024-07-09 09:25:42 +08:00
committed by GitHub
parent 7f6ba54b68
commit dc276b651b
2 changed files with 65 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
wpimath/src/main/native/include/frc/kinematics/Odometry.h
View File

@@ -7,6 +7,8 @@
#include <wpi/SymbolExports.h>
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
#include "frc/geometry/Translation2d.h"
#include "frc/kinematics/Kinematics.h"
#include "frc/kinematics/WheelPositions.h"
@@ -58,6 +60,37 @@ class WPILIB_DLLEXPORT Odometry {
m_previousWheelPositions = wheelPositions;
}
/**
* Resets the pose.
*
* @param pose The pose to reset to.
*/
void ResetPose(const Pose2d& pose) {
m_gyroOffset = m_gyroOffset + (pose.Rotation() - m_pose.Rotation());
m_pose = pose;
m_previousAngle = pose.Rotation();
}
/**
* Resets the translation of the pose.
*
* @param translation The translation to reset to.
*/
void ResetTranslation(const Translation2d& translation) {
m_pose = Pose2d{translation, m_pose.Rotation()};
}
/**
* Resets the rotation of the pose.
*
* @param rotation The rotation to reset to.
*/
void ResetRotation(const Rotation2d& rotation) {
m_gyroOffset = m_gyroOffset + (rotation - m_pose.Rotation());
m_pose = Pose2d{m_pose.Translation(), rotation};
m_previousAngle = rotation;
}
/**
* Returns the position of the robot on the field.
* @return The pose of the robot.