-
Notifications
You must be signed in to change notification settings - Fork 162
Closed
Labels
PathPlannerLibChanges to PathPlannerLibChanges to PathPlannerLibbugSomething isn't workingSomething isn't working
Milestone
Description
From code inspection, the C++ implementation does not honour the rotation target override.
Lines 59 to 68 in 05a123d
| frc::Rotation2d targetRotation = referenceState.targetHolonomicRotation; | |
| if (rotationTargetOverride) { | |
| targetRotation = rotationTargetOverride().value_or(targetRotation); | |
| } | |
| units::radians_per_second_t rotationFeedback { | |
| m_rotationController.Calculate(currentPose.Rotation().Radians(), | |
| referenceState.targetHolonomicRotation.Radians(), | |
| { maxAngVel, | |
| referenceState.constraints.getMaxAngularAcceleration() }) }; |
Compare with he Java implementation:
Lines 147 to 156 in 05a123d
| Rotation2d targetRotation = targetState.targetHolonomicRotation; | |
| if (rotationTargetOverride != null) { | |
| targetRotation = rotationTargetOverride.get().orElse(targetRotation); | |
| } | |
| double rotationFeedback = | |
| rotationController.calculate( | |
| currentPose.getRotation().getRadians(), | |
| new TrapezoidProfile.State(targetRotation.getRadians(), 0), | |
| rotationConstraints); |
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 =Metadata
Metadata
Assignees
Labels
PathPlannerLibChanges to PathPlannerLibChanges to PathPlannerLibbugSomething isn't workingSomething isn't working