#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);
 
 
}
 

// Leader-Follower  Lab -  Robot B (1 corner version)
#include "XbeeTools.h"
 
task main()
{
  InitRS485();
 
  string messageReceived;
  int count;
 
  while(true)
  {
          ReceiveString(messageReceived);  // wait for commands from Robot A
 
          nxtDisplayString(1, "A Said:%s", messageReceived);
 
          if (StringFind(messageReceived, "Fwd") >= 0)
          {
            StringDelete(messageReceived, 0, 3);
            count = atoi(messageReceived);
 
            nMotorEncoder[motorB] = 0;
 
            while ( nMotorEncoder[motorB] < count)  // move fwd until blk line
            {
             motor[motorB] = 50;
             motor[motorC] = 50;
            }
 
            motor[motorB] = 0;  // stop robot
            motor[motorC] = 0;
 
            SendString("Done");  // report back to Robot A
          }
 
 
          else if (StringFind(messageReceived, "TurnR") >= 0)
          {
            nMotorEncoder[motorC] = 0;
 
                  while (nMotorEncoder[motorC] < 165)
                  {
                    motor[motorB] = -50;
                    motor[motorC] = 50;
                  }
 
                  motor[motorB] = 0;  // stop robot
                  motor[motorC] = 0;
 
                  SendString("Done");  // report back to Robot A
          }
 
  }
 
}