 boolean insertCmd(String s) {
   // inserts s at head of queue
   // return true if inserted ok, false if buffer full
-  
+
   if (!isQFull()) {
     qHead--;
     if (qHead < 0) qHead += QUEUE_LENGTH;
-    
+
     cmdQ[qHead].cmd = String(s);
-    
+
     qSize++;
-    
+
     return true;
-  } else 
+  } else
     return false;
 }
 
 boolean pushCmd(String s) {
   // push s onto tail of queue
   // return true if pushed ok, false if buffer full
-  
+
   if (!isQFull()) {
     int next = qHead + qSize;
     if (next >= QUEUE_LENGTH) next -= QUEUE_LENGTH;
-    
+
     cmdQ[next].cmd = String(s);
-    
+
     qSize++;
-    
+
     return true;
-  } else 
+  } else
     return false;
 }
 

  NEXTLETTER
}

static void writeM() 
{
  float x = state.x;
  float y = state.y;

    char c = Serial.read();
    if (c == '\r' || c == '\n') {
      if (cmd != "") {
        if (isQFull()) {
            Serial.println("BUSY");
        } else {
            if (cmd[0] == '!') {
                emergencyStop();
                insertCmd(cmd);
            } else {
                pushCmd(cmd);
            }
            Serial.println("OK:" + cmd);
        }

        cmd = "";
      }
    } else {
      cmd += c;
    }
  }

  handleCollisions();

  // Do buzzer
  if (millis() < buzzEnd)
    digitalWrite(buzzer, HIGH);
