// 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
	  }

  }

}