   } else 
     return false;
 }
+
 
 
 String popCmd() {

  
  // and distance
  float dist = sqrt(sqr(y-position.y) + sqr(x-position.x));
  dtostrf(dist,5,1,outstr);
  String s = "FD ";
  s.concat(outstr);
  insertCmd(s);
}

void drive(float distance)
{ 
  // update state
  position.x += distance * cos(position.ang * PI / 180);
  position.y += distance * sin(position.ang * PI / 180);

int qHead = 0;
int qSize = 0;

boolean insertCmd(String s) {
  // inserts s at head of queue
  // return true if inserted ok, false if buffer full
  
  Serial.println("insertCmd: " + s);
  
  if (!isQFull()) {
    qHead--;
    if (qHead < 0) qHead += QUEUE_LENGTH;
