TRABALHO PRÁTICO 2

—-

Sensor Óptico-Ativo

* Observe que há um envoltório de estrutura lego sobre o sensor, de modo que o sensor LDR receba o mínimo possível de luzes ambiente já que estas poderiam variar o valor do sensor e dificultar a padronização dos sinais recebidos pelo mesmo. Na caracterização do sensor óptico ativo obtivemos os resultados obtidos abaixo, tanto para sem luz (cobrindo a montagem robô+objeto com uma caixa de papelão) quanto para a luz ambiente, retirando a respectiva caixa, calculando para ambos os casos (com motores ligados ou não) o seus respectivos valores médios (“VM”) e os seus desvios padrões (“DP”).

Sensor Óptico-Reflexivo Infra-Vermelho

Sensor Diferencial

/*************************** madeinbrasil.c ********************************/
/*
  Test program for Handy Board
 
  version 1.7 1/05
  testmotors() updated to reduce strain on HB motor drivers
 
  version 1.6 1/04
  CMUcam test updated to use new library and serial handlers
 
  version 1.5 5/03
  Expanded by Dan Gates to add the CMUcam comm test.
  Reqires serial.icb to be in the same directory.
 
  version 1.4 1/02
  Changed by Randy Sargent for IC 4.0 and to include srf04 sonar test.
 
  version 1.3 1/01
  Changed by Anne Wright for Botball to allow selecting which test
  using the knob.  Program previously called hb-expbd-test.c.  Also
  includes hbmenu.c.
 
  version 1.2 11/00
  tests IR emitter -dpm
 
  version 1.1 7/98
  Expanded by Dave Miller 7/13/98 dmiller@kipr.org
  to include servo  and to handle menuing.
 
  version 1.0 -- 26 nov 95
  Fred G. Martin (fredm@media.mit.edu)
 
*/
/* Parametros medidos:
 - Diametro da Roda: 43 [mm]
 - Distancia mais curta entre o Eixo-Motor ate sua respectiva roda: 35 [mm]
 - Distancia entre as duas rodas: 105 [mm]
 
   Extras:
 1) Voltas necessarias para alcancar 30cm: 300/(2*PI*RAIO_RODA) [mm]
 2) Num. Voltas x 24 dentes x 6 = Num. Passos necessarios
*/
 
//Parmetros modificveis
#define RAIO_RODA    21.5                      //Raio efetivo da roda+pneu em milmetros [mm]
#define PULSOS_VOLTA 12.0*24.0                 //De acordo com o disco do shaft encoder->6*2=12
 
//Parametros no-modificveis
#define PI        3.1416                      //Constante PI
#define REDUCAO   24.0                        //Reducao do motor ate a roda devido as engrenagens
#define VOLTA     (2.0*PI*(float)RAIO_RODA)   //Perimetro da roda [mm]
#define DX        VOLTA/(PULSOS_VOLTA)        //Menor delta de deslocamento [mm]
 
#define BITOLA    118.0                       //Distancia entre rodas [mm]
#define MM300     300.0
 
#define MOTOR_RPS  (int)(140/(REDUCAO*60))    //Rotacoes por segundo do motor utilizado [rps]
#define MOTOR_1    2                          // Amarelo lado visor LCD/Laranja (Direito)
#define MOTOR_2    3                          // Roxo lado visor LCD/Cinza (Esquerdo)
 
#define ENCODER_1  0
#define ENCODER_2  1
 
#define RPSID1     18                         //Velocidade do motor1 em RPS (default: 18)
#define RPSID2     18                         //Velocidade do motor2 em RPS
 
#define MOVE       0                          //Vai para frente ou faz curva
#define RIGHT      1                          //Go to right
#define LEFT       2                          //Go to left
 
#define KP 1                                   //Parmetro de controle Kp
#define KD 0.01                                //Parametro de controle Kd
 
// Programa exemplo
#define TESTNUM 8
 
#define DIFF 3
#define VERTICAL 1
#define HORIZONTAL 2
 
#define LED_RED      4
#define LED_GREEN    2
#define LED_BLUE     3
 
#define STOP_LIMIT   195                // Limiar de parada ao encontrar o bloco
#define RED_LIMIT    80                 // Limiar de valor do LDR para o bloco vermelho
#define GREEN_LIMIT  70                 // Limiar de valor do LDR para o bloco verde
#define BLUE_LIMIT   110                // Limiar de valor do LDR para o bloco azul
 
#use "cmucamlib.ic"
int data[100];
 
void main()
{
    int sel;
    char a[TESTNUM][16]={"Menu: Frente","Menu: Horizont","Menu: Vertical",
          "Menu: LDR_test", "Menu: 360", "Menu: Line", "Menu: Light LED", "Menu: IR Value"};
 
    printf("Grupo MIB:      Aperte START! %f ** %f",DX,PULSOS_VOLTA);
    while(!start_button());
    printf("Gire o KNOB para selecionar   \n");
    sleep(1.0);
 
    // Habilita encoder do canal 0
    enable_encoder(0);
    // Habilita encoder do canal 1
    enable_encoder(1);
    // Habilita saidas digitais
    poke(0x1009, 0x3c);
    LedOff(2);
    LedOff(3);
    LedOff(4);
    while(1) {
 
        printf("Gire o KNOB para selecionar   \n");
        sleep(0.5);
 
        sel = select_string(a,TESTNUM);
        if(sel==0) Go(300.0);
        else if(sel==1)PolarizedDiffSensor(2);
        else if(sel==2) PolarizedDiffSensor(2);
        else if(sel==3) SalvaDados();
        else if(sel==4) LDR_test();
        else if(sel==5) FollowLine();
        else if(sel==6) light_led_test();
        else if(sel==7) IR_test();
        else if(sel==8) break;
    }
    printf("Pronto!\n");
}
 
