#pragma config(Sensor, S1,     TouchSensorL,   sensorTouch)
#pragma config(Sensor, S2,     LightSensor,    sensorLightActive)
#pragma config(Sensor, S3,     TouchSensorR,   sensorTouch)
#pragma config(Sensor, S4,     SonarSensor,    sensorSONAR)
#pragma config(Motor,  motorA,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorB,          rightMotor,    tmotorNXT, PIDControl, encoder)
#pragma config(Motor,  motorC,          leftMotor,     tmotorNXT, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*

           U S I N G   S E N S O R S   T O   R E S C U E   V I C T I M S
                                   Find Victims

                                       SONAR
                                     Ron Kessler
                                  Created 8/9/2018

Updated 8/12/2018



PURPOSE: This demo is built upon the ObstacleAvoidance project. It should be the first major
					goal for your team. If they can make the bot do these behaviors reliably, then
					they will have a good start on the competition.

BEHAVIOR: The robot will autonomously roam around until it gets within 20 inches of an object.
					It then approaches cautiously and stops for a few seconds. Then it backs up a bit, turns around, and goes again.
					The bot will roam around until the battery dies or you turn it off.


STEP 1: Create functions for each behavior/activity.
				Create variables to hold critical values so we can change them easily!
*/

//---create variables to hold the distance away from an object (in cm).
int const sonarLimit = 50;    //50 = 20in  const = constant. The program cannot change this value.
int const closeEnough = 17;			// 7 inches
int const wait2LogVictim = 5000;		//stay next to object for 5 seconds (use for gps reading, take photo, etc)
int const myMotorSpeed = 35;


void MoveRobot(int LMotor, int RMotor)
{
		//---now run motors forward at whatever is passed in to this function
		motor[leftMotor] = LMotor;
		motor[rightMotor] = RMotor;
}

void VictimFound()			//the bot stops and sits next to the object for a specified time
{
		//---use this to add GPS logging or take a photo of object,etc.
		MoveRobot(0,0);				//stop
		wait1Msec(wait2LogVictim);
}

void BackOff()
{
		MoveRobot(0 , 0);			//stop before changing directions!
		wait1Msec(1000);


		MoveRobot(-15, -15);	//back up slowly
		wait1Msec(3000);

		//---do a pivot turn
		MoveRobot(0,50);
		wait1Msec(1000);

}

void ApproachSlowly()
{
		//---creep up to object. Could add touch sensor code here
			while(SensorValue(SonarSensor) > closeEnough)
				{
					MoveRobot(15,15);
				}

			if(SensorValue(SonarSensor) <= closeEnough) 	//has it found another object or is it in the clear?
				VictimFound();
			else
				MoveRobot(myMotorSpeed, myMotorSpeed);

}



task main()
{
	//Step 2: Define behaviors: MOVING & MONITORING AT THE SAME TIME.
			while (true)																		//infinite loop
			{
				//no obstacle?
				if(SensorValue(SonarSensor) > sonarLimit)	//no obstacle?
					{
						MoveRobot(myMotorSpeed, myMotorSpeed);	//then keep roaming
					}
				else																					//otherwise, do these things!
					{
						ApproachSlowly();													//slows down motors automatically
						BackOff();																//move away and turn around
					}

					MoveRobot(myMotorSpeed, myMotorSpeed);	//back to normal speed & go again
			}
}