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

#include "HTIRS2-driver.h"
#include "XbeeTools.h"

// IR Ball Passing Lab (2 robots)
// Robot A (leader)
// 1) Set IR Seeker frequency detection to 600 Hz and search and gather IR ball (set to 600Hz)
// 2) Once ball is gathered, set IR Seeker sensor frequency detection to 1200 Hz (same as goal beacon)
// 3) And then move the ball to the goal (IR beacon 1200Hz).
// 4) When Robot A reaches white line on way to goal, it will stop and allow ball to roll forward.
// 5) Robot A will then reverse and stop.
// 6) Robot A will then communicate to Robot B to go get the IR ball.

task main()
{
  int dir;
  int acS1, acS2, acS3, acS4, acS5;
  int temp1, temp2, max;


  int speedFast = 30;
  int speedSlow = 10;
  tHTIRS2DSPMode _mode = DSP_600;  // set to 600Hz for IR ball
  HTIRS2setDSPMode(IRSeeker, _mode);

  InitRS485();

  // Calibrate IR seeker sensor to detect ball is "captured" (close to robot and inside arms)
  // Experimentally, we have found that when intensity is 240 and dir = 7, then ball is "gathered"
  // These values will change based on the exect IR Seeker sensor used and the battery level of the IR ball
  // Check this experimentally and determine the values in your environment with your equipment.
  // A nice added feature would be an automated calibration step which is performed in software at start of this code

  int threshold = 240;

  while(true) // move toward IR ball, until ball is captured/gathered, then break out of loop
  {

    // get direction and intensity levels from IR Seeker sensor
    dir = HTIRS2readACDir(IRSeeker);
    HTIRS2readAllACStrength(IRSeeker, acS1, acS2, acS3, acS4, acS5);

    nxtDisplayTextLine(1,"dir = %d", dir);
    nxtDisplayTextLine(3,"%d %d %d %d %d", acS1, acS2, acS3, acS4, acS5);

    // find max intensity
    temp1 = max3(acS1, acS2, acS3);
    temp2 = max2(acS4, acS5);
    max = max2(temp1, temp2);
    nxtDisplayTextLine(5,"max = %d", max);

    if ( (dir == 7) && (max > (threshold - 5)))  // this is test to see is IR ball has been gathered/captured
    {
      motor[motorB] = 0;
      motor[motorC] = 0;
      PlayTone(200, 500);  // stop robot and alert when found ball
      wait1Msec(1000);

      break;  // break out of loop and move towards goal
    }

    if (dir < 5)  // IR ball on left
    {
      motor[motorB] = speedFast;
      motor[motorC] = speedSlow;
    }

    else if (dir > 5)  // IR ball is on right
    {
      motor[motorB] = speedSlow;
      motor[motorC] = speedFast;
    }

    else if (dir == 5)  // IR ball in front
    {
      motor[motorB] = speedFast;
      motor[motorC] = speedFast;
    }

    else if (dir == 0)  // no signal from IR sensor; pivot and search
    {
      motor[motorB] = speedFast;
      motor[motorC] = -speedFast;
    }
  }  // end of while loop

  wait1Msec(100);


  // Now that robot has captured/gathered the IR ball, look for beacon (1200Hz)
  // Move toward goal until robot reaches white line, then surge momentarily, reverse, then stop
  // then communicate to Robot B to go get IR ball

  _mode = DSP_1200;  // set IR Seeker sensor to look for IR beacon (goal) of 1200Hz
  HTIRS2setDSPMode(IRSeeker, _mode);

  // light sensor reading on carpet = 30-32
  // light sensor reading on white line 60-62
  // Experimentally determine your own values based on your environment

  while (SensorValue(light) < 45)  // move toward goal until robot reaches white line
  {
    dir = HTIRS2readACDir(IRSeeker);
    HTIRS2readAllACStrength(IRSeeker, acS1, acS2, acS3, acS4, acS5);

    nxtDisplayTextLine(1,"dir = %d", dir);
    nxtDisplayTextLine(3,"%d %d %d %d %d", acS1, acS2, acS3, acS4, acS5);

    temp1 = max3(acS1, acS2, acS3);
    temp2 = max2(acS4, acS5);
    max = max2(temp1, temp2);
    nxtDisplayTextLine(5,"max = %d", max);

    if (dir < 5)  // IR source on left
    {
      motor[motorB] = speedFast;
      motor[motorC] = speedSlow;
    }

    else if (dir > 5)  // IR source is on right
    {
      motor[motorB] = speedSlow;
      motor[motorC] = speedFast;
    }

    else if (dir == 5)  // IR source in front
    {
      motor[motorB] = speedFast;  // boost speed when directed at goal
      motor[motorC] = speedFast;
      wait1Msec(500);
    }

    else if (dir == 0)  // no signal from IR sensor; pivot and search
    {
      motor[motorB] = speedFast;
      motor[motorC] = -speedFast;
    }

  }

  // stop Robot A (and reverse a bit) when white line is reached

  motor[motorB] = 50;
  motor[motorC] = 50;
  wait1Msec(1000);
  motor[motorB] = -speedFast;
  motor[motorC] = -speedFast;
  wait1Msec(3000);
  motor[motorB] = 0;
  motor[motorC] = 0;


  // send command to Robot B to fetch IR ball and move forward to goal
  SendString("B get ball");

}