diff --git a/MotorControl/commands.c b/MotorControl/commands.c
index 8674a256..83b0f3ee 100644
--- a/MotorControl/commands.c
+++ b/MotorControl/commands.c
@@ -3,11 +3,17 @@
 #include <usart.h>
 #include <freertos_vars.h>
 
+extern PCD_HandleTypeDef hpcd_USB_OTG_FS;
+
 /* Private macros ------------------------------------------------------------*/
 /* Private typedef -----------------------------------------------------------*/
 /* Global constant data ------------------------------------------------------*/
 /* Global variables ----------------------------------------------------------*/
-extern PCD_HandleTypeDef hpcd_USB_OTG_FS;
+// For now, this automatically updates to the interface that most
+// recently recieved a command. In the future we may want to separate
+// debug printf and the main serial comms.
+SerialPrintf_t serial_printf_select = SERIAL_PRINTF_IS_NONE;
+
 /* Private constant data -----------------------------------------------------*/
 // variables exposed to usb/serial interface via set/get/monitor
 // Note: this will be depricated soon
@@ -107,7 +113,10 @@ monitoring_slot monitoring_slots[20] = {0};
 static void print_monitoring(int limit);
 
 /* Function implementations --------------------------------------------------*/
-void motor_parse_cmd(uint8_t* buffer, int len) {
+void motor_parse_cmd(uint8_t* buffer, int len, SerialPrintf_t response_interface) {
+    // Set response interface
+    serial_printf_select = response_interface;
+
     // TODO very hacky way of terminating sscanf at end of buffer:
     // We should do some proper struct packing instead of using sscanf altogether
     buffer[len] = 0;
@@ -279,7 +288,7 @@ void usb_cmd_thread(void const * argument) {
                     if (c == '\r' || c == '\n' || c == '!') {
                         // End of command string: exchange end char with terminating null
                         parse_buffer[parse_buffer_idx-1] = '\0';
-                        motor_parse_cmd(parse_buffer, parse_buffer_idx);
+                        motor_parse_cmd(parse_buffer, parse_buffer_idx, SERIAL_PRINTF_IS_UART);
                         // Reset receieve state machine
                         reset_read_state = true;
                         break;
diff --git a/MotorControl/commands.h b/MotorControl/commands.h
index ec5e101a..a23756b8 100644
--- a/MotorControl/commands.h
+++ b/MotorControl/commands.h
@@ -1,10 +1,23 @@
 #ifndef COMMANDS_H
 #define COMMANDS_H
 
+/* Includes ------------------------------------------------------------------*/
 #include <low_level.h>
+/* Exported types ------------------------------------------------------------*/
+typedef enum {
+    SERIAL_PRINTF_IS_NONE,
+    SERIAL_PRINTF_IS_USB,
+    SERIAL_PRINTF_IS_UART,
+} SerialPrintf_t;
+
+/* Exported constants --------------------------------------------------------*/
+/* Exported variables --------------------------------------------------------*/
+extern SerialPrintf_t serial_printf_select;
+/* Exported macro ------------------------------------------------------------*/
+/* Exported functions --------------------------------------------------------*/
 
 /* Exported functions --------------------------------------------------------*/
 void cmd_parse_thread(void const * argument);
-void motor_parse_cmd(uint8_t* buffer, int len);
+void motor_parse_cmd(uint8_t* buffer, int len, SerialPrintf_t response_interface);
 
 #endif /* COMMANDS_H */
diff --git a/MotorControl/low_level.c b/MotorControl/low_level.c
index 5a219052..6f799f4c 100755
--- a/MotorControl/low_level.c
+++ b/MotorControl/low_level.c
@@ -34,11 +34,6 @@
 // Arbitrary non-zero inital value to avoid division by zero if ADC reading is late
 float vbus_voltage = 12.0f;
 
-// For now, this automatically updates to the interface that most
-// recently recieved a command. In the future we may want to separate
-// debug printf and the main serial comms.
-SerialPrintf_t serial_printf_select = SERIAL_PRINTF_IS_NONE;
-
 // TODO stick parameter into struct
 #define ENCODER_CPR (600*4)
 #define POLE_PAIRS 7
diff --git a/MotorControl/low_level.h b/MotorControl/low_level.h
index 5df86f39..49012092 100644
--- a/MotorControl/low_level.h
+++ b/MotorControl/low_level.h
@@ -142,19 +142,11 @@ typedef struct{
         int index;
 } monitoring_slot;
 
-typedef enum {
-    SERIAL_PRINTF_IS_NONE,
-    SERIAL_PRINTF_IS_USB,
-    SERIAL_PRINTF_IS_UART,
-} SerialPrintf_t;
-
 /* Exported constants --------------------------------------------------------*/
+extern const int num_motors;
+/* Exported variables --------------------------------------------------------*/
 extern float vbus_voltage;
 extern Motor_t motors[];
-extern const int num_motors;
-extern SerialPrintf_t serial_printf_select;
-
-/* Exported variables --------------------------------------------------------*/
 /* Exported macro ------------------------------------------------------------*/
 /* Exported functions --------------------------------------------------------*/
 
diff --git a/Odrive.ioc b/Odrive.ioc
index 1c222c69..86cc79fd 100755
--- a/Odrive.ioc
+++ b/Odrive.ioc
@@ -592,11 +592,13 @@ TIM8.Period=TIM_1_8_PERIOD_CLOCKS
 TIM8.TIM_MasterOutputTrigger=TIM_TRGO_UPDATE
 UART4.IPParameters=VirtualMode
 UART4.VirtualMode=Asynchronous
+USB_DEVICE.APP_RX_DATA_SIZE-CDC_FS=64
+USB_DEVICE.APP_TX_DATA_SIZE-CDC_FS=64
 USB_DEVICE.CLASS_NAME_FS=CDC
-USB_DEVICE.IPParameters=VirtualMode-CDC_FS,VirtualModeFS,CLASS_NAME_FS,MANUFACTURER_STRING-CDC_FS,PRODUCT_STRING_CDC_FS,VID-CDC_FS,PID_CDC_FS,SERIALNUMBER_STRING_CDC_FS
-USB_DEVICE.MANUFACTURER_STRING-CDC_FS=ODrive
-USB_DEVICE.PID_CDC_FS=0x0D31
-USB_DEVICE.PRODUCT_STRING_CDC_FS=ODrive rev 3.1
+USB_DEVICE.IPParameters=VirtualMode-CDC_FS,VirtualModeFS,CLASS_NAME_FS,MANUFACTURER_STRING-CDC_FS,PRODUCT_STRING_CDC_FS,VID-CDC_FS,PID_CDC_FS,SERIALNUMBER_STRING_CDC_FS,APP_RX_DATA_SIZE-CDC_FS,APP_TX_DATA_SIZE-CDC_FS
+USB_DEVICE.MANUFACTURER_STRING-CDC_FS=ODrive Robotics
+USB_DEVICE.PID_CDC_FS=0x0D33
+USB_DEVICE.PRODUCT_STRING_CDC_FS=ODrive rev 3.3
 USB_DEVICE.SERIALNUMBER_STRING_CDC_FS=000000000001
 USB_DEVICE.VID-CDC_FS=0x1209
 USB_DEVICE.VirtualMode-CDC_FS=Cdc
diff --git a/Src/syscalls.c b/Src/syscalls.c
index d8fd9283..cce3b77e 100644
--- a/Src/syscalls.c
+++ b/Src/syscalls.c
@@ -15,11 +15,26 @@
 //int _fstat(int file, struct stat *st) {}
 //int _isatty(int file) {}
 
+//static char uart_tx_buf
+
 int _write(int file, char *data, int len) {
 
-  // transmit over CDC
-  uint8_t status = CDC_Transmit_FS((uint8_t*)data, len);
+  //number of bytes written
+  int written = 0;
 
-  // return number of bytes written
-  return (status == USBD_OK ? len : 0);
+  switch (serial_printf_select) {
+    case SERIAL_PRINTF_IS_USB: {
+      // transmit over CDC
+      uint8_t status = CDC_Transmit_FS((uint8_t*)data, len);
+      written = (status == USBD_OK) ? len : 0;
+    } break;
+    case SERIAL_PRINTF_IS_UART: {
+
+    } break;
+    default: {
+      written = 0;
+    } break;
+  }
+
+  return written;
 }
diff --git a/Src/usbd_cdc_if.c b/Src/usbd_cdc_if.c
index a9d9d29c..50696d9b 100644
--- a/Src/usbd_cdc_if.c
+++ b/Src/usbd_cdc_if.c
@@ -275,7 +275,7 @@ static int8_t CDC_Receive_FS (uint8_t* Buf, uint32_t *Len)
   int null_idx = MACRO_MIN(*Len, APP_RX_DATA_SIZE-1);
   Buf[null_idx] = 0;
 
-  motor_parse_cmd(Buf, *Len);
+  motor_parse_cmd(Buf, *Len, SERIAL_PRINTF_IS_USB);
 
   return (USBD_OK);
   /* USER CODE END 6 */ 
@@ -296,11 +296,14 @@ uint8_t CDC_Transmit_FS(uint8_t* Buf, uint16_t Len)
 {
   uint8_t result = USBD_OK;
   /* USER CODE BEGIN 7 */ 
-  USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef*)hUsbDeviceFS.pClassData;
-  if (hcdc->TxState != 0){
-    return USBD_BUSY;
-  }
-  USBD_CDC_SetTxBuffer(&hUsbDeviceFS, Buf, Len);
+  
+  //Check Len
+  if (Len > APP_TX_DATA_SIZE)
+    return USBD_FAIL;
+  // memcpy Buf into UserTxBufferFS
+  memcpy(UserTxBufferFS, Buf, Len);
+  // Update Len
+  USBD_CDC_SetTxBuffer(&hUsbDeviceFS, UserTxBufferFS, Len);
   result = USBD_CDC_TransmitPacket(&hUsbDeviceFS);
   /* USER CODE END 7 */ 
   return result;