All kinds of small fixes, multithread servo writing, fix steering and motor pwm