Duemilanove + 2 Popolu Controllers + Xbee

I have successfully setup two way com using 2 Xbees as shown in the pictures here: http://www.robotshop.com/forum/showthread.php?614-Xbee-Series-2-Point-to-Point-Duplex-Configuration . Prior to installing Xbee I was able to control the motors/servos using code uploaded from the Arduino environment. The Xbee shield fitted to the Duemilanove blocks Pins 0 (RX) & 1 (TX) which I previously used to connect both controller boards. My most recent code tries to use the NewSoftSerial Library. Should this work ok?

/*
Sample code for RobotShop Rover v1.0
www.RobotShop.com
*/
#include <Wire.h>
#include <NewSoftSerial.h>

#define MIN_POS0 1800 // the minuimum pulse width for your servos
#define MAX_POS0 4200 // maximum pulse width for your servos
#define MIN_POS1 2700 // the minuimum pulse width for your servos
#define MAX_POS1 4000 // maximum pulse width for your servos
#define srfAddress 0x70                           // Address of the SRF08
#define cmdByte 0x00                              // Command byte
#define lightByte 0x01                            // Byte to read light sensor
#define rangeByte 0x02                            // Byte for start of ranging data
// set servo speed, goes from 1 to 127
int servoSpeed = 120; // 1 = fastest  127 = slowest
int motor_reset = 2; //digital pin 2 assigned to motor reset
int irpin = 3;
int speakerOut = 9;
NewSoftSerial mySerial =  NewSoftSerial(4, 5);
int idistance = 0;
int period;
int timeUp = 680; // 1000000/(2 * frequency)
int pos0 = 3000;
int pos1 = 2500;
long loopcount = 0;
struct t_mtab { char c, pat; } ;

struct t_mtab morsetab[] = {
} ;

#define N_MORSE  (sizeof(morsetab)/sizeof(morsetab[0]))
#define SPEED  (10)
#define DOTLEN  (1200/SPEED)
#define DASHLEN  (3*(1200/SPEED))

int distance = 0;                     // variable for storing the distance (cm)
byte highByte = 0x00;                             // Stores high byte from ranging
byte lowByte = 0x00;      // Stored low byte from ranging

void
dash()
{
}

void
dit()
{
}

void
send(char c)
{
}

void
sendmsg(char *str)
{
}

void setup()
{
  pinMode(motor_reset, OUTPUT);
  pinMode(speakerOut, OUTPUT);
  Wire.begin();  // join i2c bus (address optional for master) 
  Serial.begin(9600);
  mySerial.begin(9600);
  delay(100);
  // reset motor controller
  digitalWrite(motor_reset, LOW);
  delay(50);
  digitalWrite(motor_reset, HIGH);
  delay(50);
  // set servo pin and speed
  if (servoSpeed >= 1 && servoSpeed <= 127)
    {
    servoSetSpeed(0, servoSpeed);
    servoSetSpeed(1, servoSpeed);
    }
  int softRev = getSoft();  // Calls function to get software revision
  Serial.println(softRev);
  pos0 = 2900;
  if (pos0 > MAX_POS0)
    pos0 = MAX_POS0;
  if (pos0 < MIN_POS0)
    pos0 = MIN_POS0;
  servoSetPosition(0, pos0);
  delay(25);
  pos1 = 2700;
  if (pos1 > MAX_POS1)
    pos1 = MAX_POS1;
  if (pos1 < MIN_POS1)
    pos1 = MIN_POS1;
  servoSetPosition(1, pos1);
  delay(25);
}

void loop()
{
  loopcount++;
  if (Serial.available() > 0) {
    int b = Serial.read();
    if (b == 's')  
    {
    motorstop();
    delay(100);
    sonicdetection();
    }
    if (b == 'j') 
    {
    motorstop(); // stop the motors
    delay(100);
    rotateccw(); // rotate for 1 second
    delay(1000);
    motorstop(); // stop the motors
    delay(100);
    sonicdetection();
    }
    if (b == 'k') 
    {
    motorstop(); // stop the motors
    delay(100);
    rotatecw(); // rotate for 1 second
    delay(1000);
    motorstop(); // stop the motors
    delay(100);
    sonicdetection();
    }
    if (b == 'i')  
    {
    motorforward();
    irdetection();
    delay(100);
    }
    if (b == 'm') 
    {
    Serial.println(b);
    sendmsg("SOS") ;
    motorreverse();
    delay(5000);
    motorstop();
    delay(100);
    }
    Serial.flush();
  }
  if (loopcount > 50000L)
  {
    irdetection();
    loopcount = 0;
  }
}

void itoa(int n, int type)
{
}

int getRange(){                                   // This function gets a ranging from the SRF08
}

int getLight(){                                   // Function to get light reading
}

int getSoft(){                                    // Function to get software revision
}

void irdetection()
{
}

void sonicdetection()
{
}

