#pragma config(Sensor, S1,     bump,                sensorTouch)
#pragma config(Sensor, S3,     light,               sensorLightActive)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "XbeeTools.h"

//Robot A code (Robot A is first robot to transmit)

task main()
{
  InitRS485();

  wait1Msec(2000);

  nMotorEncoder[motorB] = 0;

  int countA;  // distance traveled by Robot A

  while(SensorValue[bump] == 0) // follow line; stop when collision
  {
    countA = nMotorEncoder[motorB];

    nxtDisplayString(1, "My d = :%d", countA);
    nxtDisplayString(3, "Team d = :0");
    nxtDisplayString(5, "Total d = :0");

    if  (SensorValue[light] > 50)  // follow right edge of black line
    {
      motor[motorB] = 50;
      motor[motorC] = 0;
    }

    else
    {
      motor[motorB] = 0;
      motor[motorC] = 50;
    }

  }

  motor[motorB] = 0;  // stop robot when collision
  motor[motorC] = 0;
  PlayTone(300, 30);


  //Robot A sends data to RobotB (RobotB will be listening)
  //Robot B will receive data from Robot A, then Robot B will send its data to Robot A
  countA = nMotorEncoder[motorB];
  wait1Msec(1500);
  string messageToSend;
  StringFormat(messageToSend, "%d", countA);
  SendString(messageToSend);


  // receive distance data from Robot B
  string messageReceived;
  ReceiveString(messageReceived);  //wait to receive data
  PlayTone(300, 30);
  // convert string to integer
  int countB = atoi(messageReceived);  // distance traveled by Robot B

  nxtDisplayString(1, "My d = :%d", countA);
  nxtDisplayString(3, "Team d = :%d", countB);
  nxtDisplayString(5, "Total d = :%d", countA + countB); //add to find total distance

  wait1Msec(20000);  // leave display on robot for 20 seconds

}