Revert "Servo and USART only servo command realized"

This reverts commit e2f6b9cffb64467d1d9271622af746d3205ee33c.