#pragma config(Sensor, S1,     TouchSensor,    sensorTouch)
#pragma config(Sensor, S2,     LightSensor,    sensorLightActive)
#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               !!*//

/*

                   I N T R O D U C T I O N   T O   S E N S O R S
                                Avoiding Obstacles
                                    SONAR & TOUCH
                                     Ron Kessler
                                  Created 8/9/2018



Updated 8/25/2018
				Setup for sonar and 1 touch sensor! See code below. (for the Workshops)

PURPOSE: This demo shows how to use the Sonar Sensor to roam around. Sonar is in port 4
					and TouchSensor is in port 1. I show how to use two sensors simultaneously. This really
					helps when the bot runs into something.

					As before when we set up the motors, open menu Robot | Motors & Sensor Setup
					Under the sensors tab, define port 4 for a Sonar sensor and name it SonarSensor and
					do the same for the TouchSensor unless you already have done so.

BEHAVIOR: The robot will move around the room until it gets within 8 inches of an object OR the bumper
					is pressed (ran into something). If is finds an obstacle, it stops, 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 safeDistanceFromObstacle = 20;    //20cm = 8 in  const = constant. The program cannot change this value.int const closeEnough = 17;			// 7 inches
int const myMotorSpeed = 50;


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

}


task main()
{
	//Step 2: Define behaviors: MOVING & MONITORING AT THE SAME TIME.
			while (true)																		//infinite loop
			{
				//using touch sensor? Use line 75. No? Use line 76.
				if(SensorValue(SonarSensor) > safeDistanceFromObstacle && SensorValue(TouchSensor) == 0)	//no obstacle?
				//if(SensorValue(SonarSensor) > safeDistanceFromObstacle)	//using sonar only? no obstacle?
					{
						MoveRobot(myMotorSpeed, myMotorSpeed);	//then keep roaming
					}
				else																					//otherwise, back off
					{
						BackOff();																//move away and continue roaming
					}

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