Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01: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.
*