persistent float PADRAO_VERMELHO[4];
persistent int ACAO_VERMELHO;
persistent float PADRAO_AZUL[4];
persistent int ACAO_AZUL;
persistent float PADRAO_AMARELO[4];
persistent int ACAO_AMARELO;
persistent float PADRAO_VERDE[4];
persistent int ACAO_VERDE;
persistent float LINHA_PRETA_D;
persistent float MESA_BRANCA_D;
persistent float LINHA_PRETA_E;
persistent float MESA_BRANCA_E;
persistent float MAX_LUZ;
persistent float MIN_LUZ;
int ACAO_PARE=0;
int ACAO_VIRE_ESQ=1;
int ACAO_VIRE_DIR=2;
int ACAO_SIGA=3;
int SIGA_TEMPO;
void MenuCalibraCor()
{
float maxesc[]={64.0, 128.0};
int opcoes[]={0,0}, i=0, opcaoant=-1, mudou=0;
char Cor[4][13];
char Tipo[2][13];
Cor[0]="Vermelho";
Cor[1]="Verde";
Cor[2]="Azul";
Cor[3]="Amarelo";
Tipo[0]="Calibrar Cor";
Tipo[1]="Alterar Acao";
sleep(.2);
while(1)
{
opcoes[i]=(int)((float)knob()/maxesc[i]);
if ((opcoes[i]!=opcaoant)&&(i==0))
{
printf("Selecione Cor: %d - %s \n", opcoes[0]+1, Cor[opcoes[0]]);
}
if (((opcoes[i]!=opcaoant)&&(i==1))||(mudou==1))
{
printf("Opcoes: %d - %s \n", opcoes[1]+1, Tipo[opcoes[1]]);
mudou=0;
}
if (start_button())
{
i++;
mudou=1;
sleep(0.1);
}
if (i==2)
{
LerOpcoesCor(opcoes);
sleep(0.5);
return;
}
opcaoant=opcoes[i];
sleep(.1);
}
}
void LerOpcoesCor(int opcoes[2])
{
if (opcoes[1]==0)
{
CalibraCor(opcoes[0]);
}
else
{
if (opcoes[0] == 0)
ACAO_VERMELHO=SelecionaAcao();
if (opcoes[0] == 1)
ACAO_VERDE=SelecionaAcao();
if (opcoes[0] == 2)
ACAO_AZUL=SelecionaAcao();
if (opcoes[0] == 3)
ACAO_AMARELO=SelecionaAcao();
}
}
void CalibraCor(int opcoes)
{
float Cor[4];
int CA[2];
int CB[2];
int CC[2];
int CD[2];
int A[4];
int i;
for (i=0; i<4; i++)
{
LerCores(A);
}
for (i=0; i<2; i++)
{
LerCores(A);
CA[i]=A[0];
CB[i]=A[1];
CC[i]=A[2];
CD[i]=A[3];
}
Cor[0]= EncontrarMedias(CA,2);
Cor[1]= EncontrarMedias(CB,2);
Cor[2]= EncontrarMedias(CC,2);
Cor[3]= EncontrarMedias(CD,2);
if (opcoes == 0)
{
PADRAO_VERMELHO[0] = Cor[0];
PADRAO_VERMELHO[1] = Cor[1];
PADRAO_VERMELHO[2] = Cor[2];
PADRAO_VERMELHO[3] = Cor[3];
}
if (opcoes == 1)
{
PADRAO_VERDE[0] = Cor[0];
PADRAO_VERDE[1] = Cor[1];
PADRAO_VERDE[2] = Cor[2];
PADRAO_VERDE[3] = Cor[3];
}
if (opcoes == 2)
{
PADRAO_AZUL[0] = Cor[0];
PADRAO_AZUL[1] = Cor[1];
PADRAO_AZUL[2] = Cor[2];
PADRAO_AZUL[3] = Cor[3];
}
if (opcoes == 3)
{
PADRAO_AMARELO[0] = Cor[0];
PADRAO_AMARELO[1] = Cor[1];
PADRAO_AMARELO[2] = Cor[2];
PADRAO_AMARELO[3] = Cor[3];
}
printf("%f %f %f %f \n", Cor[0],Cor[1],Cor[2],Cor[3]);
sleep(0.4);
}
int DetectaCor(int imprime)
{
int A[4];
float MenorDist;
int Cor;
char Color[5][13];
LerCores(A);
if ( DistanciaCor(A,PADRAO_VERMELHO)<DistanciaCor(A,PADRAO_VERDE))
{
MenorDist=DistanciaCor(A,PADRAO_VERMELHO);
Cor=0;
}
else
{
MenorDist=DistanciaCor(A,PADRAO_VERDE);
Cor=1;
}
if ( DistanciaCor(A,PADRAO_AZUL)<MenorDist )
{
MenorDist=DistanciaCor(A,PADRAO_AZUL);
Cor=2;
}
if ( DistanciaCor(A,PADRAO_AMARELO)<MenorDist )
{
MenorDist=DistanciaCor(A,PADRAO_AMARELO);
Cor=3;
}
if (MenorDist>5000.0)
Cor=4;
if ( (Cor==0)&&(MenorDist<1200.0))
{
Cor=3;
}
Color[0]="Vermelho";
Color[1]="Verde ";
Color[2]="Azul ";
Color[3]="Amarelo ";
Color[4]="Nada ";
if (imprime)
{
printf("Vejo %s:\n", Color[Cor]);
}
return Cor;
}
float DistanciaCor(int corteste[4], float coralvo[4])
{
float soma=0.0;
float pesos[]={1.5,1.8,0.9,0.8};
int i;
float tmp;
for (i=0; i<4; i++)
{
tmp= (float)corteste[i];
soma=soma+pesos[i]*sqr(tmp-coralvo[i]);
}
return soma;
}
float sqr(float a)
{
return (a*a);
}
void LerCores(int A[])
{
int i;
for (i=0; i<4; i++)
{
AcenderLed(i);
sleep(0.08);
A[i]=analog(0);
sleep(0.08);
ApagarLed(i);
}
}
int SelecionaAcao()
{
float maxesc[]={64.0, 8.6};
int opcoes[]={0,0}, i=0, opcaoant=-1, mudou=0;
char Acao[4][13];
Acao[0]="Esq. 90 Graus";
Acao[1]="Dir. 90 Graus";
Acao[2]="Pare";
Acao[3]="Frente X segs";
sleep(.2);
while(1)
{
opcoes[i]=(int)((float)knob()/maxesc[i]);
if ((opcoes[i]!=opcaoant)&&(i==0))
{
printf("Acao: %d - %s \n", opcoes[i]+1, Acao[opcoes[i]]);
}
if ((((opcoes[i]!=opcaoant) && (i==1)) || (mudou==1)) && (opcoes[0]==3))
{
printf("Frente %d segs \n", (int)((float)knob()/maxesc[i]));
mudou=0;
}
if (start_button())
{
i++;
mudou=1;
sleep(0.1);
}
if (i==1)
{
if (opcoes[0]!=3)
return (opcoes[0]);
}
if (i==2)
{
SIGA_TEMPO= opcoes[1];
return (opcoes[0]);
}
opcaoant=opcoes[i];
sleep(.1);
}
return 0;
}
float EncontrarMedias(int valores[], int tamanho)
{
int i;
int soma=0;
for(i=0;i<tamanho;i++)
{
soma=soma+valores[i];
}
return ((float)soma/(float)tamanho);
}
void SigaLuz()
{
while(1)
{
int i=(int)(knob()/255)*2-1;
sleep(0.1);
printf("Polarisacao: %d",i);
while (1)
{
SegueLuz(AchaLuz(i),i);
}
}
}
int AchaLuz(int Lado)
{
int i;
int melhorposicao=0;
int melhorvalor=-255*Lado;
int valor;
GiraEsquerda();
for (i=0; i<180; i++)
{
valor=(analog(0)-analog(2));
if (Lado==1)
{
if (valor>melhorvalor)
{
melhorvalor=valor;
melhorposicao;
}
}
else
{
if (valor<melhorvalor)
{
melhorvalor=valor;
melhorposicao;
}
}
printf("%d \n",melhorvalor);
sleep(6.0/180.0);
}
GiraDireita();
for (i=0; i<180; i++)
{
valor=(analog(0)-analog(2));
if (Lado==1)
if (melhorvalor-valor<=2)
break;
if (Lado==-1)
if (melhorvalor-valor>=-2)
break;
printf("%d \n",melhorvalor-valor);
sleep(6.0/180.0);
}
alloff();
return melhorvalor;
}
void SegueLuz(int melhorvalor, int Lado)
{
int valor;
while (!stop_button())
{
Frente();
sleep(3.0);
valor=(analog(0)-analog(2));
valor=(analog(0)-analog(2));
valor=(analog(0)-analog(2));
if (Lado==1)
{
if (valor>=melhorvalor)
{
melhorvalor=valor;
}
else
break;
}
else
{
if (valor<=melhorvalor)
{
melhorvalor=valor;
}
else
break;
}
}
}
void GiraEsquerda()
{
motor(0,-40);
motor(1,40);
}
void GiraDireita()
{
motor(0,40);
motor(1,-40);
}
void Frente()
{
motor(0,40);
motor(1,40);
}