Capixabots - Clarc Klener

#pragma config(Sensor, S1,     EyeBoxSize,     sensorEV3_Color)
#pragma config(Sensor, S2,     EyeDown,        sensorEV3_Color, modeEV3Color_Color)
#pragma config(Sensor, S3,     EyeRight,       sensorEV3_Color)
#pragma config(Sensor, S4,     EyeLeft,        sensorEV3_Color)
#pragma config(Motor,  motorA,          motorRight,    tmotorEV3_Large, PIDControl, encoder)
#pragma config(Motor,  motorB,          motorLeft,     tmotorEV3_Large, PIDControl, encoder)
#pragma config(Motor,  motorC,          motorGarra_Abrir, tmotorEV3_Large, openLoop, encoder)
#pragma config(Motor,  motorD,          motorGarra_Levantar, tmotorEV3_Large, openLoop, encoder)

#define ESQUERDA 0
#define DIREITA 1
#define RETO 2
#define PARAR 3
#define MEIAVOLTA_ESQ 4
#define MEIAVOLTA_DIR 5

#define PEGAR 0 //garra
#define SOLTAR 1

#define AZUL 3
#define VERDE 4
#define AMARELO 5
#define VERMELHO 6
#define BRANCO 7

#define SPOT_AZUL 0
#define SPOT_AMARELO 1
#define SPOT_VERMELHO 2

#define GRANDE 1




//CALIBRAÇÃO
int Angle_turn90 = 194;
int LF_blackThreshold = 13;




bool bIniciar = false, bTerminar = false, bReposicionar = false;
bool SPOT[3][2]; //[x][0] = ESQUERDA, [x][1] = GRANDE

int iBlocoCount = 0;




void Beep(int Quantidade)
{
	for(int i = 0; i < Quantidade; i++)
	{
		playTone(440, 15);
		wait1Msec(300);
	}
}

void MoverReto(int Angle, int Velocidade)
{
	moveMotorTarget(motorLeft, Angle, Velocidade);
	moveMotorTarget(motorRight, Angle, Velocidade);

	waitUntilMotorStop(motorLeft);
	waitUntilMotorStop(motorRight);
}

void Girar(int Movement, int Velocidade = 30, bool Intersecao = false, int Angulo = Angle_turn90)
{
	if(Intersecao)
		MoverReto(125, 30); //quando girar estar alinhado com a linha caso esteja na intersecao

	switch(Movement)
	{
		case ESQUERDA:
			moveMotorTarget(motorLeft, Angulo, -Velocidade);
			moveMotorTarget(motorRight, Angulo, Velocidade);
			break;
		case DIREITA:
			moveMotorTarget(motorLeft, Angulo, Velocidade);
			moveMotorTarget(motorRight, Angulo, -Velocidade);
			break;
		case MEIAVOLTA_ESQ:
			moveMotorTarget(motorLeft, Angulo*2, -Velocidade);
			moveMotorTarget(motorRight, Angulo*2, Velocidade);
			break;
		case MEIAVOLTA_DIR:
			moveMotorTarget(motorLeft, Angulo*2.14, Velocidade);
			moveMotorTarget(motorRight, Angulo*2.14, -Velocidade);
			break;
	}

	waitUntilMotorStop(motorLeft);
	waitUntilMotorStop(motorRight);
}



