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 initial heading behavior in HolonomicDriveController (#3290)

Browse Source
This commit is contained in:
Prateek Machiraju
2021-04-19 00:00:11 -04:00
committed by GitHub
parent 659b37ef9d
commit aaf24e2552
5 changed files with 47 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -31,6 +31,8 @@ public class HolonomicDriveController {
private final PIDController m_yController;
private final ProfiledPIDController m_thetaController;
private boolean m_firstRun = true;
/**
* Constructs a holonomic drive controller.
*
@@ -82,6 +84,13 @@ public class HolonomicDriveController {
@SuppressWarnings("LocalVariableName")
public ChassisSpeeds calculate(
Pose2d currentPose, Pose2d poseRef, double linearVelocityRefMeters, Rotation2d angleRef) {
// If this is the first run, then we need to reset the theta controller to the current pose's
// heading.
if (m_firstRun) {
m_thetaController.reset(currentPose.getRotation().getRadians());
m_firstRun = false;
}
// Calculate feedforward velocities (field-relative).
double xFF = linearVelocityRefMeters * poseRef.getRotation().getCos();
double yFF = linearVelocityRefMeters * poseRef.getRotation().getSin();