//subroutine motor forward
void motorforward()
{
//left motor
unsigned char buff1[6];
buff1[0]=0x80; //start byte - do not change
buff1[1]=0x00; //Device type byte
buff1[2]=0x01; //Motor number and direction byte; motor one =00,01
buff1[3]=0x45; //Motor speed "0 to 128" (ex 100 is 64 in hex)
for(int i=0; i<4; i++) {mySerial.print(buff1[i], BYTE);}
//right motor
unsigned char buff2[6];
buff2[0]=0x80; //start byte - do not change
buff2[1]=0x00; //Device type byte
buff2[2]=0x03; //Motor number and direction byte; motor two=02,03
buff2[3]=0x45; //Motor speed "0 to 128" (ex 100 is 64 in hex)
for(int i=0; i<4; i++) {mySerial.print(buff2[i], BYTE);}
}
//subroutine reverse at half speed
void motorreverse()
{
//left motor
unsigned char buff3[6];
buff3[0]=0x80; //start byte - do not change
buff3[1]=0x00; //Device type byte
buff3[2]=0x00; //Motor number and direction byte; motor one =00,01
buff3[3]=0x35; //Motor speed "0 to 128" (ex 100 is 64 in hex)
for(int i=0; i<4; i++) {mySerial.print(buff3[i], BYTE);}
//right motor
unsigned char buff4[6];
buff4[0]=0x80; //start byte - do not change
buff4[1]=0x00; //Device type byte
buff4[2]=0x02; //Motor number and direction byte; motor two=02,03
buff4[3]=0x35; //Motor speed "0 to 128" (ex 100 is 64 in hex)
for(int i=0; i<4; i++) {mySerial.print(buff4[i], BYTE);}
}
//Motor all stop
void motorstop()
{
//left motor
unsigned char buff3[6];
buff3[0]=0x80; //start byte - do not change
buff3[1]=0x00; //Device type byte
buff3[2]=0x00; //Motor number and direction byte; motor one =00,01
buff3[3]=0x00; //Motor speed "0 to 128" (ex 100 is 64 in hex)
for(int i=0; i<4; i++) {mySerial.print(buff3[i], BYTE);}
//right motor
unsigned char buff4[6];
buff4[0]=0x80; //start byte - do not change
buff4[1]=0x00; //Device type byte
buff4[2]=0x02; //Motor number and direction byte; motor two=02,03
buff4[3]=0x00; //Motor speed "0 to 128" (ex 100 is 64 in hex)
for(int i=0; i<4; i++) {mySerial.print(buff4[i], BYTE);}
}

void rotatecw()
{
//left motor
unsigned char buff1[6];
buff1[0]=0x80; //start byte - do not change
buff1[1]=0x00; //Device type byte
buff1[2]=0x00; //Motor number and direction byte; motor one =00,01
buff1[3]=0x40; //Motor speed "0 to 128" (ex 100 is 64 in hex)
for(int i=0; i<4; i++) {mySerial.print(buff1[i], BYTE);}
//right motor
unsigned char buff2[6];
buff2[0]=0x80; //start byte - do not change
buff2[1]=0x00; //Device type byte
buff2[2]=0x03; //Motor number and direction byte; motor two=02,03
buff2[3]=0x40; //Motor speed "0 to 128" (ex 100 is 64 in hex)
for(int i=0; i<4; i++) {mySerial.print(buff2[i], BYTE);}
}

void rotateccw()
{
//left motor
unsigned char buff1[6];
buff1[0]=0x80; //start byte - do not change
buff1[1]=0x00; //Device type byte
buff1[2]=0x01; //Motor number and direction byte; motor one =00,01
buff1[3]=0x40; //Motor speed "0 to 128" (ex 100 is 64 in hex)
for(int i=0; i<4; i++) {mySerial.print(buff1[i], BYTE);}
//right motor
unsigned char buff2[6];
buff2[0]=0x80; //start byte - do not change
buff2[1]=0x00; //Device type byte
buff2[2]=0x02; //Motor number and direction byte; motor two=02,03
buff2[3]=0x40; //Motor speed "0 to 128" (ex 100 is 64 in hex)
for(int i=0; i<4; i++) {mySerial.print(buff2[i], BYTE);}
}

void servoSetPosition(int servo, int angle)
{
  //servo is the servo number (typically 0-7)
  //angle is the absolute position from 500 to 5500
  unsigned char buff[6];
  unsigned int temp;
  unsigned char pos_hi,pos_low;

  //Convert the angle data into two 7-bit bytes
  temp=angle&0x1f80;
  pos_hi=temp>>7;
  pos_low=angle & 0x7f;

  //Construct a Pololu Protocol command sentence
  buff[0]=0x80; //start byte: always 0x80
  buff[1]=0x01; //device id: always 0x01 for an pololu 8 servo controller
  buff[2]=0x04; //command number: 0x04 to set absolute values
  buff[3]=servo; //servo number: 0 to 7
  buff[4]=pos_hi; //data1
  buff[5]=pos_low; //data2

  //Send the command to the servo controller
  for(int i=0;i<6;i++){
    mySerial.print(buff[i],BYTE);
  }
}

void servoSetSpeed(int servo, int speed){
  //servo is the servo number (typically 0-7)
  //speed is servo speed (1=fastest, 127=slowest)
  //set speed to zero to turn off speed limiting
  unsigned char buff[5];
  unsigned char speedcmd;
  speedcmd=speed&0x7f;//take only lower 7 bits of speed

  buff[0]=0x80;//start byte
  buff[1]=0x01;//device id
  buff[2]=0x01;//command number
  buff[3]=servo;//servo number
  buff[4]=speed;//data1

  for(int i=0;i<5;i++){
    mySerial.print(buff[i],BYTE);
  }
}