void LineSquaring(int EndMovement = PARAR, int speedTimes = 5, int speed = 10, int retAfter = 2, int target = 20, int targetBlack = 16, int targetWhite = 22)
{
	retAfter *= 1000; //msec

	//int speedFineAdjs = speed/2;

	//retAfter in seconds
	//target intensity value (metade preto/branco) | targetBlack (dentro da linha) | targetWhite (fora da linha)

	setLEDColor(ledRed); //start the first stage
	//writeDebugStreamLine("----- First Stage -----");

	motor[motorLeft] = speed*speedTimes;		//EyeLeft
	motor[motorRight] = speed*speedTimes;	//EyeRight

	while(true) //achar linha
	{
		if(SensorValue[EyeLeft] <= target) //da esquerda achou a linha
		{
			motor[motorLeft] = 0; //B
			//motor[motorRight] = speed; //anda pra frente
			//while(SensorValue[EyeRight] > target) //andar pra frente at� que o valor seja menor ou = que o target
			//{}
			motor[motorRight] = 0;
			break;
		}
		else if(SensorValue[EyeRight] <= target) //da direita achou a linha
		{
			motor[motorRight] = 0; //C
			//motor[motorLeft] = speed; //frente
			//while(SensorValue[EyeLeft] > target)
			//{}
			motor[motorLeft] = 0;
			break;
		}
	}

	setLEDColor(ledOrange); //start the fine adjustment
	//writeDebugStreamLine("----- Fine Stage -----");

	clearTimer(T1);
	while(time1[T1] <= retAfter)
	{
		if(SensorValue[EyeLeft] < targetBlack) //da esquerda vendo tudo preto
			motor[motorLeft] = -speed; //anda pra tr�s pq queremos que ele fique no target
		else if(SensorValue[EyeLeft] > targetWhite) //da esquerda vendo tudo branco
			motor[motorLeft] = speed; //anda pra frente
		else //ja ta no luga certo!
			motor[motorLeft] = 0;

		if(SensorValue[EyeRight] < targetBlack) //mesmo que ja feito acima mas para o da direita!
			motor[motorRight] = -speed;
		else if(SensorValue[EyeRight] > targetWhite)
			motor[motorRight] = speed;
		else
			motor[motorRight] = 0;

		wait1Msec(50);
		//final check
		if((SensorValue[EyeLeft] > targetBlack && SensorValue[EyeLeft] < targetWhite) &&
			(SensorValue[EyeRight] > targetBlack && SensorValue[EyeRight] < targetWhite)) //valores estiverm entre
		{
			break;
		}
	}

	//writeDebugStreamLine("Line Squared! Target(%d-%d) EyeLeft: %d | EyeRight: %d", targetBlack, targetWhite, SensorValue[EyeLeft], SensorValue[EyeRight]);
	setLEDColor(ledGreen);

	if (EndMovement != PARAR)
		Girar(EndMovement, speed*3.5, true);
}

void SeguirLinha_Mover(int Movement, int Velocidade = 35)
{
	switch(Movement)
	{
		case ESQUERDA:
			motor[motorLeft] = Velocidade-10;
			motor[motorRight] = Velocidade+10;
			break;
		case DIREITA:
			motor[motorLeft] = Velocidade+10;
			motor[motorRight] = Velocidade-10;
			break;
		case RETO:
			motor[motorLeft] = Velocidade;
			motor[motorRight] = Velocidade;
			break;
		case PARAR:
			motor[motorLeft] = 0;
			motor[motorRight] = 0;
			break;
	}
}

void SeguirLinha(int EndMovement, bool MoveForwardAfterSTOP = true, int Velocidade = 45, int leftThreshold = LF_blackThreshold, int rightThreshold = LF_blackThreshold)
{
	while(true)
	{
		if ((SensorValue(EyeLeft) >= leftThreshold) && (SensorValue(EyeRight) >= rightThreshold)) //os 2 vendo branco
			SeguirLinha_Mover(RETO, Velocidade);
		else if ((SensorValue(EyeLeft) <= leftThreshold) && (SensorValue(EyeRight) <= rightThreshold)) //os 2 vendo preto
		{
			if (EndMovement == PARAR)
			{
				SeguirLinha_Mover(PARAR);
				if(MoveForwardAfterSTOP)
					MoverReto(125, 30);
				return;
			}
			else
			{
				Girar(EndMovement, Velocidade, true); //nao precisa zerar os motores pq atualiza
				return;
			}
		}
		else if (SensorValue(EyeLeft) >= leftThreshold) // esq vendo branco e dir vendo preto
			SeguirLinha_Mover(DIREITA, Velocidade);
		else if (SensorValue[EyeRight] >= rightThreshold) //dir vendo branco e esq vendo preto
			SeguirLinha_Mover(ESQUERDA, Velocidade);


		//wait1Msec(10);
	}
}


