Working PID!!! Needs tuning, though...
Wierd crash happened also. May be serial output related?