P0 - Endelig version

#pragma config(Sensor, S1, colorsens, sensorEV3_Color)
#pragma config(Sensor, S4, distsense, sensorEV3_Ultrasonic)
#pragma config(Motor, motorA, claw, tmotorEV3_Medium, openLoop, encoder)
#pragma config(Motor, motorB, , tmotorEV3_Large, openLoop, reversed, encoder)
#pragma config(Motor, motorC, right, tmotorEV3_Large, openLoop, reversed, driveRight, encoder)
#pragma config(Motor, motorD, left, tmotorEV3_Large, openLoop, reversed, driveLeft, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//Variable to hold the value form the colorsensor
short colorVal;

//variable that holds the number of markers completed.
int marker = 0;

// variable that holds a floating number of the distance received from the distance sensor
float distance;

//Threashold values to indicate when the reflected color is black or white.
int blackThreshold = 25;
int whiteThreshold = 70;

//  Test variables to check whather the robot is standing on the blakeline or a false posetive
int test1, test2, test3, test4;

// A function that moves the robot a fixed angle rotation on each wheel, only forward
void MoveForwardAngle(int rightAngle, int leftAngle)
{
	moveMotorTarget(motorC, rightAngle, 20);
	moveMotorTarget(motorD, leftAngle, 20);
	waitUntilMotorStop(motorC);
	waitUntilMotorStop(motorD);
}
// A function that moves the robot a fixed angle rotation on each wheel, only backward



void checkSoundFile(const char *pFileName)
{
   bool bExists;

   bExists = bEv3FileExists(pFileName);
   while (!bSoundQueueAvailable)
   {}
   playSoundFile(pFileName);
   return;
}



void MoveBackwardAngle(int rightAngle, int leftAngle)
{
	moveMotorTarget(motorC, rightAngle, -20);
	moveMotorTarget(motorD, leftAngle, -20);
	waitUntilMotorStop(motorC);
	waitUntilMotorStop(motorD);
}

// Function to move towards bottle, with arm extension when distance i less then 5
//This function is used where we have to move the bottle to the center of the target
void MoveForwardToBottle2() {
	distance = getUSDistance(S4);
	if(	distance > 5 ) {
		setMotorSync(motorC, motorD, 0, -25);
	}
	else if(distance < 5) {
		setMotorSync(motorC, motorD, 0, 0);
		moveMotorTarget(motorA, 3800, -100);
		waitUntilMotorStop(motorA);
		marker++;
	}
}


// Function to move towards bottle, with arm extension when distance i less then 5
// This is used where we have to move the bottle past the black line
void MoveForwardToBottle(int clawSpeed)
{
	distance = getUSDistance(S4);
	colorVal = getColorReflected(colorsens);
	if (colorVal > blackThreshold && colorVal < whiteThreshold)
	{

		setMotorSpeed(motorC, 5);
		setMotorSpeed(motorD, 25);
	}
	else if (colorVal > whiteThreshold)
	{
		setMotorSpeed(motorC, 25);
		setMotorSpeed(motorD, 5);
	}

	else if (colorVal < blackThreshold)
	{
		setMotorSpeed(motorC, 30);
		setMotorSpeed(motorD, 30);
	}
	else if (distance < 10)
	{
		{
			setMotorSpeed(motorC, 0);
			setMotorSpeed(motorD, 0);
			while (distance > 5)
			{
				distance = getUSDistance(S4);
				setMotorSync(motorC, motorD, 0, -20);
			}
			setMotorSpeed(motorC, 0);
			setMotorSpeed(motorD, 0);
			// negative speed for arm to extend and posetive speed for the arm to go back
			moveMotorTarget(motorA, 3800, clawSpeed);
			waitUntilMotorStop(motorA);
			marker++;
		}
	}
}
//Function to move forward to grey, this is used to drive forward when following the grey line
void moveForwardToGrey()
{
	colorVal = getColorReflected(S1);

	while (colorVal < blackThreshold || colorVal > whiteThreshold)
	{
		colorVal = getColorReflected(S1);
		setMotorSync(motorC, motorD, 0, -25);
	}
}

