A start to my Arduino-based robot controller

Utilizes a Solarbotics Freeduino, Parallax backlit 2x16 serial LCD w/ Speaker, a pair of Sparkfun XBee breakout boards w/ 1  mW XBee pair, and a Pololu mini Maestro servo controller. Basic functionality so far. Read wirelessly from Xbee connected serial terminal, print to LCD and back to terminal, and sending commands to the servos.


//RoboCon v.4a
// www.parallax.com (part #27977-RT)
// June/16/2012 Arduino ver: 023
// M. Ramirez "mannyr7" on Trossen Robotics forum
int i=0;


#include <NewSoftSerial.h>
NewSoftSerial maestroSerial(7, 5); 
// yellow wire Arduino tx to Mestro rx
// blue wire Arduino rx to Maestro tx
                                   
NewSoftSerial LCD(1, 2);
// second variable defines the tx pin to the LCD


NewSoftSerial xBee(3, 4); 
// yellow wire Arduino tx to Xbee rx
// green wire Arduino rx to Xbee tx                              


void setup()  
{
  LCD.begin(9600);  
// LCD dip switches set to 1=OFF, 2=ON for 9600 baud


  delay(100); // suggested delay from Parallax
  
  LCD.write(0x0C); // Clear display, move cursor to top-left
  LCD.write(0x19); // Display on, cursor on, char blink
  LCD.write(0x11); // LCD backlight on
  LCD.print("Arduino active");
  delay(1000);
  LCD.write(0x0C); // Clear display, move cursor to top-left
  LCD.print("LCD ready"); 
  delay(1000);
  
  xBee.begin(38400);
  delay(1000); //wait for xbees to pair up
  LCD.write(0x0C); // Clear display, move cursor to top-left
  LCD.print("Xbees paired");  


  maestroSerial.begin(9600);
  delay(1000); 
  settarget(0, 0); // servo port #, position in degrees
  settarget(1, 0);
  settarget(2, 0);
  delay(1000);
  settarget(0, 180);
  settarget(1, 180);
  settarget(2, 180);
  delay(1000);
  settarget(0, 90);
  settarget(1, 90);
  settarget(2, 90);  
  
  LCD.write(0x0C); // Clear display, move cursor to top-left
  LCD.print("Servos ready"); 
  
  // Play "Charge!!!"
  LCD.write(0xD2); // set note length to 1/16 note
  LCD.write(0xE6); // G note
  LCD.write(0xDF); // C note
  LCD.write(0xD3); // set note length to 1/8 note
  LCD.write(0xE3); // E note
  LCD.write(0xD4); // set note length to 1/4 note
  LCD.write(0xE6); // G note
  LCD.write(0xD3); // set note length to 1/8 note
  LCD.write(0xE3); // E note
  LCD.write(0xD5); // set note length to whole note
  LCD.write(0xE6); // G note
  
  delay(1000);
  LCD.write(0x0C); // Clear display, move cursor to top-left
  LCD.print("RoboCon v.4a"); 
}
void loop() // listen for keyboard and print to LCD and serial window
{
  char string='/0';
  xBee.listen();
  if (xBee.available())  
  {
    char c = xBee.read();
    LCD.write(0x0C); // Clear display, move cursor to top-left   
    LCD.println(c);
    xBee.println(c);
    i=0;
    delay(1000);
  }
  else
    {
    LCD.write(0x0C); // Clear display, move cursor to top-left      
    LCD.print(i+=1);
    delay(1000);
    }
}


//Send a Set Target command to the Maestro.
//Target is in units of quarter microseconds,
// so the normal range is 4000 to 8000.
void settarget(unsigned char servo, unsigned int target)
{
  target = map(target, 0, 180, 2400, 9500);
  maestroSerial.print(0xAA,BYTE); //start byte
  maestroSerial.print(0x0C,BYTE) ; //device id
  maestroSerial.print(0x04,BYTE); //command number
  maestroSerial.print(servo,BYTE); //servo number
  maestroSerial.print(target & 0x7F, BYTE);
//  LCD.print(target & 0x7F, BYTE);  
  maestroSerial.print((target >> 7) & 0x7F,BYTE);
//  LCD.print((target >> 7) & 0x7F,BYTE);  
}