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

[wpilib] Fix HolonomicDriveController atReference() behavior (#3163)

Browse Source 
The atReference() method previously used the rotation error between the
desired trajectory state and the current pose. This was a bug because we
allow teams to use custom rotation setpoints and that wasn't being taken
into account.
This commit is contained in:
Prateek Machiraju
2021-02-13 01:11:57 -05:00
committed by GitHub
parent fe5c2cf4b7
commit f82aa1d564
3 changed files with 6 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/HolonomicDriveController.java
View File

@@ -23,6 +23,7 @@ import edu.wpi.first.wpilibj.trajectory.Trajectory;
@SuppressWarnings("MemberName")
public class HolonomicDriveController {
private Pose2d m_poseError = new Pose2d();
private Rotation2d m_rotationError = new Rotation2d();
private Pose2d m_poseTolerance = new Pose2d();
private boolean m_enabled = true;
@@ -52,7 +53,7 @@ public class HolonomicDriveController {
*/
public boolean atReference() {
final var eTranslate = m_poseError.getTranslation();
final var eRotate = m_poseError.getRotation();
final var eRotate = m_rotationError;
final var tolTranslate = m_poseTolerance.getTranslation();
final var tolRotate = m_poseTolerance.getRotation();
return Math.abs(eTranslate.getX()) < tolTranslate.getX()
@@ -88,6 +89,7 @@ public class HolonomicDriveController {
m_thetaController.calculate(currentPose.getRotation().getRadians(), angleRef.getRadians());
m_poseError = poseRef.relativeTo(currentPose);
m_rotationError = angleRef.minus(currentPose.getRotation());
if (!m_enabled) {
return ChassisSpeeds.fromFieldRelativeSpeeds(xFF, yFF, thetaFF, currentPose.getRotation());