The crawler (built with Arduino mega 2560, ultrasonic sensor and Hitec 485HB servos)

#include <Wire.h>
#include "LCD12864RSPI.h"

// Chinese character display
unsigned char currentOrder[] = {0xC3, 0xFC, 0xC1, 0xEE, ':'};
unsigned char left[] = {0xD7, 0xF3, 0xD7, 0xAA};
unsigned char right[] = {0xD3, 0xD2, 0xD7, 0xAA};
unsigned char forward[] = {0xCF, 0xF2, 0xC7, 0xB0};
unsigned char backward[] = {0xCF, 0xF2, 0xBA, 0xF3};
unsigned char forwardBackward[] = {0xC7, 0xB0, 0xBA, 0xF3};
unsigned char stepCount[] = {0xB2, 0xBD, 0xCA, 0xFD, ':'};
unsigned char stepLabel[] = {0xB2, 0xBD};

// Structure to define movement
// A movement for example, left, consists of several phases.
typedef struct Movement{
  int dir; // Identifier to indicate type of movement: 0 left; 1 right; 2 forward; 3 backward; 4 initialize.
  int *times; // time in milliseconds used to perform each phase of a movement
  char** servos; // servo position control
  int phases; // number of phases in a single movement
  int stepCount; // the number of steps for this type of movement.
};


Movement rMove; // Right movement
Movement lMove; // Left movement
Movement fMove; // Forward movement
Movement bMove; // Backward movement
Movement iMove; // Initialization movement

// Servo positions for each type of movement
char* lServos[] = {"#1 P1200 #4 P1200", "#0 P700 S1500 #3 P2300 S1500", "#1 P1500 S1000 #4 P1500 S1000", "#0 P1500 #3 P1500"};
char* rServos[] = {"#1 P2100 #4 P2100", "#0 P700 S1500 #3 P2300 S1500", "#1 P1500 S1000 #4 P1500 S1500", "#0 P1500 #3 P1500 "};
char* fServos[] = {"#1 P2000 #4 P1000", "#0 P700 S1500 #3 P2300 S1500", "#1 P700 S1000 #4 P2300 S1000", "#0 P1500 #3 P1500 "};
char* bServos[] = {"#1 P1000 #4 P2000", "#0 P700 S1500 #3 P2300 S1500", "#1 P2300 S1000 #4 P700 S1000", "#0 P1500 #3 P1500 "};
char* iServos[] = {"#0 P1500 #1 P1500 #2 P1500 #3 P1500 #4 P1500 #5 P1500"};

// Time used for each phase of each movement.
int lTimes[] = {200, 200, 200, 200};
int rTimes[] = {200, 200, 400, 200};
int fTimes[] = {300, 400, 500, 300};
int bTimes[] = {300, 400, 800, 300};
int iTimes[] = {500};

// Position to display messages.
int messagePosition[8] = {2, 5, 1, 5, 3, 5, 3, 5};

boolean shouldChangeDirection = false; // Should next movement be used to change direction, in case that an obstacle is detected by the ultrasonic sensor.
boolean isForward = true; // If current direction is forward
int EN = 2; // Pin number to enable XBee expansion board V5

// Measure distance using the URM04V2 ultrasonic sensor.
void measureDistance(byte device) {
  digitalWrite(EN, HIGH);
  // Trigger distance measurement.
  uint8_t DScmd[6]={0x55,0xaa,device,0x00,0x01,0x00};  
  for(int i=0; i<6; i++) {
    Serial.write(DScmd[i]);
    DScmd[5] += DScmd[i];
  }
  delay(30);
  // Send command to read measured distance.
  uint8_t STcmd[6]={0x55,0xaa,device,0x00,0x02,0x00};  
  for(int i=0; i<6; i++) {
    Serial.write(STcmd[i]);
    STcmd[5] += STcmd[i];
  }  
  delay(3);
}

// Return last measured distance by the URM04V2 ultrasonic sensor.
// -1 means the last measurement is out of range or unsuccessful.
int readDistance() {
  uint8_t data[8];
  digitalWrite(EN,LOW);
  boolean done = false;
  int counter = 0;
  int result = -1;
 
  while(!done){
    int bytes = Serial.available();
    if(bytes==8) {  
      for(int i=0; i<8; i++) {
        data[i] = Serial.read();
      }
      result = (int)data[5] * 256 + data[6];
      done = true;
    } else {
      delay(10);
      counter++;
      if(counter==5) {
        done = true;
      }
    }
  }
  return result;
}

// Perform movement m by repeating it for steps times.
boolean performMovement(struct Movement *m, int steps) {
  boolean inChange = false;
  if(shouldChangeDirection) {
    inChange = true;
  }
  for(int s=0; s< steps; s++) {
    for(int i=0; i<m->phases; i++) {
      Serial3.println(m->servos[i]);
      delay(m->times[i]);
      if(!inChange) {
        changeHeading();      
        if(shouldChangeDirection) {
          return shouldChangeDirection;
        }
      }
    }
    m->stepCount++;
    int number = 0;
    if(m->dir==3 || m->dir==4) {
      number = fMove.stepCount + bMove.stepCount;
    } else {
      number = m->stepCount;
    }
    if(m->dir!=5) {
      displayNumber(messagePosition[m->dir * 2], messagePosition[m->dir * 2 + 1], number);            
    }
  }
  if(inChange) {
    shouldChangeDirection = false;
  }
  return false;
}

