Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-05 03:21:42 +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

3
wpilibc/src/main/native/cpp/controller/HolonomicDriveController.cpp
View File

@@ -19,7 +19,7 @@ HolonomicDriveController::HolonomicDriveController(
bool HolonomicDriveController::AtReference() const {
const auto& eTranslate = m_poseError.Translation();
const auto& eRotate = m_poseError.Rotation();
const auto& eRotate = m_rotationError;
const auto& tolTranslate = m_poseTolerance.Translation();
const auto& tolRotate = m_poseTolerance.Rotation();
return units::math::abs(eTranslate.X()) < tolTranslate.X() &&
@@ -41,6 +41,7 @@ ChassisSpeeds HolonomicDriveController::Calculate(
currentPose.Rotation().Radians(), angleRef.Radians()));
m_poseError = poseRef.RelativeTo(currentPose);
m_rotationError = angleRef - currentPose.Rotation();
if (!m_enabled) {
return ChassisSpeeds::FromFieldRelativeSpeeds(xFF, yFF, thetaFF,

1
wpilibc/src/main/native/include/frc/controller/HolonomicDriveController.h
View File

@@ -98,6 +98,7 @@ class HolonomicDriveController {
private:
Pose2d m_poseError;
Rotation2d m_rotationError;
Pose2d m_poseTolerance;
bool m_enabled = true;

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());