Adjusting PID values and some debugging