#pragma config(Sensor, S1,     ltouch,              sensorTouch)
#pragma config(Sensor, S2,     rtouch,              sensorTouch)

//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "P:\Isaac\Multirobot Developement\programs\Xbee Tools Includes\OLDXbeeTools (improved).h"


task main()
{
  InitRS485(); //Initialize the Xbee Radio
  nMotorEncoder[motorC] = 0; //Set switch back to 0
  string messageToSend; //String to fill with commands to transmit
  char leftButton = 'F';
  char rightButton = 'F';
  int Delay = 50; //Delay to slow down command transmission
  //50 works well, 30 is the lowest possible number

  while(true)
  {
    /* When the Switch is placed at zero or in a negative
    position the controller will transmit commands that only
    Robot 1 can understand. Since the motor encoder of the
    switch is set to 0 at the start, the controller will
    always start transmitting to Robot 1*/

    while(nMotorEncoder[motorC] <= 0) //Transmit to Robot 1
    {
      nxtDisplayBigTextLine(1,"Robot 1");//Display Robot 1 on screen

      if (SensorValue[rtouch] == 1)
      {
        rightButton = 'T';
      }
      else
      {
        rightButton = 'F';
      }

      if (SensorValue(ltouch) == 1)
      {
        leftButton = 'T';
      }
      else
      {
        leftButton = 'F';
      }

      StringFormat(messageToSend, "R1R%cL%c", rightButton, leftButton);
      SendString(messageToSend);

      wait1Msec(Delay); //Delay before sending next command
    }

    /* When the Switch is in a negative position the controller
    will transmit commands that only Robot 2 can understand.*/

    while(nMotorEncoder[motorC] > 0) //Transmit to Robot 2
    {
      nxtDisplayBigTextLine(1,"Robot 2"); //Display Robot 2 on screen

      if (SensorValue[rtouch] == 1)
      {
        rightButton = 'T';
      }
      else
      {
        rightButton = 'F';
      }

      if (SensorValue(ltouch) == 1)
      {
        leftButton = 'T';
      }
      else
      {
        leftButton = 'F';
      }

      StringFormat(messageToSend, "R2R%cL%c", rightButton, leftButton);
      SendString(messageToSend);

      wait1Msec(Delay); //Delay before sending next command
    }
  }
}