Function feedforward_pid 

pub fn feedforward_pid(
    setpoint: f64,
    actual: f64,
    kp: f64,
    ki: f64,
    kd: f64,
    kf: f64,
    prev_setpoint: f64,
    dt_s: f64,
) -> f64

Feed-Forward + PID Controller for high-speed trajectory tracking