#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
                                       TOUCH
                                     Ron Kessler
                                  Created 8/9/2018

Updated 8/12/2018




PURPOSE: This demo shows how to use the Touch Sensor in Port 1.
					As before when we set up the motors, open menu Robot | Motors & Sensor Setup
					Under the sensors tab, define port 1 for a touch sensor and name it TouchSensor.

BEHAVIOR: The robot will move forward until it runs into an object. It then stops & backs up a bit,
					turns around, then stops.


STEP 1: Create functions for setting motor speed and detecting touch sensor pressed. We will tell the computer these
				numbers are going to be integers.
*/

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 BumperPressed()
{
	MoveRobot(0,0);				//stop
	MoveRobot(-15,-15);		//back away slowly
	wait1Msec(3000);
}

void BackOff()
{
	//---do a 180 degree pivot turn
	nMotorEncoder[leftMotor] = 0;
	nMotorEncoder[rightMotor]= 0;
	nMotorEncoderTarget[rightMotor] = 720;		//adjust as needed for wheel size

	MoveRobot(0,50);
	wait1Msec(3000);

}


//Step 2: Press F7 to update our code

task main()
{
	//Step 3: Now use our new functions. Just type in their name.It will appear in the intellisense
	//        drop down menu. Press TAB to pop in in your code! It only shows up if you F7 first.

	//---use a loop move until object is detected.
	//---when the sensor is bumped, the loop finishes and the program continues....
	while(SensorValue(TouchSensor) == 0)
	{
		MoveRobot(35,35);
	}


	//---We ran into something so call the BumperPressed Function.
	BumperPressed();
	BackOff();
	MoveRobot(0,0);					//stop

}