     }
     
     // Wait for USB connection again.
+    yield();
     return;
   }
   
   // Set LED to green when USB is connected.
-  rgb_led.set(0,1,0); 
+  rgb_led.R(0);
+  rgb_led.G(1);
     
   // 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: ");
+  if (bytes_read <= 0) 
+  {
+    yield();
+    return;
+  }
+  
+  // Copy incoming message to debug console.
+  Serial.print("<- ");
   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[64];
  snprintf(motor_update, 64,
    "{"
      "\"m0\":{"
        "\"v\":%f,"

    motor[0]->velocity(), motor[0]->current(),
    motor[1]->velocity(), motor[1]->current()
  );
  send(motor_update);
}