//Desliga os motores
void StopMotors()
{
    motor(MOTOR_1,0);
    motor(MOTOR_2,0);
    alloff();
}
 
//Funo de ramp up e ramp down para motores
//Obs.: Aumenta/Diminui gradativamente a velocidade do motor em 1s
void StartMotors(int speed1, int speed2)
{ 
    int i;
    int power1, power2;
    power1 = (speed1/10);
    power2 = (speed2/10);
    for (i=0;i<=10;i++)
      {
        motor(MOTOR_1,i*power1); //Gira motor 1 com i*int(speed2/10) de velocidade
        motor(MOTOR_2,i*power2); //Gira motor 2 com i*int(speed2/10) de velocidade
        msleep(200L);            //Aguarda movimento mecnico
    }
}
 
void Left()
{   
    int encid1, encid2;
    int pid = 0;
    float dist1=0.0, dist2=0.0;
    int  erroant1=0, erroant2=0,encsum1=0,encsum2=0;
 
    printf("Esquerda!\n");     //Imprime Status
    reset_encoder(0);
    reset_encoder(1);
    encid1 = 20;
    encid2 = 20;
 
    pid = start_process(PDControl(&encid1, &encid2, RIGHT));
    while((dist1 < (((float)BITOLA*0.94*(float)(PI))/4.0)) || (dist2 <(((float)BITOLA*0.94*(float)(PI))/4.0)))    //Meia volta = 90 graus
      {         
        encsum1 = encsum1 + read_encoder(0);
        encsum2 = encsum2 + read_encoder(1);         //soma acumulativa dos encoders
 
        reset_encoder(0);
        reset_encoder(1);
 
        dist1 = ((float)(encsum1))*DX;       //Calcula a distancia percorrida
        dist2 = ((float)(encsum2))*DX;
        printf("Dist1.: %f mm    Dist2.: %f mm\n",dist1,dist2);       //Imprime distancia percorrida
        msleep(30L);                         //Aguarda movimento mecanico
    }
    kill_process(pid);
    //Quando a distncia x for alcancada
    StopMotors();                                                //Desliga os motores
    sleep(0.5);                                                  //Aguarda movimento mecnico
 
    dist1 = ((float)(encsum1))*DX;       //Calcula a distancia percorrida
    dist2 = ((float)(encsum2))*DX;
    printf("D1.: %f mm D2.: %f mm\n",dist1,dist2);       //Imprime distancia percorrida
    //sleep(0.5);
    reset_encoder(0);
    reset_encoder(1);
    //printf("Parado.\n");                  //Imprime Status
    //while(!stop_button());
}
 
//Gira para a direita
void Right()
{
    float dist=0.0;
    int  erroant1=0, erroant2=0,encsum=0;
 
    /*teste*/
    int enc1sum=0,enc2sum=0;
    int encid1, encid2;
    int pid = 0;
 
    encid1 = 20;
    encid2 = 20;
 
 
    // Habilita encoder do canal 0
    reset_encoder(0);
    reset_encoder(1);
 
 
    printf("Direita!\n");   //Imprime Status
    pid = start_process(PDControl(&encid1, &encid2, LEFT));    
    while((dist <= (((float)BITOLA*(float)(PI))/4.0) ))    //Meia volta = 90 graus
      { 
        //        PDControl2(18,18,&erroant1,&erroant2,LEFT);
 
        //encsum = encsum + read_encoder(0) + read_encoder(1);         //soma acumulativa dos encoders
 
        enc1sum = enc1sum + read_encoder(0);
        enc2sum = enc1sum + read_encoder(1);
 
        reset_encoder(0);
        reset_encoder(1);
 
        dist = (float)((enc1sum + enc2sum)/2)*DX;    //Calcula a distancia percorrida
        printf("Dist.: %f mm\n",dist);       //Imprime distancia percorrida
        msleep(30L);                        //Aguarda movimento mecanico
 
 
    }                        //Gira at completar meia volta en cada roda
    //Quando a distncia x for alcancada
    kill_process(pid);
 
    StopMotors();                                                //Desliga os motores
    sleep(1.0);                                                  //Aguarda movimento mecnico
    //encsum = encsum + read_encoder(0) + read_encoder(1);         //soma acumulativa dos encoders
    enc1sum = enc1sum + read_encoder(0);
    enc2sum = enc1sum + read_encoder(1);
    dist = (float)((enc1sum+enc2sum)/2)*DX;    //Calcula a distancia percorrida
    printf("Dist.: %f mm\n",dist);       //Imprime distancia percorrida
 
    reset_encoder(0);
    reset_encoder(1);
}
 
