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

[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,