PID tuning + little fixes