//Desloca para frente x unidades de distncia [mm]
void Go(float x)
{
    int enc1=0, enc2=0, speed1, speed2;
    float dist=0.0;
    int erroant1=0, erroant2=0,encsum=0;
    int pid=0, pid1=1;
    int encid1, encid2;
 
    // Habilita encoder do canal 0
    reset_encoder(0);
    reset_encoder(1);
 
    if(x>=0.0)
      { printf("Indo pra frente!\n");  //Imprime Status
        //StartMotors(100,80);         //Liga os motores
        motor(MOTOR_1,100);
        motor(MOTOR_2,80);
        encid1 = 20;
        encid2 = 20;    
 
        pid = start_process(PDControl(&encid1, &encid2, MOVE));
        pid1 = start_process(le_motor());
    }
    else{
        printf("Indo pra Tras!\n");    //Imprime Status
        StartMotors(-100,-80);         //Liga os motores   
        x = -x;
    }
 
    while ((dist<x))                   //Enquanto dist<x e nao pressionar stop
      { 
        //        PDControl(20,20,&erroant1,&erroant2,MOVE); 
 
        encsum = encsum + read_encoder(0) + read_encoder(1);         //soma acumulativa dos encoders
 
        //        reset_encoder(0);
        //      reset_encoder(1);
 
        dist = (float) ((encsum)/2)*DX;    //Calcula a distancia percorrida
        //      printf("Dist.: %f mm\n",dist);       //Imprime distancia percorrida
        msleep(30L);                        //Aguarda movimento mecanico
 
    }
    //Quando a distncia x for alcancada
    kill_process(pid);
    kill_process(pid1);    
    StopMotors();                                                //Desliga os motores
    sleep(1.0);                                                  //Aguarda movimento mecnico
    encsum = encsum + read_encoder(0) + read_encoder(1);         //soma acumulativa dos encoders
    dist = (float)(encsum/2)*DX;    //Calcula a distancia percorrida
    printf("Dist.: %f mm\n",dist);       //Imprime distancia percorrida
 
    sleep(0.5);
    reset_encoder(0);
    reset_encoder(1);
}
 
void PDControl(int *encid1, int *encid2, int tipo)
{
    int enc1=0, enc2=0;
    int erro1=0, erro2=0;
    int i, pterm1, pterm2, dterm1, dterm2;
    int erroant1=0, erroant2=0;
    int speed1=0,speed2=0;
    int curve1[9]={9,13,14,16,17,18,19,19,20};
    int curve2[9]={10,14,16,18,19,20,20,20,20};
 
    reset_encoder(0);
    reset_encoder(1);
 
 
    while(!stop_button())
      {
        enc1 = read_encoder(0);
        enc1 = 5*enc1; //*5 = 30*enc1/6
        enc2 = read_encoder(1);
        enc2 = 5*enc2;
 
        erro1 = (*encid1-enc1);
        erro2 = (*encid2-enc2);
 
        pterm1 = (int)((float)KP*(float)erro1+0.5);
        pterm2 = (int)((float)KP*(float)erro2+0.5);
 
        dterm1 = (int) ((float)KD*(float)(erro1-erroant1)+0.5);
        dterm2 = (int) ((float)KD*(float)(erro2-erroant2)+0.5);
 
        erroant1 = erro1;
        erroant2 = erro2;
 
        /*
          integ = integ+erro;
          deriv = (erro/tempo) - deriv;
        */
 
        enc1 = *encid1+pterm1+dterm1;
        enc2 = *encid2+pterm2+dterm2;
 
        //relacao enc e speed   
        for(i=0;i<9;i++)
          {
            if(enc1>=curve1[i])
              speed1=i*10+20;
            if(enc2>=curve2[i])
              speed2=i*10+20;
        }
 
        //Saturacao dos valores (manter por seguranca)
        if (speed1<-100)
          speed1=-100;
        if(speed1>100)
          speed1=100;
        if(speed2<-100)
          speed2=-100;
        if(speed2>100)
          speed2=100;
 
        switch(tipo)
          {        
            case 0: //Frente ou curva
              motor(MOTOR_1,speed1);
            motor(MOTOR_2,speed2);
            break;
            case 1: //Direita
              motor(MOTOR_1,speed1);
            motor(MOTOR_2,-speed2);
            break;
            case 2: //Esquerda
              motor(MOTOR_1,-speed1);
            motor(MOTOR_2,speed2);
            break;
            default: {
                printf("Switch error!\n");
                break;
            }
        }
 
        reset_encoder(0);
        reset_encoder(1);
 
        msleep(33L);
    }
    StopMotors();
}
 
