added more debug info
tuned PID parameters