Added thread safety to USB TX, servo control thread, primitive ADC thread, python test codes

servo_controller/src/command_message.h

@@ -8,12 +8,20 @@
 #define COMMAND_PREFIX 0x69
 #define COMMAND_ID 0x00
 #define COMMAND_DATA_SIZE 160
+#define COMMAND_HEADER_SIZE 5
 
 typedef enum {
 	COMMAND_ACK,
 	COMMAND_NACK,
 	LED_TOGGLE,
 	LED_SET,
+	SERVO_SET,
+	SERVO_SET_ALL,
+	ADC_READ,
+	ADC_READ_ALL,
+
+	// Keep last
+	NUM_COMMANDS,
 } commands_e;
 
 struct command_message_t {
@@ -40,7 +48,7 @@ void command_message_init(struct command_message_t *msg);
  * @param command Command type
  * @param data Data payload
  */
-void command_create_message(struct command_message_t *msg, uint8_t length, commands_e command, uint8_t data[160]);
+void command_create_message(struct command_message_t *msg, uint8_t length, commands_e command, uint8_t data[]);
 
 /**
  * @brief Calculate CRC for command message