Refactor continuous input logic from ProfiledPIDController into a MathUtil helper function

[WesleyJ-128]

Is your feature request related to a problem? Please describe. ProfiledPIDController allows for a trapezoidal motion profile to be used on a system with continuous input. However, TrapezoidProfile itself does not have this feature, meaning that using a TrapezoidProfile to feed a different control loop, such as a smart motor controller on a continuous mechanism is not natively supported.

Describe the solution you'd like Add an enableContinuousInput() method to TrapezoidProfile. This should be trivial to implement by adding the same distance test and modulus addition logic that PIDController and ProfiledPIDController use to modify the given goal and setpoint at the beginning of the TrapezoidProfile constructor.

Describe alternatives you've considered In my particular case, a ProfiledPIDController should be usable, but I've been having issues getting it to function correctly. However, this is still a useful feature for anyone who wants to use motion profiling with a motor controller PID loop.

Additional context My current solution. Note that here the maximum and minimum inputs have been hardcoded to pi and -pi. It should be simple to add the continuous input logic to the TrapezoidProfile constructor.

    // ------------------------------------------------------------------------------------------------------------------------------------
    // Get error which is the smallest distance between goal and measurement                                            |
    double goalMinDistance = //                                                                                         |
        MathUtil.inputModulus(angle - currentHeading.getRadians(), -Math.PI, Math.PI); //                               |
    double setpointMinDistance = //                                                                                     |
        MathUtil.inputModulus(setpoint.position - currentHeading.getRadians(), -Math.PI, Math.PI); //                   |
    //                                                                                                                  |
    // Recompute the profile goal with the smallest error, thus giving the shortest path. The goal      Taken from ProfiledPIDController
    // may be outside the input range after this operation, but that's OK because the controller            Applies continuous input
    // will still go there and report an error of zero. In other words, the setpoint only needs to                      |
    // be offset from the measurement by the input range modulus; they don't need to be equal.                          |
    angle = goalMinDistance + currentHeading.getRadians(); //                                                           |
    setpoint.position = setpointMinDistance + currentHeading.getRadians(); //                                           |
    // ------------------------------------------------------------------------------------------------------------------------------------

    TrapezoidProfile profile = new TrapezoidProfile(
      trapProfileConstraints,
      new TrapezoidProfile.State(angle, 0),
      setpoint);
    setpoint = profile.calculate(Constants.RIO_LOOP_TIME);

    /*omega = (Math.abs(currentHeading.getRadians() - angle) > Drivebase.HEADING_TOLERANCE) ?
      thetaController.calculate(currentHeading.getRadians(), angle) :
      0;*/

    omega = thetaController.calculate(currentHeading.getRadians(), setpoint.position) + setpoint.velocity;

If I was better able to test this and ensure it's working properly, I'd write a pull request instead.

[calcmogul]

Instead of adding this logic to every motion profile class (we have two now), it should probably be refactored into a separate MathUtil function that operates on a goal and setpoint.

[calcmogul]

Here's the profile wrapping code: https://github.com/wpilibsuite/allwpilib/blob/main/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java#L344-L356 https://github.com/wpilibsuite/allwpilib/blob/main/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h#L309-L323

The function can go here: https://github.com/wpilibsuite/allwpilib/blob/main/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java https://github.com/wpilibsuite/allwpilib/blob/main/wpimath/src/main/native/include/frc/MathUtil.h

This refactor would allow users to easily plumb the motion profile and PID controller themselves, so the profile implementation can be swapped out. Possible resolution to https://github.com/wpilibsuite/allwpilib/issues/5915.

Something like this is an option:

<State> State continuousProfileShortestGoal(double minimumPosition, double maximumPosition, State current, State goal);

but we can't really make it generic without a base class for the motion profile trajectory states, and adding that might be a breaking change that has to wait for 2025.

Perhaps things will get better after this refactor, which moves State to a common base class: https://github.com/jlmcmchl/allwpilib/blob/jmcmichael/motion-profile-rework/wpimath/src/main/java/edu/wpi/first/math/trajectory/MotionProfile.java