mjansen4857 / pathplanner

A simple yet powerful path planning tool for FRC robots
https://pathplanner.dev
MIT License
393 stars 116 forks source link

C++ `PPHolonomicDriveController` ignores `rotationTargetOverride` #686

Closed bovlb closed 2 days ago

bovlb commented 4 months ago

From code inspection, the C++ implementation does not honour the rotation target override.

https://github.com/mjansen4857/pathplanner/blob/05a123d86f47976c564564674fdb997e25b03773/pathplannerlib/src/main/native/cpp/pathplanner/lib/controllers/PPHolonomicDriveController.cpp#L59-L68

Compare with he Java implementation:

https://github.com/mjansen4857/pathplanner/blob/05a123d86f47976c564564674fdb997e25b03773/pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PPHolonomicDriveController.java#L147-L156

I suggest that this should should instead say:

        units::radians_per_second_t rotationFeedback {
                        m_rotationController.Calculate(currentPose.Rotation().Radians(),
                                        targetRotation.Radians(),
                                        { maxAngVel,
                                                        referenceState.constraints.getMaxAngularAcceleration() }) };
        units::radians_per_second_t rotationFF =