Implement ProfiledPIDController

[LiranBratt2121]

📌 Task: Implement ProfiledPIDController by Extending PIDController

🧠 Objective:

Create a ProfiledPIDController subclass that reuses your existing PIDController core and adds motion-profiling constraints (max velocity/acceleration) on top of it.

✅ Functionality Requirements:
Extend the base class

class ProfiledPIDController(
    kP: Double,
    kI: Double,
    kD: Double,
    val maxVelocity: Double,
    val maxAcceleration: Double,
    tolerance: Double = 1e-3
) : PIDController(kP, kI, kD, tolerance) {
    // profiling-specific state & methods
}

• Inherit reset(), update(measured, target, dt): Double, and atSetpoint().

• Override update(...) to:

Compute a profiled target velocity (e.g., trapezoidal profile) based on current setpoint, maxVelocity, maxAcceleration, and dt.

Call super.update(measured, profiledTarget, dt) to get the PID output.

Return that output.

Internal profiling state

Track currentProfiledPosition and currentProfiledVelocity.

In reset(), call super.reset() then clear profiling state.

Maintain atSetpoint

You may either:

Use the inherited atSetpoint() (based on position error), or

Override it to also check velocity is near zero.

🧪 Definition of Done (DOD):

ProfiledPIDController extends PIDController and compiles without errors.

Profiling logic lives only in the subclass; no core PID code is duplicated.

reset() clears both PID and profiling state.

update(...) enforces maxVelocity and maxAcceleration, then delegates to super.update.

atSetpoint() correctly reflects when the motion profile and position error are within tolerance.

A simple example or unit test demonstrates smooth, constrained motion using the profiled controller.