Incorporate Ki into the integral early - this allows tuning changes without causing a big output jump
Incorporate Kp and Kd early too for consistency - this will also help when interpreting logs
Added fine control when we are close to the setpoint - integral will change half as fast when difference is less than PIDFineControl