void FindColor(int Color, int Speed = 45)
{
	motor[motorLeft] = Speed;		//EyeLeft
	motor[motorRight] = Speed;	//EyeRight

	bool Loop = true;

	while(Loop) //achar linha
	{
		switch(Color)
		{
			case VERDE:
				if( ((SensorValue[EyeLeft] >= 7) && (SensorValue[EyeLeft] <= 22)) &&
					((SensorValue[EyeRight] >= 7) && (SensorValue[EyeRight] <= 22)) )
				{
					Loop = false;
				}
				break;
			case BRANCO:
				if( ((SensorValue[EyeLeft] >= 30) && (SensorValue[EyeLeft] <= 50)))
				{
					motor[motorLeft] = 0;
					while((SensorValue[EyeRight] <= 30) || (SensorValue[EyeRight] >= 50))
					{}
					motor[motorRight] = 0;
					Loop = false;
				}
				else if( ((SensorValue[EyeRight] >= 30) && (SensorValue[EyeRight] <= 50)))
				{
					motor[motorRight] = 0;
					while((SensorValue[EyeLeft] <= 30) || (SensorValue[EyeLeft] >= 50))
					{}
					motor[motorLeft] = 0;
					Loop = false;
				}
				break;
		}
	}

	motor[motorLeft] = 0; //B
	motor[motorRight] = 0;
}

void Garra(int Movement)
{
	switch(Movement)
	{
		case SOLTAR:
			moveMotorTarget(motorGarra_Levantar, 1000, 127); //abaixar
			waitUntilMotorStop(motorGarra_Levantar);

			motor[motorGarra_Abrir] = 127; //abrir
			wait1Msec(1550);
			motor[motorGarra_Abrir] = 0;

			bReposicionar = true;
			break;
		case PEGAR:
			moveMotorTarget(motorGarra_Levantar, 1630, 127); //abaixar
			waitUntilMotorStop(motorGarra_Levantar);

			motor[motorGarra_Abrir] = -127; //fechar
			wait1Msec(2000);
			motor[motorGarra_Abrir] = 0;

			moveMotorTarget(motorGarra_Levantar, 1630, -127); //levantar
			waitUntilMotorStop(motorGarra_Levantar);
			break;
	}
}

bool GetBlockSize()
{
	if(SensorValue(EyeBoxSize) > 0)
	{
		Beep(1);
		return true;
	}
	else
	{
		Beep(2);
		return false;
	}
}

bool PegarBloco(int IndexSpot) //retorna esquerda ou direita
{
	wait1Msec(100); //esperar as rodas pararem/encoder se nao ele vai torto!
	MoverReto(132, 30);

	if(GetBlockSize() == SPOT[IndexSpot][GRANDE])
	{
		Garra(PEGAR);
		return true; //parou do lado direito
	}
	else
	{
		MoverReto(64, -30);
		//Girar(MEIAVOLTA_ESQ);
		Girar(ESQUERDA, 30, false, Angle_turn90*(2+(iBlocoCount/9)));
		MoverReto(200, 30);
		Garra(PEGAR);

		iBlocoCount++;
		return false;
	}
}

void SoltarBloco(int IndexSpot)
{
	MoverReto(320, 20);

	if(SPOT[IndexSpot][ESQUERDA])
	{
		Girar(ESQUERDA, 30, false, Angle_turn90*0.40);
		Garra(SOLTAR);
		Girar(DIREITA, 30, false, Angle_turn90*0.40);
	}
	else
	{
		Girar(DIREITA, 30, false, Angle_turn90*0.40);
		Garra(SOLTAR);
		Girar(ESQUERDA, 30, false, Angle_turn90*0.40);
	}

	FindColor(BRANCO, -20); //andar pra tr�s at� chegar no branco
	LineSquaring(PARAR, 1,-10); //alinhar com o preto
}

