#pragma config(Sensor, S4,     SonarSensor,    sensorSONAR)
#pragma config(Motor,  motorB,          motorLeft,     tmotorNXT, PIDControl, encoder)
#pragma config(Motor,  motorC,          motorRight,    tmotorNXT, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//Basic Sonar Navigation for LEGO NXT   WATCH OUT!
//Uses If-Else

//ROBOTC_Version 3.65


task main()
{
	// Distance to maintain to the target (30 cm)
	const int distanceToMaintain = 30;

	int currentDistance = 0;

	while(true)
	{
		// Read the sensor & display distance on LCD
		currentDistance = SensorValue[SonarSensor];
		nxtDisplayTextLine(4, "Dist: %3d cm", currentDistance); //%3d means format number to 3 digits

		// We're too far away, move forward
		if (currentDistance > distanceToMaintain)
		{
			motor[motorLeft] = 30;
			motor[motorRight] = 30;
		}
		// We're too close, move backwards
		else if (currentDistance < distanceToMaintain)
		{
			motor[motorLeft] = -30;
			motor[motorRight] = -30;
			wait1Msec(1000);

			//---do a pivot turn
			motor[motorLeft] = 0;
			motor[motorRight] = 50;
			wait1Msec(1000);
		}

		//Loop to monitor value in Sensor debugger window
		Sleep(50);  //so we can see the readings
	}
}