PID Algorithm changes