//Descobre qual a cor do bloco a frente do sensor
void DetectColor() //ldramb = valor do ldr apenas com luz ambiente
{ //Uma variavel para cada aquisicao do LDR com uma cor
    int ldramb, ldrall, ldrblue, ldrgreen,ldryellow,ldrred,ldrblack, ldrmaior=0;
    int cor=0;
    LedOff(LED_BLUE);
    LedOff(LED_RED);
    LedOff(LED_GREEN);
 
    //Aquisita valor atual do LDR
    sleep(1.0);
    ldramb=light(2);
    printf("LDR: %d\n", ldramb);
    sleep(3.0);
 
    //Ascende cada led e aquisita o valor do LDR (falta ajustar qual canal ascende cada LED)  
    LedOn(LED_RED);//bit_set(0x1008, 0b00010000);
    sleep(1.0);
    ldrred=light(2);
    printf("Led vermelho! %d\n",ldrred-ldramb);
    sleep(1.0);
    LedOff(LED_RED);//bit_clear(0x1008, 0b00010000);
 
    LedOn(LED_GREEN);//bit_set(0x1008, 0b00000100);
    sleep(1.0);
    ldrgreen=light(2);
    printf("Led verde! %d\n",ldrgreen-ldramb);
    sleep(1.0);
    LedOff(LED_GREEN);//bit_clear(0x1008, 0b00000100);
 
    LedOn(LED_BLUE);//bit_set(0x1008, 0b00001000);
    sleep(1.0);
    ldrblue=light(2);
    printf("Led azul! %d\n",ldrblue-ldramb);
    sleep(1.0);
    LedOff(LED_BLUE);//bit_clear(0x1008, 0b00001000);
 
    LedOn(LED_BLUE);
    LedOn(LED_RED);
    LedOn(LED_GREEN);
    sleep(1.0);
    ldrall=light(2);
    printf("Led todos! %d\n",ldrall-ldramb);
    sleep(1.0);
    LedOff(LED_BLUE);
    LedOff(LED_RED);
    LedOff(LED_GREEN);
 
    //Calcula cor do bloco
    //maior
    if(ldrmaior<ldrgreen-ldramb)
      ldrmaior=ldrgreen-ldramb;
    if (ldrmaior<ldrred-ldramb)
      ldrmaior=ldrred-ldramb;
    if (ldrmaior<ldrblue-ldramb)
      ldrmaior=ldrblue-ldramb;
 
    if(ldrred-ldramb==ldrmaior)
      {
        if((ldrred-ldrgreen)<15) {
            if(ldramb>120) cor=cor+3; //amarelo
            else cor=0; //preto
        }
        else cor=cor+1; //vermelho
    }
    if(ldrmaior==ldrgreen-ldramb){
        if((ldrgreen-ldrred)<15) {
            //if(ldramb>120) cor=cor+3; //amarelo
            cor=0; //preto
        }
        else
          cor=cor+2; //verde
    }
    if(ldrblue-ldramb==ldrmaior) cor=cor+4;  //azul
 
 
    //printf("LDR maior: %d\n",ldrmaior);
    //sleep(1.0);
    switch (cor) {
        // Verifica todas as combinacoes possiveis do calculo acima
        case 1: //vermelho
          printf("Bloco vermelho!\n");
        ColorAction();
        break;
        case 2: //verde
          printf("Bloco verde!\n");
        ColorAction();
        break;
        case 3: //vermelho+verde = amarelo
          printf("Bloco amarelo!\n");
        ColorAction();
        break;
        case 4: //azul
          printf("Bloco azul!\n");
        ColorAction();
        break;
        case 5: //azul + vermelho = roxo
          printf("Bloco roxo!\n");
        ColorAction();
        break;
        case 6: //verde+azul = anil/ciano (azul claro)
          printf("Bloco azul claro?\n");
        ColorAction();
        break;
        case 7: //azul+vermelho+verde = branco
          printf("Bloco Amarelo!\n");
        ColorAction();
        break;
        case 0: //preto
          printf("Bloco preto!\n");
        ColorAction();
        break;
        default: //Cor desconhecida
          break;
    }   
}
 
//Selecao da acao
void ColorAction() {
    int sel,sair=0;
    char a[5][16]={"Parar","Contornar Dir.","Contornar Esq.",
          "Empurrar","Sair"};
    //Espera o LCD mostrar a cor identificada
    sleep(5.0);
 
    printf("Gire o KNOB para selecionar\n");
    sleep(1.0);
 
    while(sair==0) {      
        printf("Gire o KNOB para selecionar\n");
        sleep(0.5);
 
        sel = select_string(a,5);
        if(sel==0) alloff();                         //Para todos os motores
        else if(sel==1) ColorTurnRight();            //Contorna pela direita
        else if(sel==2) ColorTurnLeft();             //Contorna pela esquerda
        else if(sel==3) ColorPushBlock();            //Empurra por x segundos
        else if(sel==4) sair=1;
        else if(sel==5) break;
    }
 
}
 
void ColorTurnLeft()
{
    int i;
    //Volta alguns centimetros
    //Go(-50.0);
    motor(MOTOR_1,-100);
    motor(MOTOR_2,-100);
    sleep(2.5);
    //Contorna
    Left();
    Go(200.0);
    Right();
    Go(500.0);
    Right();
    Go(200.0);
    Left();
}
 
void ColorTurnRight()
{
    int i;
    //Volta alguns centimetros
    //Go(-50.0);
    motor(MOTOR_1,-100);
    motor(MOTOR_2,-100);
    sleep(2.5);
    //Contorna
    Right();
    Go(200.0);
    Left();
    Go(500.0);
    Left();
    Go(200.0);
    Right();
}
 
