#pragma config(Sensor, S3,     sonar3,              sensorSONAR)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

// Robot A  - move through obstacle course using sensors (1 corner version)
// Send commands to Robot B (no sensors) to follow Robot A
// 1) move fwd using sonar; stop at wall
// 2) create commmand string for RobotB
// 3) turn right
// 4) create commmand string for Robot B
// 5) move 2 feet out out obstacle course, then stop
// 6) create commmand string for Robot B (avoid collision)
// 7) send each command to Robot B and wait for response before sending next cmd

#include "XbeeTools.h"

task main()
{
  InitRS485();

  string messageReceived;

  wait1Msec(2000);  // avoid having hand blocking sonar

// 1) move fwd using sonar; stop at wall
  nMotorEncoder[motorB] = 0;

  while (SensorValue[sonar3] > 30)  // move fwd until wall
  {
    motor[motorB] = 50;
    motor[motorC] = 50;
  }

  motor[motorB] = 0;  // stop robot
  motor[motorC] = 0;
  wait1Msec(200);

  // 2) create commmand string for RobotB
  int count = nMotorEncoder[motorB];
  string messageToSend1;
  StringFormat(messageToSend1, "Fwd%d", count+500);  // RobotB starts behind RobotA


  // 3) turn right

  nMotorEncoder[motorC] = 0;

  while (nMotorEncoder[motorC] < 160)
  {
    motor[motorB] = -50;
    motor[motorC] = 50;
  }

  motor[motorB] = 0;  // stop robot
  motor[motorC] = 0;
  wait1Msec(200);

  // 4) create commmand string for RobotB
  string messageToSend2 = "TurnR";



  // 5) move 2 feet out out obstacle course

  nMotorEncoder[motorB] = 0;
  while (nMotorEncoder[motorB] < 1500)
  {
    motor[motorB] = 50;
    motor[motorC] = 50;
  }
   motor[motorB] = 0;  // stop robot
   motor[motorC] = 0;
   wait1Msec(200);

   //6) create commmand string for RobotB

   string messageToSend3 = "Fwd800";  // avoid collision


  // 7) send commands sequentially to RobotB
  // Wait for response from RobotB that command has been completed
  // before sending next command

  SendString(messageToSend1);       // send command to RobotB to move forward
  ReceiveString(messageReceived);   // wait until RobotB has indicated it has completed task
  PlayTone(200, 20);

  SendString(messageToSend2);       // send command to RobotB to turn right
  ReceiveString(messageReceived);   // wait until RobotB has indicated it has completed task
  PlayTone(200, 20);

  SendString(messageToSend3);       // send command to RobotB to move forward
  ReceiveString(messageReceived);   // wait until RobotB has indicated it has completed task
  PlayTone(200, 20);


}