void setup()
{  
  lMove.servos = lServos;
  lMove.times = lTimes;
  lMove.phases = 4;
  lMove.stepCount = 0;
  lMove.dir = 0; //Left

  rMove.servos = rServos;
  rMove.times = rTimes;
  rMove.phases = 4;
  rMove.stepCount = 0;
  rMove.dir = 1; //Right
 
  fMove.servos = fServos;
  fMove.times = fTimes;
  fMove.phases = 4;
  fMove.stepCount = 0;
  fMove.dir = 2; //Forward
 
  bMove.servos = bServos;
  bMove.times = bTimes;
  bMove.phases = 4;
  bMove.stepCount = 0;  
  bMove.dir = 3; //Backward

  iMove.servos =iServos;
  iMove.times = iTimes;
  iMove.phases = 1;
  iMove.stepCount = 0;  
  iMove.dir = 5;
 
  randomSeed(analogRead(0));

  // Initialize LCD display.
  LCDA.Initialise();
  delay(100);
   
  LCDA.CLEAR();
  LCDA.DisplayString(0, 0, currentOrder, 5);
  LCDA.DisplayString(1, 0, left, 4);
  LCDA.DisplayString(1, 2, stepCount, 5);
  LCDA.DisplayString(2, 0, right, 4);
  LCDA.DisplayString(2, 2, stepCount, 5);
  LCDA.DisplayString(3, 0, forwardBackward, 4);  
  LCDA.DisplayString(3, 2, stepCount, 5);

  // Initialize all servos to neutral position.
  Serial3.begin(115200);
  Serial3.println("#0 P1500 #1 P1500 #2 P1500 #3 P1500 #4 P1500 #5 P1500 #6 P1500 #7 P600");
 
  pinMode(EN, OUTPUT);
  Serial.begin(19200);
  delay(200);
  digitalWrite(EN,HIGH);
  delay(2000);  
}

void changeHeading() {
  measureDistance(0x11);
  int distance = readDistance();
  if(distance !=-1) {
    if(isForward) {
      if(distance < 15) {
        shouldChangeDirection = true;
      }
    } else {
    }
  }
}

// Display the current order.
void displayCurrentOrder(unsigned char *order, int steps) {
  LCDA.DisplayString(0, 3, order, 4);
  String txt = String(steps);
  char* buf = "      ";
  txt.toCharArray(buf, 4);
  buf[2] = 0xB2;
  buf[3] = 0xBD;
  LCDA.DisplayString(0, 5, (unsigned char*)buf, 4);
}

// Display a number in row and column.
void displayNumber(int row, int column, int number) {
  String txt = String(number);
  char* buf = "      ";
  txt.toCharArray(buf, 5);
  LCDA.DisplayString(row, column, (unsigned char*)buf, txt.length());
}

// Turn the robot to the right.
void turnRight(long steps) {
  displayCurrentOrder(right, steps);    
  performMovement(&rMove, steps);  
}

// Turn the robot to the left.
void turnLeft(long steps) {
  displayCurrentOrder(left, steps);
  performMovement(&lMove, steps);
}

// Move the robot forwards.
void goForward(long steps) {  
  displayCurrentOrder(forward, steps);
  performMovement(&fMove, steps);
}

// Move th robot backwards.
void goBackward(long steps) {
  displayCurrentOrder(backward, steps);
  isForward = false;
  performMovement(&bMove, steps);
  isForward = true;  
}

long lastMove = 0;
long move=0;

// Check which way to go next by finding the direction where the obstacle is the farest away.
// dirs will be setup by this function.
// dirs is an integer array with two elements.
// The first element indicates the type of movements (1~4)
// The second element indicates the number of steps to go in that direction.
void checkDistance(int* dirs) {
  performMovement(&iMove, 1);
  shouldChangeDirection = true;
  Serial3.println("#6 P600");    
  delay(500);
  int j = 0;
  int maxDis = -1;
  int dir = 0;
  for(int i=600; i<=2400; i+=150) {
      Serial3.print("#6P");    
      Serial3.println(i);
      delay(200);
      measureDistance(0x11);
      int d = readDistance();
      if(d==-1) {
        d = 400;
      }
      if(d > maxDis) {
        maxDis = d;
        dir = i;
      }
  }
  Serial3.println("#6 P1500");
  delay(500);
 
  if(maxDis < 40) {
    // Measure backward distance.
    Serial3.println("#7 P2400");
    delay(500);
    measureDistance(0x11);
    int bd = readDistance();
    Serial3.println("#7 P600");
    delay(500);
    if(bd > 40) {
      dirs[0] = 4;
      dirs[1] = (int)random(5, 10);
    } else {
      dirs[0] = 2;
      dirs[1] = 5;
    }
  } else {    
    if(dir > 0) {      
      dir = (dir - 600) / 150;      
      if(dir<6) {
        dirs[0] = 1;
        dirs[1] = (6-dir);
      } else if(dir>6) {
        dirs[0] = 2;
        dirs[1] = dir - 6;
      } else {
        dirs[0] = 3;
        dirs[1] = (int)random(5, 10);
      }
    } else {
      dirs[0] = 4;
      dirs[1] = 2;
    }
  }
}

int steps = 0;

void loop() {
  steps = 0;
  if(shouldChangeDirection) {
    int dirs[] = {0, 0};
    checkDistance(dirs);
    move = dirs[0];    
    steps = dirs[1];    
  } else {
    move = random(1, 4);
    if(lastMove==1 || lastMove==2) {
      move = 3;
    } else if(lastMove==0) {
      move = 3;
    }    
  }  
  if(move==1) {
    if(steps==0) {
      steps = (int)random(3, 6);
    }
    turnLeft((int)steps);
  }
  else if(move==2) {
    if(steps==0) {
      steps = (int)random(3, 6);
    }
    turnRight(steps);
  }
  else if(move==4) {
    if(steps==0) {
      steps = (int)random(5, 15);
    }
    goBackward(steps);
  }
  else {
    if(steps==0) {
      steps = (int)random(5, 15);
    }    
    goForward(steps);    
  }
  lastMove = move;  
}