void ColorPushBlock()
{
    int seg;
    while(!start_button()) {
        seg=knob_int_value(0,10);
        printf("Empurrar por: %d seg\n",seg);
        msleep(500L);
    }  
    motor(MOTOR_1,100);
    motor(MOTOR_2,80);
    sleep((float)seg);
    StopMotors();
}
 
//Acende o led da porta D2,D3,D4,D5 ou D4/D5
void LedOn(int led)
{
    switch(led)
      {       
        case 2: //Porta D2
          bit_set(0x1008, 0b00000100);
        break;
        case 3: //Porta D3
          bit_set(0x1008, 0b00001000);
        break;
        case 4: //Porta D4
          bit_set(0x1008, 0b00010000);
        break;
        case 5: //Porta D5
          bit_set(0x1008, 0b00100000);
        break;
        case 6: //Porta D4 e D5
          bit_set(0x1008, 0b00010000);
        bit_set(0x1008, 0b00100000);
        break;
        default:
          printf("Led invalido!\n");
        break;
 
    }
}
 
//Apaga o led da porta D2,D3,D4,D5 ou D4/D5
void LedOff(int led)
{
    switch(led)
      {        
        case 2: //Porta D2
          bit_clear(0x1008, 0b00000100);
        break;
        case 3: //Porta D3
          bit_clear(0x1008, 0b00001000);
        break;
        case 4: //Porta D4
          bit_clear(0x1008, 0b00010000);
        break;
        case 5: //Porta D5
          bit_clear(0x1008, 0b00100000);
        break;
        case 6: //Porta D4 e D5
          bit_clear(0x1008, 0b00010000);
        bit_clear(0x1008, 0b00100000);
        break;
        default:
          printf("Led invalido!\n");
        break;
    }
 
}
 
//Inverte o sentido da leitura de LDR, torna-a mais logica.
//Quanto maior o valor de light, maior o valor.
int light(int port)
{ 
    return 255 - analog(port); //Nao utilizar portas 0 e 1
}
 
void SalvaDados ()/* variavel apontador, int data[100]; na func principal*/
{
    //- Memoria Limitada da HB: coleta pouco mais de 2500 inteiros no vetor
    //- Lembrar de inicializar o vetor
    //- Para retirar os dados: Tools -> Upload Array -> Seleciona nome do vetor
    // e onde quer salvar , save to CSV (excell)
 
    int i;
    beep();
    sleep(3.0);
 
    for (i = 0; i< 100; i++)
      {
        data[i] = light(2);
        msleep(100L);
    }
    beep();
}
 
 
//Testa valores de LDR
//Mais iluminado, maior o valor.
void LDR_test(/**/)
{
    while(!stop_button()) {
        printf("LDR: %d\n", light(2)); 
        msleep(100L);
    }
}
 
//Gira para a esquerda 360 graus
void Gira360(float results[8])
{
    int i = 0;
    int oitavo = 0;
 
    float dist=0.0;
    printf("Gira 360!\n");    //Imprime Status
    reset_encoder(0);
    results[oitavo] = 0.0;
 
    for(i=0; i<20; i++)
      {
        results[oitavo] = results[oitavo] + (float)(light(2)-light(DIFF));
    }
    results[oitavo] = results[oitavo]/20.0;
 
    oitavo++;
    StartMotors(50,-50);      //Liga os motores e gira para a direita
 
 
    while(dist <=(BITOLA*PI*0.94))    //uma volta = 360 graus
      { 
        if(dist >=(float)oitavo*(BITOLA*PI*0.94)/8.0)
          { printf("captando %d\n", light(2)-light(DIFF));
            StopMotors();
            msleep(30L);
            results[oitavo] = 0.0;
            for(i=0; i<20; i++)
              {
                results[oitavo] = results[oitavo] + (float)(light(2)-light(DIFF));
            }
            results[oitavo] = results[oitavo]/20.0;
            StartMotors(50,-50);      //Liga os motores e gira para a direita
            oitavo++;
        }
        if(stop_button())
          { break;}
        dist = (float)read_encoder(0);
        dist = (dist*DX);       //Calcula a distancia percorrida
        printf("Dist.: %f mm\n",dist);
        msleep(100L);
    }
    StopMotors();             //Desliga os motores
    if(oitavo<8)
      {
        results[oitavo] = 0.0;
        for(i=0; i<20; i++)
          {
            results[oitavo] = results[oitavo] + (float)(light(2)-light(DIFF));
        }
        results[oitavo] = results[oitavo]/20.0;
    }
 
}
 
//Seta direcao onde o melhor valor da luz foi encontrado
void SetDirection(int iteracao)
{
    int i = 0;
    int oitavo = 0;
    float dist=0.0;
    printf("%d\n", iteracao);    //Imprime Status
    reset_encoder(0);
    //avalia em qual sentido corrigira(horario, anti-horario)
    if(iteracao <5)
      StartMotors(50,-50);      //Liga os motores e gira para a esquerda
    else
      {
        StartMotors(-50,50);
        iteracao = 8 - iteracao;
    }
 
 
    while(dist <=(float)(iteracao)*(BITOLA*PI*0.94)/8.0)    //uma volta = 360 graus
      { 
        if(stop_button())
          { break;}
        dist = (float)read_encoder(0);
        dist = (dist*DX);       //Calcula a distancia percorrida
        printf("Dist.: %f mm\n",dist);
        msleep(100L);
    }
    StopMotors();             //Desliga os motores
 
}
 
