Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-28 02:11:43 +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

32
wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java
View File

@@ -6,6 +6,7 @@ package edu.wpi.first.math.kinematics;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
/**
* Class for odometry. Robot code should not use this directly- Instead, use the particular type for
@@ -63,6 +64,37 @@ public class Odometry<T> {
m_previousWheelPositions = m_kinematics.copy(wheelPositions);
}
/**
* Resets the pose.
*
* @param poseMeters The pose to reset to.
*/
public void resetPose(Pose2d poseMeters) {
m_gyroOffset = m_gyroOffset.plus(poseMeters.getRotation().minus(m_poseMeters.getRotation()));
m_poseMeters = poseMeters;
m_previousAngle = m_poseMeters.getRotation();
}
/**
* Resets the translation of the pose.
*
* @param translation The translation to reset to.
*/
public void resetTranslation(Translation2d translation) {
m_poseMeters = new Pose2d(translation, m_poseMeters.getRotation());
}
/**
* Resets the rotation of the pose.
*
* @param rotation The rotation to reset to.
*/
public void resetRotation(Rotation2d rotation) {
m_gyroOffset = m_gyroOffset.plus(rotation.minus(m_poseMeters.getRotation()));
m_poseMeters = new Pose2d(m_poseMeters.getTranslation(), rotation);
m_previousAngle = m_poseMeters.getRotation();
}
/**
* Returns the position of the robot on the field.
*

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.