 }
 
 static void writeText(String s) {
-  // overwrite write text
-  text = s;
-  text.toUpperCase();
-
-  // reset current state
-  bot.resetPosition();
-}
+  if (s.length() > 0) {
+    char c = s[0];  // grab the next character to write
+    
+    // put the rest of the command back at the beginning of the queue
+    cmdQ.insert(s.substring(1), LOGO_CMD_WT);
+
+    // use the LogobotText library to write the letter
+    LogobotText::writeChar(c, bot.state.x, bot.state.y);  
+  } 
+}

  // keep the bot moving (this triggers the stepper motors to move, so needs to be called frequently, i.e. >1KHz)
  bot.run();

  // see if we can queue the next command to the bot...
  // Needs to be a movement command and the bot movement queue must not be full
  if (cmdQ.peekAtType() <= LOGO_MOVE_CMDS && !bot.isQFull()) {
      // pop and process next command from queue
      doLogoCommand(cmdQ.dequeue());

  } else if (!bot.isBusy()) {  // See if the bot has finished whatever it's doing...
    // see if we've got more text to write
    if (text.length() > 0) {
      // check if we're ready for another letter - need the queue to be empty as letters take a LOT of commands
      if (bot.isQEmpty()) {
        char c = text[0];  // grab the next character to write
        text = text.substring(1);  // and remove the first character from the text writing buffer
        LogobotText::writeChar(c, bot.state.x, bot.state.y);  // use the LogobotText library to write the letter
      }
    } else {
      // pop and process next command from queue
      doLogoCommand(cmdQ.dequeue());
    }
  }
}

static void handleCollision(byte collisionData)

#include <Servo.h>

Bot bot(MOTOR_L_PIN_1, MOTOR_L_PIN_2, MOTOR_L_PIN_3, MOTOR_L_PIN_4, MOTOR_R_PIN_1, MOTOR_R_PIN_2, MOTOR_R_PIN_3, MOTOR_R_PIN_4);
CommandQueue cmdQ(15);
CommandQueue textQ(20);
String cmd; // cmd received over serial - builds up char at a time

void setup()