//corrige direcao, caso a setagem nao seja boa
void CorrectDirection(float results[8], int *correct, int *direction, int polo)
{
    float dist = 0.0;
    int i;
    switch(polo){
        case VERTICAL:
          if(results[0]<(float)(2*(light(2)-light(DIFF))))
          {*correct = 1;
            //define direcao a girar
            if(*direction==1)
              StartMotors(50,-50);      //Liga os motores e gira para a esquerda
            else
              {
                StartMotors(-50,50);
                *direction = 2;
            }
 
 
            while(dist <=(BITOLA*PI*0.94)/8.0)    //uma volta = 360 graus
              { 
                if(stop_button())
                  { break;}
                dist = (float)read_encoder(0);
                dist = (dist*DX);       //Calcula a distancia percorrida
                //      printf("Dist.: %f mm\n",dist);
                msleep(100L);
            }
            StopMotors();             //Desliga os motores
            results[1] = 0.0;
            for(i=0; i<5; i++)
              {
                results[1] = results[1] + (float)(light(2)-light(DIFF));
            }
            results[1] = results[1]/5.0;
            results[0] = results[1];
        }  
        break;
        case HORIZONTAL:
          if(results[0]>(float)(2*(light(2)-light(DIFF))))
          {*correct = 1;
            if(*direction==1)
              StartMotors(50,-50);      //Liga os motores e gira para a esquerda
            else
              {
                StartMotors(-50,50);
                *direction = 2;
            }
 
 
            while(dist <=(BITOLA*PI*0.94)/8.0)    //uma volta = 360 graus
              { 
                if(stop_button())
                  { break;}
                dist = (float)read_encoder(0);
                dist = (dist*DX);       //Calcula a distancia percorrida
                //      printf("Dist.: %f mm\n",dist);
                msleep(100L);
            }
            StopMotors();             //Desliga os motores
            results[1] = 0.0;
            for(i=0; i<5; i++)
              {
                results[1] = results[1] + (float)(light(2)-light(DIFF));
            }
            results[1] = results[1]/5.0;
            results[0] = results[1];
        }  
        break;
    }
}  
 
 
//Funcao controladora do sensor diferencial polarizado
void PolarizedDiffSensor(int polo)
{
    int iteracao=0;
    int encid1, encid2;
    int pid = 0, pid1 = 1;
    int direction = 0, correct = 0;
    int i = 0;
    float results[8], dist;
 
    //Gira e capta medias de valores   
    Gira360(results);
 
    //define qual luz vai ser localizada
    //parte do codigo esta comentado devido a um overflow
    switch(polo)
      {
        case VERTICAL:
          /*        for(i=1; i<8; i++)
          {
            if(results[0] > results[i]){
                iteracao = i;
                results[0] = results[i];
            }
          }
        if(iteracao>0)
          {
            SetDirection(iteracao);
   //     pid1 = start_process(LDR_test());            
         //   if(results[0]>(float)(light(2)-light(DIFF)))
 
              if(iteracao >=5)
                {
                  direction = 1;
              }
 
//            CorrectDirection(results, &correct, &direction, VERTICAL);
          }
        if(correct)
          {
            if(direction==2)
              {
                encid1 = 14;
                encid2 = 20;
                direction = 1;
              }
            else
              {
                encid1 = 20;
                encid2 = 14;
                direction = 2;
            }
          }
        else
          {
            encid1 = 20;
            encid2 = 20;
            printf("Reto!\n");
            sleep(2.0);
            results[1] = 0.0;
                for(i=0; i<20; i++)
                  {
                    results[1] = results[1] + (float)(light(2)-light(DIFF));
                }
                results[1] = results[1]/20.0;
            results[0] = results[1];
            direction = 2;
          }
 
        pid = start_process(PDControl(&encid1, &encid2, MOVE));
        pid1 = start_process(LDR_test());
 
        i=0;
        while(!stop_button()/* && (light(2) > 225)*) {
 
            if(i==3)
              {
                results[1] = 0.0;
                StopMotors();
                kill_process(pid);
                sleep(2.0);
                for(i=0; i<20; i++)
                  {
                    results[1] = results[1] + (float)(light(2)-light(DIFF));
                }
                results[1] = results[1]/20.0;
 
                if((results[0]*0.99)<=results[1])
                  {
                    if(direction==2)
                      {
 
                        encid1 = 14;
                        encid2 = 20;
                        pid = start_process(PDControl(&encid1, &encid2, MOVE));
                        direction = 1;                        
                    }
                    else
                      {
                        encid1 = 20;
                        encid2 = 14;
                        pid = start_process(PDControl(&encid1, &encid2, MOVE));
                        direction = 2;    
                      }
 
                  }
                else
                  {
                    pid = start_process(PDControl(&encid1, &encid2, MOVE));
                    encid1 = 20;
                    encid2 = 20;
                    if(direction==2) direction = 1;
                    else direction = 2;
                }
                results[0] = results[1];
                i=0;
              }
            sleep(1.0);
            i++;            
          }
        kill_process(pid1);
        kill_process(pid);
 
        StopMotors();
  */      
 
          break;
 
        case HORIZONTAL:
          //avalia melhor direcao
          for(i=1; i<8; i++)
          {
            if(results[0] < results[i]){
                iteracao = i;
                results[0] = results[i];
            }
        }
        if(iteracao>0)
          {
            SetDirection(iteracao);
            //     pid1 = start_process(LDR_test());            
            //   if(results[0]>(float)(light(2)-light(DIFF)))
 
            if(iteracao >=5)
              {
                direction = 1;
            }
 
            //            CorrectDirection(results, &correct, &direction, HORIZONTAL);
        }
        //faz correcao se necessario
        if(correct)
          {
            if(direction==2)
              {
                encid1 = 14;
                encid2 = 20;
                direction = 1;
            }
            else
              {
                encid1 = 20;
                encid2 = 14;
                direction = 2;
            }
        }
        //inicia movimento em linha reta (correcao no realizada)
        else
          {
            encid1 = 20;
            encid2 = 20;
            printf("Reto!\n");
            sleep(2.0);
            results[1] = 0.0;
            for(i=0; i<20; i++)
              {
                results[1] = results[1] + (float)(light(2)-light(DIFF));
            }
            results[1] = results[1]/20.0;
            results[0] = results[1];
            direction = 2;
        }
 
        pid = start_process(PDControl(&encid1, &encid2, MOVE));
        pid1 = start_process(LDR_test());
 
        i=0;
        while(!stop_button()/* && (light(2) > 225)*/) {
 
            //avalia se o sentido esta correto
            if(i==3)
              {
                results[1] = 0.0;
                StopMotors();
                kill_process(pid);
                sleep(2.0);
                for(i=0; i<20; i++)
                  {
                    results[1] = results[1] + (float)(light(2)-light(DIFF));
                }
                results[1] = results[1]/20.0;
                //avalia se o sinal captado, corresponde ao esperado, e corrige caso contrario.
                if((results[0]*0.99)>=results[1])
                  {
                    if(direction==2)
                      {
 
                        encid1 = 14;
                        encid2 = 20;
                        pid = start_process(PDControl(&encid1, &encid2, MOVE));
                        direction = 1;                        
                    }
                    else
                      {
                        encid1 = 20;
                        encid2 = 14;
                        pid = start_process(PDControl(&encid1, &encid2, MOVE));
                        direction = 2;    
                    }
 
                }
                else
                  {
                    pid = start_process(PDControl(&encid1, &encid2, MOVE));
                    encid1 = 20;
                    encid2 = 20;
                    if(direction==2) direction = 1;
                    else direction = 2;
                }
                results[0] = results[1];
                i=0;
            }
            sleep(1.0);
            i++;            
        }
        kill_process(pid1);
        kill_process(pid);
 
        StopMotors();
 
        break;
 
        default: printf("Polo inexistente!\n");
    }
 
    StopMotors();                                                //Desliga os motores    
}
 
