     motor[0]->velocity(), motor[0]->current(),
     motor[1]->velocity(), motor[1]->current()
   );
-  adk.write(strlen(motor_update), (uint8_t *)motor_update);
-}
-
-
+  send(motor_update);
+}
+
+

    }
    
    // Wait for USB connection again.
    return;
  }
  
  // Set LED to green when USB is connected.
  rgb_led.set(0,1,0); 
    
  // Read next command from USB.
  adk.read(&bytes_read, INPUT_BUFFER_SIZE, (uint8_t*)input_buffer);
  if (bytes_read <= 0) return;
  
  // TODO: remove this debug print
  Serial.print("RCV: ");
  Serial.println(input_buffer);
    
  // Parse command as JSON string

  
  // TODO: move this to another location (e.g. Motor)
  // Send motor status update over USB
  char motor_update[128];
  snprintf(motor_update, 128,
    "{"
      "\"m0\":{"
        "\"v\":%f,"