// Turn each wheel of the robot to a fixed angle, can either turn right or left.
void Turn(int rightMotorAngle, int leftMotorAngle, int direction)
{

	// takes an integer of 1 that represent that the robot should make a right turn.
	if (direction == 1)
	{
		moveMotorTarget(motorC, rightMotorAngle, -20);
		moveMotorTarget(motorD, leftMotorAngle, 20);
		waitUntilMotorStop(motorC);
		waitUntilMotorStop(motorD);
	}
	// takes the integer of 0 to indicate that the robot should make a left turn.
	else if (direction == 0)
	{

		moveMotorTarget(motorC, rightMotorAngle, 20);
		moveMotorTarget(motorD, leftMotorAngle, -20);
		waitUntilMotorStop(motorC);
		waitUntilMotorStop(motorD);
	}
}
// Function used to detect black marker, and detect false markers, that sometimes occure om the track
void MoveForwardToMarker(int lowSpeed, int highSpeed)
{
	colorVal = getColorReflected(colorsens);
	if (colorVal > blackThreshold && colorVal < whiteThreshold)
	{

		setMotorSpeed(motorC, lowSpeed);
		setMotorSpeed(motorD, highSpeed);
	}
	else if (colorVal > whiteThreshold)
	{
		setMotorSpeed(motorC, highSpeed);
		setMotorSpeed(motorD, lowSpeed);
	}

	else if (colorVal < blackThreshold)
	{
		setMotorSpeed(motorC, 0);
		setMotorSpeed(motorD, 0);
		playTone(400, 20);
		sleep(100);
		colorVal = getColorReflected(S1);

		if (colorVal < blackThreshold)
		{
			test1 = 1;
		}
		Turn(5, 5, 0);
		colorVal = getColorReflected(S1);
		if (colorVal < blackThreshold)
		{
			test2 = 1;
		}
		Turn(10, 10, 1);
		colorVal = getColorReflected(S1);
		if (colorVal < blackThreshold)
		{
			test3 = 1;
		}
		Turn(5, 5, 0);
		MoveForwardAngle(10, 10);
		colorVal = getColorReflected(S1);
		if (colorVal < blackThreshold)
		{
			test4 = 1;
		}
		MoveBackwardAngle(20,20);
		if (test1 == 1 && test2 == 1 && test3 == 1 && test4)
		{

			marker++;
		}
		test1 = 0;
		test2 = 0;
		test3 = 0;
		test4 = 0;
	}
}

