#pragma config(Sensor, S1,     touch,          sensorTouch)
#pragma config(Sensor, S3,     ultrasonic,     sensorSONAR)
#pragma config(Sensor, S4,     light,          sensorLightActive)
#pragma config(Motor,  motorB,          Right_Motor,   tmotorNXT, PIDControl, encoder)
#pragma config(Motor,  motorC,          Left_Motor,    tmotorNXT, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//Global Variables
	int touchSensor;
	int ultraSensor;
	int lightSensor;
//sensor task
task sensor ()
{
	touchSensor = SensorValue[touch];
	ultraSensor = SensorValue[ultrasonic];
	lightSensor = SensorValue[light];
	wait1Msec(100);
}
//straight function

void straight (int distRef, int power)
{
	int whlDia = 2.2;
	int distance =0;
	int rgtMtrEnc = 0;
	int disRef = 20;

	motor[Right_Motor] = power;
	motor[Left_Motor] = power;

	nMotorEncoder[motorB] = 0;

	distance = 0;
	while (distance < disRef)
	{
		rgtMtrEnc = nMotorEncoder[Right_Motor];
		distance = 	(rgtMtrEnc/360)*(whlDia*3.14);
		wait1Msec(50);
	}
	motor[Right_Motor] = 0;
	motor[Left_Motor] = 0;
	return;
}

//Function turn
void turn(int angle, int power)
{

	nMotorEncoder[Left_Motor] = 0;
	nMotorEncoder[Right_Motor] = 0;

	int rgtMtrDir;
	int lftMtrDir;
	int turnRef;
	int turnEnc;

	switch (angle)
	{
	case 180:
		rgtMtrDir = -1;
		lftMtrDir = 1;
		turnRef = 500;
		break;
	case -180:
		rgtMtrDir = 1;
		lftMtrDir = -1;
		turnRef = 500;
		break;
	case 90:
		rgtMtrDir = -1;
		lftMtrDir = 1;
		turnRef = 500;
		break;
	case 45:
		rgtMtrDir = -1;
		lftMtrDir = 1;
		turnRef = 500;
		break;
	case 20:
		rgtMtrDir = -1;
		lftMtrDir = 1;
		turnRef = 500;
		break;
	case -90:
		rgtMtrDir = 1;
		lftMtrDir = -1;
		turnRef = 500;
		break;
	case -45:
		rgtMtrDir = 1;
		lftMtrDir = -1;
		turnRef = 500;
		break;
	case -20:
		rgtMtrDir = 1;
		lftMtrDir = -1;
		turnRef = 500;
		break;
	default:
		rgtMtrDir = -1;
		lftMtrDir = 1;
		turnRef = 500;
		break;
	}
	nMotorEncoder[Left_Motor] = 0;
	nMotorEncoder[Right_Motor] = 0;
	motor[Left_Motor] = lftMtrDir*power;
	motor[Right_Motor] = rgtMtrDir*power;
	turnEnc= 0;
	while (turnEnc > turnRef)
	{
		turnEnc = abs(nMotorEncoder[Left_Motor])+abs(nMotorEncoder[Right_Motor]);
		wait1Msec(50);
	}
	motor[Left_Motor] = 0;
	motor[Right_Motor] = 0;

}


task main()
	{
		int numberSeg = 3;

		int actions[9] = (0,0,0,20,0,75,0,90,50);
		int power;
		int angle;
		int distance;

		startTask (sensor);

		displayString(0, "Follow Track");
		displayString(1, "LMtr-C,RMtr-B");
		displayString(2, "Tch-1, US-3");
		displayString(3, "Lgt-4");
		displayString(6, "Press ORG");
		displayString(7, "To Cont");
		while (nNxtButtonPressed !=3)
		{
			wait1Msec(100);
		}
		eraseDisplay();
		displayCenteredBigTextLine(3, "GO");
		playTone(440, 50);
		numberSeg = numberSeg*3;
		for (int i = 0; i < numberSeg; i=i+3)
		{
			if (actions[i] != 0)
			{
				straight (actions[i],actions[i+2]);
			}
	}
	playImmediateTone(440, 50);
	wait1Msec(500);

	int whlDia = 2.2;
	int distance =0;
	int rgtMtrEnc = 0;
	int disRef = 20;


	turn(180,45);
	playImmediateTone(440, 50);
	wait1Msec(500);
	while (true)
		wait1Msec(100);

	}