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] ChassisSpeeds fromRelative and discretize methods made instance methods (#7115)

Browse Source
This commit is contained in:
Nicholas Armstrong
2024-10-22 19:23:17 -04:00
committed by GitHub
parent cbc9264468
commit a3b12b3bd9
8 changed files with 127 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java
View File

@@ -102,9 +102,10 @@ public class HolonomicDriveController {
m_poseError = trajectoryPose.relativeTo(currentPose);
m_rotationError = desiredHeading.minus(currentPose.getRotation());
ChassisSpeeds speeds = new ChassisSpeeds(xFF, yFF, thetaFF);
if (!m_enabled) {
return ChassisSpeeds.fromFieldRelativeSpeeds(xFF, yFF, thetaFF, currentPose.getRotation());
speeds.toRobotRelativeSpeeds(currentPose.getRotation());
return speeds;
}
// Calculate feedback velocities (based on position error).
@@ -112,8 +113,10 @@ public class HolonomicDriveController {
double yFeedback = m_yController.calculate(currentPose.getY(), trajectoryPose.getY());
// Return next output.
return ChassisSpeeds.fromFieldRelativeSpeeds(
xFF + xFeedback, yFF + yFeedback, thetaFF, currentPose.getRotation());
speeds.vxMetersPerSecond += xFeedback;
speeds.vyMetersPerSecond += yFeedback;
speeds.toRobotRelativeSpeeds(currentPose.getRotation());
return speeds;
}
/**