//segue a linha atraves dos chaves opticas-reflexivas
void FollowLine()
{
    int pid=0, pid1=1;
    int encid1, encid2,lamb;
 
    encid1 = 20;
    encid2 = 20;
    lamb=light(2);
    //faz controle Proporcional Direcional em paralelo
    pid = start_process(PDControl(&encid1, &encid2, MOVE));
    pid1 = start_process(le_sensores());
    while(!stop_button()) {
 
        //caso um o sensor da direita esteja sobre a linha, corrige para direita
        if(!digital(10)) 
          correct_right_side(&encid1, &encid2);
        else if(!digital(11)) //oposto do de cima
          correct_left_side(&encid1, &encid2);
        //caso um bloco seja encontrado, para        
        if(light(2)<(int)(0.58*(float)lamb))
          break;
 
    }
    kill_process(pid);
    kill_process(pid1);
    StopMotors();
    //detecta cor dos blocos
    DetectColor();
}
//le os dados colhidos pelos sensores ir
void le_sensores(){  
    while(!stop_button()) {
        if(digital(10) && digital(11)) printf("IR: D-%d E-%d\n", digital(10), digital(11)); 
        msleep(100L);
    }
}
//corrige para direita
void correct_right_side(int *encid1, int *encid2)
{
    *encid1 = 16;
    *encid2 = 20;
    while(!digital(10)) printf("correct right\n");
    *encid1 = 20;
    *encid2 = 20;
 
}
//corrige para esquerda
void correct_left_side(int *encid1, int *encid2)
{
    *encid1 = 20;
    *encid2 = 16;
    while(!digital(11)) printf("correct left\n");
    *encid1 = 20;
    *encid2 = 20;
}
 
 
/****************************** hbmenu.c ********************************/
/* Menu functions which also allow variables to be set via the knob
   and selection buttons  
 
   Version 1.0 
   Written for MIT 6.270 contest by Anne Wright 11/14/1991 
 
   Version 2.0
   Converted for Handy Board for Botball by Anne Wright 1/13/2001
*/
 
