Java Example: Override Target Rotation

There are some cases where you may wish to override the target rotation while path following, such as targeting a game piece. This can be accomplished by providing a function to the PPHolonomicDriveController class that will supply an optional rotation override. This is a static method so it will apply to all path following commands.

NOTE: If you supply a method to override the rotation target, it will be called every loop. You must make sure to return an empty optional when you do not wish to override the rotation.

public SwerveSubsystem(){
    // ... other initialization

    // Set the method that will be used to get rotation overrides
    PPHolonomicDriveController.setRotationTargetOverride(this::getRotationTargetOverride);
}

public Optional<Rotation2d> getRotationTargetOverride(){
    // Some condition that should decide if we want to override rotation
    if(Limelight.hasGamePieceTarget()) {
        // Return an optional containing the rotation override (this should be a field relative rotation)
        return Optional.of(Limelight.getRobotToGamePieceRotation());
    } else {
        // return an empty optional when we don't want to override the path's rotation
        return Optional.empty();
    }
}