Linear PID, cleaner version