/* abstractions for chosen_button */
#define NEITHER_B 0
#define START_B  1
#define STOP_B  2
 
/* abstractions for wait_button */
#define UP_B      3
#define DOWN_B    4
#define CYCLE_B   5
 
/*****************************button routines*************************/
/* Return minimum of two integers */
/* defined in cmucam3.ic which is loaded by this file -dpm 1/03
int min(int a,int b)
{
    if(a<b)
      return(a);
    else
      return(b);
}
*/
/* Return minimum of two floats */
float fmin(float a,float b)
{
    if(a<b)
      return(a);
    else
      return(b);
}
 
/* Returns which button is depressed using definitions above.  If
   both are pressed, start has precedence */
int chosen_button()
{
    if(start_button())
      return START_B;
    else if(stop_button())
      return STOP_B;
    else 
    return NEITHER_B;
}  
 
/* wait until button is depressed(DOWN_B), released(UP_B), or
   both(CYCLE_B) and return which button if any activated the
   sequence */
 
int wait_button(int i)
{
    if(i==DOWN_B){
        while(!(start_button() || stop_button()));
        return chosen_button();
    }
    else if (i==UP_B) {
        int b;
        b=chosen_button();
        while(start_button() || stop_button());
        return b;
    }
    else {
        int b;
        while(!(start_button() || stop_button()));
        b=chosen_button();
        while(start_button() || stop_button());
        return b;
    }
} 
 
/********************* Knob to Number routines*****************************/
 
/* Returns an integer value from min_val to max_val based on the current
   position of the knob */  
int knob_int_value(int min_val,int max_val)
{
    int val, coarseness=(255)/(max_val-min_val),selection;
 
    val=min((knob())/coarseness+min_val,max_val);
    return min(val,max_val);
}
 
/* Returns an float value from min_val to max_val based on the current
   position of the knob */  
float knob_float_value(float min_val,float max_val)
{
    float val, coarseness=(255.)/(max_val-min_val),selection;
    val=fmin(((float)knob())/coarseness+min_val,max_val);
    return fmin(val,max_val);
}
 
/******************** Menu selection routines ****************************/
 
/* While waiting for a button press, display the string passed in and
   the val, the integer value betwen min_val and max_val for the knob.
   If the button pressed is the start button, returns the final value
   of val.  If the button pressed is the stop button, returns -1.  */
int select_int_value(char s[],int min_val,int max_val)
{
    int val, button;
 
    printf("%s %d to %d\n",s,min_val, max_val);
    sleep(0.8);
 
    /* Wait until no button is pressed */
    wait_button(UP_B);
 
    /* While no button is pressed, display the string passed in and the
       current value of val */
    while((button = chosen_button())==NEITHER_B) {
        val=knob_int_value(min_val,max_val);
        printf("%s %d\n",s,val);
        msleep(100L);
    }
 
    /* Wait until no button is pressed */
    wait_button(UP_B);
 
    if(button==STOP_B)
      return(-1); /** -1 means stop pressed -- do not reset value **/
    else
      return(val); /* Stop not pressed, return val */
}
 
/* While waiting for a button press, display the string passed in and
   the val, the float value betwen min_val and max_val for the knob.
   If the button pressed is the start button, returns the final value
   of val.  If the button pressed is the stop button, returns -1.  */
float select_float_value(char s[],float min_val,float max_val)
{
    float val;
    int button;
 
    printf("%s %f to %f\n",s,min_val, max_val);
    sleep(0.8);
 
    /* Wait until no button is pressed */
    wait_button(UP_B);
 
    /* While no button is pressed, display the string passed in and the
       current value of val */
    while((button = chosen_button())==NEITHER_B) {
        val=knob_float_value(min_val,max_val);
        printf("%s %f\n",s,val);
        msleep(100L);
    }
 
    /* Wait until no button is pressed */
    wait_button(UP_B);
 
    if(button==STOP_B)
      return(-1.0); /** -1 means stop pressed -- do not reset value **/
    else
      return(val); /* Stop not pressed, return val */
}
 
/* While waiting for a button press, display the string from the array
   of strings passed in which corresponds to the current position of
   the knob (see select_int_value).  If the button pressed is the
   start button, returns the index of the string selected (0 to n-1).
   If the button pressed is the stop button, returns -1.  */
int select_string(char choices[][],int n)
{
    int selection,last_selection=-1,button;
 
    if(n>_array_size(choices))
      n=_array_size(choices);
 
 
    /* Wait until no button is pressed */
    wait_button(UP_B);
 
    /* While no button is pressed, display the string from the array
       of strings passed in which corresponds to the current position
       of the knob */
    while((button = chosen_button())==NEITHER_B) {
        selection=knob_int_value(0,n-1);
        if(selection!=last_selection) {
            printf("%s\n",choices[selection]);
            msleep(150L);
            last_selection=selection;
        }
    }
 
    /* Wait until no button is pressed */
    wait_button(UP_B);
 
    if(button==STOP_B)
      return(-1); /** -1 means stop pressed -- do not reset value **/
    else
      return(selection); /* Stop not pressed, return val */
}
 
/*
 * Local variables:
 *  comment-column: 40
 *  c-indent-level: 4
 *  c-basic-offset: 4
 * End:
 */