various improvements, added untested servo stuff