task main()
{
	displayBigTextLine(1, "Marker%f", marker);
	switch (marker)
	{
		// moves to the first marker.
	case 0:
		displayBigTextLine(1, "Marker%f", marker);
		while (marker == 0)
		{
			MoveForwardToMarker(13, 50);
		}
		setSoundVolume(100);
   	checkSoundFile("Marker1.rsf");         // Original EV3 soundfile (one.rsf), works fine
  	sleep(500);
		// turns towards the broken part of the lane and start driven until it meets the new lane, and follows the lane until the black marker.

	case 1:
		displayBigTextLine(1, "Marker%f", marker);
		Turn(90, 90, 1);
		moveForwardToGrey();

		while (marker == 1)
		{
			MoveForwardToMarker(10, 45);
		}
		setSoundVolume(100);
   	checkSoundFile("Marker2.rsf");         // Original EV3 soundfile (one.rsf), works fine
  	sleep(500);
		// turns towards the new lane after the broken lane and drives to the new lane and follows it until the next black marker.
	case 2:
		displayBigTextLine(1, "Marker%f", marker);
		Turn(45, 45, 0);
		MoveForwardAngle(100, 100);
		moveForwardToGrey();
		while (marker == 2)
		{
			MoveForwardToMarker(20, 50);
		}
		setSoundVolume(100);
   	checkSoundFile("Marker3.rsf");         // Original EV3 soundfile (one.rsf), works fine
  	sleep(500);
		//Turns the robot towards the side lane and drives up to the bottle and grabs it.
	case 3:
		displayBigTextLine(1, "Marker%f", marker);
		Turn(80, 80, 1);
		MoveForwardAngle(15, 15);
		moveForwardToGrey();
		playTone(800,15);
		while (marker == 3)
		{
			MoveForwardToBottle(-100);
		}


		// Drive the robot with the bottle forward
	case 4:
		displayBigTextLine(1, "Marker%f", marker);
	  MoveForwardAngle(380, 380);
		moveMotorTarget(motorA, 3800, 100);
		waitUntilMotorStop(motorA);
		marker++;
		setSoundVolume(100);
   	checkSoundFile("Marker6.rsf");         // Original EV3 soundfile (one.rsf), works fine
  	sleep(500);
		// Turns backward from the bottle marker, and returns to the track
	case 5:
		displayBigTextLine(1, "Marker%f", marker);
		moveMotorTarget(motorC, 100, -30);
		moveMotorTarget(motorD, 100, -30);
		waitUntilMotorStop(motorC);
		waitUntilMotorStop(motorD);
		Turn(225, 225, 0);
		MoveForwardAngle(200, 200);
		moveForwardToGrey();
		setMotorSpeed(motorC, 0);
		setMotorSpeed(motorD, 0);
		while (marker == 5)
		{
			MoveForwardToMarker(13, 50);
		}
		setSoundVolume(100);
   	checkSoundFile("Wii.rsf");         // Original EV3 soundfile (one.rsf), works fine
  	sleep(500);
		//moves past the tilt
	case 6:
		displayBigTextLine(1, "Marker%f", marker);

		MoveForwardAngle(200, 200);
		while (marker == 6)
		{
			MoveForwardToMarker(13, 50);
		}
		setSoundVolume(100);
   	checkSoundFile("Marker7.rsf");         // Original EV3 soundfile (one.rsf), works fine
  	sleep(500);
		// Moves past the parrallel lines
	case 7:
		displayBigTextLine(1, "Marker%f", marker);
		Turn(55, 55, 0);
		MoveForwardAngle(120, 120);
		moveForwardToGrey();
		colorVal = getColorReflected(S1);
		while (colorVal < whiteThreshold)
		{
			colorVal = getColorReflected(S1);
			setMotorSync(motorC, motorD, 0, -20);
		}
		moveForwardToGrey();
		while (marker == 7)
		{
			MoveForwardToMarker(13, 50);
		}
		setSoundVolume(100);
   	checkSoundFile("Marker4.rsf");         // Original EV3 soundfile (one.rsf), works fine
  	sleep(500);

		// Moves forward to the beginning of the big target
	case 8:
		displayBigTextLine(1, "Maker%f", marker);
		Turn(60, 60, 0);
		MoveForwardAngle(270, 270);
		while(marker == 8) {
			MoveForwardToMarker(13, 50);
		}
		setSoundVolume(100);
   	checkSoundFile("Wii.rsf");         // Original EV3 soundfile (one.rsf), works fine
		//Turn to the right, move forward untill we detect the bottle
	case 9:
		displayBigTextLine(1, "marker%f", marker);
		Turn(4,4,0);
		MoveForwardAngle(820,820);
		Turn(42,42,0);


		while( marker == 9) {
			MoveForwardToBottle2();
		}
		//Moves backwards and places the bottle in the middle of the target.
		//Then we drives backwards
	case 10:
		displayBigTextLine(1, "marker%f", marker);
		MoveBackwardAngle(880, 880);
		MoveForwardAngle(20, 20);
		moveMotorTarget(motorA, 3800, 100);
		waitUntilMotorStop(motorA);
		MoveBackwardAngle(820, 820);
		Turn(210, 210, 0);
		MoveForwardAngle(1000,1000);
		moveForwardToGrey();
		MoveForwardAngle(100, 100);
		Turn(90, 90, 0);
		while(marker == 10) {
			MoveForwardToMarker(13, 50);
		}
		setSoundVolume(100);
   	checkSoundFile("Marker6.rsf");
   	sleep(500);
		// moves around the bottle and finds the line to follow it again
	case 11:
		displayBigTextLine(1, "Marker%f", marker);
		Turn(55, 55, 1);
		moveMotorTarget(motorC, 650, 20);
		moveMotorTarget(motorD, 650, 20);
		waitUntilMotorStop(motorC);
		waitUntilMotorStop(motorD);
		Turn(100, 100, 0);
		moveForwardToGrey();
		Turn(70, 50, 1);
		while (marker == 11)
		{
			MoveForwardToMarker(25, 50);
		}
		setSoundVolume(100);
   	checkSoundFile("Marker5.rsf");
   	sleep(500);

	case 12:
		displayBigTextLine(1, "Marker%f", marker);
		MoveForwardAngle(700, 700);
		Turn(80,80,0);
		MoveForwardAngle(400, 400);
		Turn(80,80,1);
		MoveForwardAngle(200, 200);
		Turn(65,65,1);
		MoveForwardAngle(600, 600);
		Turn(70, 70, 0);
		MoveForwardAngle(200, 200);
		while(marker == 12){
			MoveForwardToMarker(15,40);
		}
		setSoundVolume(100);
   	checkSoundFile("Wii.rsf");
   	sleep(500);

	case 13:
		displayBigTextLine(1, "Marker%f", marker);
		Turn(55, 55, 1);
		moveMotorTarget(motorC, 650, 20);
		moveMotorTarget(motorD, 650, 20);
		waitUntilMotorStop(motorC);
		waitUntilMotorStop(motorD);
		Turn(110, 110, 0);
		moveMotorTarget(motorC, 500, 20);
		moveMotorTarget(motorD, 500, 20);
		waitUntilMotorStop(motorC);
		waitUntilMotorStop(motorD);
		while (marker == 13)
		{
			MoveForwardToMarker(25, 50);
		}

	case 14:
		Turn(8, 8, 0);
		setSoundVolume(100);
   	checkSoundFile("Lykke.rsf");
   	sleep(500);
		MoveForwardAngle(2700, 2700);
		setSoundVolume(100);
   	checkSoundFile("Lykke.rsf");
   	sleep(10000);
	}



}