int GetColor_EyeDown()
{
	long i_redValue = 0;
	long i_greenValue = 0;
	long i_blueValue = 0;

	long redValue;
	long greenValue;
	long blueValue;

	wait1Msec(250); //esperar parar de tremer

	for(int i = 0; i < 50; i++)
	{
		getColorRGB(EyeDown, redValue, greenValue, blueValue);
		i_redValue += redValue;
		i_greenValue += greenValue;
		i_blueValue += blueValue;
		wait1Msec(25);
	}

	redValue = i_redValue/50.0;
	greenValue = i_greenValue/50.0;
	blueValue = i_blueValue/50.0;

	if(redValue >= 15 && greenValue >= 15 && blueValue >= 15) //branco 17,19,18
		return BRANCO;
	else if(redValue >= 12 && greenValue <= 5 && blueValue <= 5) //vermelho 18,2,2
		return VERMELHO;
	else if(redValue >= 15 && greenValue >= 12 && blueValue <= 8) //amarelo 17,16,4
		return AMARELO;
	else
		return AZUL;

	return -1;
}

bool BlockSizeSpot(int IndexSpot, int GndColor)
{
	switch(IndexSpot)
	{
		case SPOT_AZUL:
			if(GndColor == AZUL)
				return true;
			break;
		case SPOT_AMARELO:
			if(GndColor == AMARELO)
				return true;
			break;
		case SPOT_VERMELHO:
			if(GndColor == VERMELHO)
				return true;
			break;
	}

	return false;
}

void AttCorSpot(int IndexSpot)
{
	int GndColor;
	int MoveAngle = 160;

	FindColor(BRANCO, 20);
	FindColor(VERDE, 20);
	FindColor(BRANCO, -20); //andar pra trás até chegar no branco
	LineSquaring(PARAR, 1,-10); //alinhar com o preto

	MoverReto(300, 20); //chegar perto da parede
	Girar(DIREITA, 30, false, Angle_turn90*1.03); //� melhor ele errar do que mover a parede!
	MoverReto(MoveAngle, -20); //ir para trás pra ver o piso de trás

	GndColor = GetColor_EyeDown();

	if(GndColor == BRANCO)
	{
		MoverReto(MoveAngle*2, 30); //pra frente pra ver o outro!
		GndColor = GetColor_EyeDown(); //atualiza

		SPOT[IndexSpot][ESQUERDA] = false; //o piso está na direita

		MoverReto(MoveAngle, -30);
	}
	else
	{
		SPOT[IndexSpot][ESQUERDA] = true; //achou o piso no spot da esquerda

		MoverReto(MoveAngle, 30);
	}

	SPOT[IndexSpot][GRANDE] = BlockSizeSpot(IndexSpot, GndColor);

	writeDebugStreamLine(" Cor Atualizada! Spot: %d \n Piso na esquerda: %d \n Bloco Grande: %d \n ---------------------------------------\n", IndexSpot, SPOT[IndexSpot][ESQUERDA], SPOT[IndexSpot][GRANDE]);

	Girar(ESQUERDA); //voltou pro inicial

	FindColor(BRANCO, -20); //andar pra trás até chegar no branco
	LineSquaring(PARAR, 1,-10); //alinhar com o preto
}



task GarraLoop()
{
	while(true)
	{
		if(bIniciar)
		{
			moveMotorTarget(motorGarra_Levantar, 1690, -127); //levantar
			waitUntilMotorStop(motorGarra_Levantar);

			motor[motorGarra_Abrir] = 127; //abrir
			wait1Msec(1700);
			motor[motorGarra_Abrir] = 0;

			bIniciar = false;
		}
		else if(bTerminar)
		{
			moveMotorTarget(motorGarra_Levantar, 1690, 127); //abaixar
			waitUntilMotorStop(motorGarra_Levantar);

			motor[motorGarra_Abrir] = -127; //fechar
			wait1Msec(2000);
			motor[motorGarra_Abrir] = 0;

			bTerminar = false;
		}
		else if(bReposicionar)
		{
			moveMotorTarget(motorGarra_Levantar, 1000, -127); //levantar
			waitUntilMotorStop(motorGarra_Levantar);

			bReposicionar = false;
		}
	}
}



task GetRGB()
{
	long redValue;
	long greenValue;
	long blueValue;

	while (true)
	{
		getColorRGB(EyeDown, redValue, greenValue, blueValue);

		displayCenteredTextLine(1, "RGB: %d, %d, %d", redValue, greenValue, blueValue);
		sleep(100);
	}
}



