#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,  motorB,          rightMotor,    tmotorNXT, PIDControl, encoder)
#pragma config(Motor,  motorC,          leftMotor,     tmotorNXT, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*RESCUE ROBOTICS WORKSHOP #2


                          C A L - T R A N S   D E M O
                                  Ron Kessler
                               Created 8/13/2018

Updated 8/14/2018
					Tested different approach distances and settled on 25cm (10in).


PURPOSE:	Demonstrates how to combine line following with obstacle avoidance! Several robots
					will follow a course without running into each other... just like on the highway.
					Your bot can use the code from the line follower and Watch Out projects	on the test track.

BEHAVIORS: Code the bot to do line following and monitor the sonar sensor at the same time. The goal is to
						have multiple bots run the course at the same time without crashing. Your bot should stop if it gets
						too close the the bot in front. When the bot in front moves away, your machine should go again.


SETUP:		When setting up sensors, be sure to select lightSensor | ActiveLight on port 2 as before so you
					can re-use your code. Make sure to add the sonar sensor to port 4 and name it as we did before.

					Remember, the light sensor returns a higher number when it sees white and lower for the black line.
					Calibrate your light sensor like before and determine the threshold used to keep the bot following the line.



SUGGESTIONS: Make your bot move slowly so it can respond to changing light and also the distance from the
							vehicle in front.
*/

//---STEP 1: CREATE VARIABLES AND OUR FUNCTIONS AS BEFORE

int lightThreshold = 40; 						//for my robot.
int LMotorSpeed = 20;								//same as before
int RMotorSpeed = 10;


//*************NEW STUFF******************
//---add this variable and new function to managed the sonar part.
int mySafeDistance = 25;						//about 10" is close enough to the car in front


void StopAndWait()									//is there an obstacle in front of you? Wait until it is clear.
{
	while(SensorValue(sonarSensor) <= mySafeDistance)
	{
		motor[leftMotor] = 0;
		motor[rightMotor] = 0;
	}

}
//****************************************


//**************UNCHANGED***************

//---these are the same as the line follower app
void MoveLeft (int LMotor, int RMotor)
{
	//---make rt motor go faster than left one
	motor[rightMotor] = LMotor;
	motor[leftMotor] = RMotor;
}

void MoveRight (int LMotor, int RMotor)
{
	//---make rt motor go faster than left one
	motor[rightMotor] = LMotor;
	motor[leftMotor] = RMotor;
}
//**************************************

//*************************MORE NEW STUFF******************
//STEP 2: Add this function here. Since I use MoveRight/Left functions, they MUST be declared first.
//---drive on the line because the coast is clear!
void goAhead()
{
	 if(SensorValue[lightSensor] < lightThreshold)		//does it see dark? then turn left
      {
        MoveLeft(LMotorSpeed, RMotorSpeed);
      }
      else                              							//it sees white so turn right
      {
        MoveRight(RMotorSpeed, LMotorSpeed);
		  }

}
//**********************************************************

//STEP 3: MAKE IT SO! Since stopping is the mission critical behavior, it needs to be the first test we make!!!

task main()
{

   while(true)                           							// drive forever
   {
     if(SensorValue[sonarSensor] <= mySafeDistance)		//is there something in the way?
       StopAndWait();																	//then stop & wait until the road is clear
     else
       goAhead();																			//else, all clear so keep driving on the line


   }
}



// Option 2: Use a timer to control how long the bot drives around

//task main()
//{
//  ClearTimer(T1);
//	while(time1[T1] < 20000)               							// drive for 20seconds
//	{
//     if(SensorValue[sonarSensor] <= mySafeDistance)		//is there something in the way?
//       StopAndWait();																	//then stop & wait until the road is clear
//     else
//       goAhead();																			//else, all clear so keep driving on the line


//   }
//}