Better scaling of kp, clamping integral term, real PID (not multiplied)