Fixes servo code and firmware, updates example