Adjusted PID Parameters, removed debug outputs