Fix UART code, only set motor speed once in main