#pragma config(Sensor, S1,     leftTouch,      sensorTouch)
#pragma config(Sensor, S2,     rightTouch,     sensorTouch)
#pragma config(Sensor, S3,     backwards,      sensorTouch)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

// master robot is the remote control

#include "XbeeTools.h"
// include the function:XbeeTools

task main()
{
   InitRS485();

   string messageToSend="";  //Creates an empty string to store the message to send in
   string sendNextMessage=""; //Creates an empty string to store the response message in

   string leftEncoderCount = "Calculating";
   string rightEncoderCount = "Calculating";

   while(true)
   {
      if(SensorValue(rightTouch)==1)//if the sensor value of the right touch is equal to 1, robot adds "R" to messageToSend
         strcat(messageToSend, "R");  //forward right motor

      else//or else it adds "1" to the messageToSend
         strcat(messageToSend, "1");  //stop right motor

      if (SensorValue(leftTouch)==1)//if the sensor value of the left touch is equal to 1, robot adds "L" to messageToSend
         strcat(messageToSend, "L");  // Add "R" to the messageToSend

      else//or else it adds "2" to the messageToSend
         strcat(messageToSend, "2");   // off left motor

      if (SensorValue(backwards)==1) //if the sensor value of the 3rd touch is equal to 1, robot adds "B" to messageToSend
         strcat(messageToSend, "B");  // backward motor

      else//or else it adds "3" to the messageToSend
         strcat(messageToSend, "3");  // off both motors

      if (nNxtButtonPressed == 1) //If the NXT button 1 is pressed, adds
         strcat(messageToSend, "C"); //Close Arm motor

      else if (nNxtButtonPressed==2) //Else it adds '4' to the messageToSend
         strcat(messageToSend, "4"); //Open Arm motor

      SendString(messageToSend); //Sends the message to the second robot.

      ReceiveString(sendNextMessage); //Receives go-ahead message "OK" from second bot
      while(sendNextMessage != "OK")  //Until that happens, keep checking for messages
      {
         ReceiveString(sendNextMessage); // Receive message
         nxtDisplayTextLine(1, "Send? ", sendNextMessage); //Display message
         wait1Msec(10);
      }
      sendNextMessage = "No"; //Changes value of the 'sendNextMessage' string

      ReceiveString(leftEncoderCount); //Receives go-ahead message "OK" from second bot
      while(leftEncoderCount == "Calculating")  //Until that happens, keep checking for messages
      {
         ReceiveString(leftEncoderCount); // Receive message
         nxtDisplayTextLine(3, "LEncoder: ", leftEncoderCount); //Display message
         wait1Msec(10);
      }
      SendString("OK");

      ReceiveString(rightEncoderCount); //Receives go-ahead message "OK" from second bot
      while(rightEncoderCount == "Calculating")  //Until that happens, keep checking for messages
      {
         ReceiveString(rightEncoderCount); // Receive message
         nxtDisplayTextLine(5, "REncoder: ", rightEncoderCount); //Display message
         wait1Msec(10);
      }
      SendString("OK");

   }


}

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

#include "XbeeTools.h"
//master robot is the remote control

task main()
{
   InitRS485();
   //initaliting the motor
   string messageReceived = "None";
   //the string variable for the mesage receieved
   string leftEncoderCount;
   string rightEncoderCount;

   nMotorEncoder[motorC]=0;
   nMotorEncoder[motorB]=0;

   while(true)
   {

      ReceiveString(messageReceived); //this is the robot receiving the message

      while(messageReceived == "None")
      {
         ReceiveString(messageReceived); //this is the robot receiving the message
         motor[motorA]=0;
         motor[motorB]=0;
         motor[motorC]=0;
         wait1Msec(10);
      }

      if(StringFind(messageReceived, "B") == 2) //If the third character (index 2) is a B
      {
         motor[motorB] = -100;
         motor[motorC] = -100;
      }

      else if (StringFind(messageReceived, "3") == 2) //else, if the first character is a 3
      {

         if(StringFind(messageReceived, "R") == 0) //If the first character (index 0) is an R
         {
            motor[motorB] = 100; //Turn motorB on at 100 power
         }
         else if (StringFind(messageReceived, "1") == 0) //else, if the first character is a 1
         {
            motor[motorB] = 0; //Turn motorB off
         }

         if(StringFind(messageReceived, "L") == 1) //If the second character (index 1) is an L
         {
            motor[motorC] = 100; //Turn motorC on at 100 power
         }
         else if (StringFind(messageReceived, "2") == 1) //else, if the second character is a 2
         {
            motor[motorB] = 0; //Turn motorC off
         }
      }

      if(StringFind(messageReceived, "C") == 3) //If the fourth character (index 3) is an C
      {
         motor[motorC] = 50; //Turn motorC on at 100 power
      }
      else if (StringFind(messageReceived, "4") == 3) //else, if the fourth character is a 4
      {
         motor[motorB] = -50; //Turn motorC off
      }

      SendString("OK");

//*******
      sprintf(leftEncoderCount, "%d", nMotorEncoder[motorA]);
      sprintf(rightEncoderCount, "%d", nMotorEncoder[motorB]);

      messageReceived="None";
SendString(leftEncoderCount);

      while(messageReceived == "None")
      {
         ReceiveString(messageReceived); //this is the robot receiving the message
         motor[motorA]=0;
         motor[motorB]=0;
         motor[motorC]=0;
         wait1Msec(10);
      }

      messageReceived="None";
      SendString(rightEncoderCount);

      while(messageReceived == "None")
      {
         ReceiveString(messageReceived); //this is the robot receiving the message
         motor[motorA]=0;
         motor[motorB]=0;
         motor[motorC]=0;
         wait1Msec(10);
      }
   }
}