task main()
{
	bool bGirarAzulDireita = false;

	/*startTask(GetRGB);
	while(true)
	{
	}*/

	/*FindColor(VERDE);
	FindColor(BRANCO);
	LineSquaring(ESQUERDA, 2);
	return;*/

	startTask(GarraLoop);

	setSoundVolume(127);

	bIniciar = true;
	Girar(ESQUERDA, 30, false, Angle_turn90*0.5); //sair da base
	MoverReto(250, 30); //pode confundir os desenhos do centro com o line follower
	SeguirLinha(PARAR);
	SeguirLinha(PARAR, false);
	AttCorSpot(SPOT_AZUL); //spot azul

	Girar(ESQUERDA, 30, false, Angle_turn90*1.36); //ir pro spot amarelo
	MoverReto(150, 45); //sair do preto/branco
	FindColor(BRANCO);
	LineSquaring(DIREITA, 2);
	SeguirLinha(PARAR, false);
	AttCorSpot(SPOT_AMARELO); //spot amarelo
	Girar(ESQUERDA, 30, false, Angle_turn90*1.1); //ir pegar o bloco amarelo!
	MoverReto(280, 45); //sair do preto/branco
	FindColor(BRANCO);
	LineSquaring(DIREITA);
	SeguirLinha(DIREITA);	 //indo pegar o bloco amarelo
	if(PegarBloco(SPOT_AMARELO)) //achou o bloco na direita...
		Girar(DIREITA, 30, false, Angle_turn90*0.25);
	else
	{
		Girar(ESQUERDA, 30, false, Angle_turn90*0.9);
		MoverReto(570, 45); //tem q fazer isso pra nao bater no bloco
		Girar(ESQUERDA);
		LineSquaring(PARAR, 2);
		MoverReto(100, 45); //sair do preto pra nao confundir com o verde
	}
	FindColor(VERDE); //levar o bloco amarelo
	FindColor(BRANCO);
	LineSquaring(ESQUERDA, 2);
	SeguirLinha(PARAR, false); //chegou no cruzamento do spot
	SoltarBloco(SPOT_AMARELO);

	Girar(ESQUERDA, 30, false, Angle_turn90*1.27); //ir pegar o bloco azul!
	MoverReto(280, 45); //sair do preto/branco
	FindColor(BRANCO);
	LineSquaring(ESQUERDA);
	SeguirLinha(DIREITA); //chegou no cruzamento do spot AZUL
	SeguirLinha(DIREITA);	 //indo pegar o bloco AZUL
	if(PegarBloco(SPOT_AZUL)) //achou o bloco na direita...
	{
		Girar(DIREITA, 30, false, Angle_turn90*0.44);
		bGirarAzulDireita = true;
	}
	else
	{
		Girar(ESQUERDA, 30, false, Angle_turn90*0.44);
		bGirarAzulDireita = false;
	}
	MoverReto(410, 45); //sair do preto/branco
	LineSquaring(PARAR, 3);
	MoverReto(150, 45); //sair do preto/branco
	FindColor(VERDE); //levar o bloco azul!
	FindColor(BRANCO);
	if(bGirarAzulDireita) //depende de qual bloco azul ele for pegar...
	{
		LineSquaring(DIREITA, 2);
		SeguirLinha(ESQUERDA);
	}
	else
	{
		LineSquaring(ESQUERDA, 2);
		SeguirLinha(DIREITA);
	}

	SeguirLinha(PARAR, false); //chegou no cruzamento do spot
	SoltarBloco(SPOT_AZUL); //soltar o bloco azul!

	//voltar pra base
	Girar(DIREITA, 25, false, Angle_turn90*1.95);
	bTerminar = true;
	SeguirLinha(PARAR, true, 20);
	SeguirLinha(PARAR, false, 20);
	MoverReto(365, 30);
	Girar(DIREITA, 30, false, Angle_turn90*0.5);

	playSoundFile("Air release");
	wait1Msec(1000);
	playSoundFile("Cheering");

}