GRUPO 3 - Breakway Builders - TP 2

• Douglas Martins

• Elerson Rubens

• Rodrigo Maffra

• Roger Melo

O objetivo deste trabalho é construir controles opticos para permitir a interação do robô com o ambiente. Além disso, será feito o aperfeiçoamento da montagem e controles cinéticos realizados no primeiro trabalho prático a fim de se diminuir pequenos problemas encontrados.

1. Caracterização do sensor
2. Influência da distância e cor do objeto
3. Tarefas para apresentação:
   • Localização de fonte de luz
   • Seguir linhas marcadas no campo
   • Identificar blocos
   • Construir um menu interativo para dar opções de execução das tarefas acima

Antes de iniciar a montagem dos sensores opticos realizamos uma mudança na relação das coroas para fazer o robô andar mais rápido. A troca aumentou em três vezes a velocidade do robô. Além disso criamos na parte superior do robô uma estrutura para comportar a hand board e travá-la para evitar que ela caia.

Figura 1 - Alteração nas relação das coroas

Após as alterações mecânicas, prosseguimos para as alterações necessárias para fazer o robô realizar as tarefas requeridas pelo trabalho prático 2. Essas altrações consistiram na adição de sensores opticos para fazer o robô interagir com o ambiente. Os itens opticos colocados no robô estão discrimiados a seguir:

• Sensores diferenciais: foram colocados dois sensores opticos(LDR) recobertos por uma camada de borracha isolante e polarizadores de luz para reconhecer e seguir fontes de luz polarizadas.
• Sensores infravermelhos optoreflexivos: foram colocados três sensores desse tipo para que o robô seja capaz de identificar e seguir linhas.
• Sensores para captar luz: foram instalados dois sensores para captura de luz: um LDR grande e outro pequeno, ambos para captar luz ambiente e luz proveniente do LED RGB para fazer o reconhecimento de blocos.
• LED RGB: foi instalado um LED emissor de luz vermelha, azul e verde para fazer reconhecimento de blocos.

Figura 2 - Sensor Diferencial, LDR PEQUENO e LED RGB, LDR GRANDE

Figura 3 - Sensores Infra-Vermelhos Opto-Reflexivos

Figura 4 - Carro Completo

Figura 5 - Carro Reconhecendo Blocos

Foram realizados diversos testes com os sensores opticos para leitura das luzes emitidas pelo LED RGB.

O priemiro teste, conforme pedido, consistiu em fazer medições da luz emitida pelo LED RGB e refletida por um bloco de isopor(vermelho) em diversas situações:

• com luz ambiente com o motor desligado(Gráfico1).
• tampado por uma caixa de isopor e com o motor desligado(Gráfico2).
• com luz ambiente com o motor ligado(Gráfico3).
• tampado por uma caixa de isopor e com o motor ligado(Gráfico4).

Gráfico 1

Gráfico 2

Gráfico 3

Gráfico 4

A partir dos testes podemos observar que o ambiente influência na medida média captada pelo sensor LDR. Além disso podemos notar que o motor ligado também perturba o valor médio das medidas captadas pelo pelo sensor LDR e aumenta o desvio padrão dos valores registrados.

Com esses resultados percebemos que as medições para identificação de cores deveria ser feitas eliminando a luz ambiente e sempre com os motores desligados.

Manter os motores desligados foi uma tarefa simples, porém eliminar o ambiente exigiu uma técnica mais robusta que consiste em medir a luz ambiente com o LED RGB desligado e em seguida repetir a leitura mas com o LED RGB(com a cor branca) ligado. A diferença entre as medições será significativa somente quando tiver um objeto perto. Dessa forma conseguimos eliminar o ambiente e determinar com eficiência quando existe um objeto a frente do robô.

Outro teste realizado com os sensores LDR e o LED RGB foi a medição de luz para um determinado bloco em diferentes distâncias proporcionais. (Gráficos 5,6 e 7)

Medimos também a reflexão de luz de cada cor do LED RGB para cada bloco. Fizemos cada medição diversas vezes sempre com a menor distância possivel do braço inferior do robô ao bloco e calculamos todas as médias. A tabela 1 apresenta os valores médios medidos.

A tabela 1

Gráfico 5 ( um bloco de distância )

Gráfico 6 ( dois blocos de distância )

Gráfico 7 ( três blocos de distância )

Tabela 1

Pelos gráficos apresentados notamos que a distância do bloco influencia nas medições, porém esses valores são bem proporcionais. A partir dessa constatação e da tabela 1 que contem a identificação de cada cor de bloco para todas as cores do LED RGB fizemos uma função que identifica a cor de um bloco a partir da medição das sete cores do LED RGB. A função basicamente verifica qual item da tabela é “mais parecido” com as medidas feitas. Esse “mais parecido” refere-se a razão do valor medido pelo valor existente na tabela. Os valores dessa razão que apresentarem o menor desvio padrão são tido com a cor do objeto.

Para a apresentação, implementamos um programa para fazer todos as tarefas descritas para a apresentação. Esse programa apresenta um pequeno menu que possibilita ao usuário a escolha da tarefa e de todos os parâmetros requeridos por essa tarefa.

Para seguir linha foi implementado uma maquina de estados que detecta a posição do robô e o faz girar suavemente para a direção desejada.(Para o robô ficar mais preciso e para esse conseguir detectar mais estados de posição foram adicionados três sensores opto reflexivos.)

Foi possivel verificar que os sensores montados para detectar cor são muito sensiveis ao ambiente, assim, para implementar um sensor independente do ambiente, a luz colocada na frente do robô é acesa e apagada intermitentemente sendo capturados os valores da diferenças desses estados.Com isso feito, para detectar um objeto aproximando foi definido um limite para o valor lido, de forma que um valor menor que esse limite faça o robô parar.Para detectar cor foram realizadas algumas medidas para os blocos coloridos combinados para as cores do led.Com esses valores e uma medição é feita uma proporção para a cor detectada é aquela que possui o menor desvio padrão.

Para seguir a luz polarizada o robo gira até encontra um limite minimo para a luz encontrada, passado esse limite ele continua girando até pegar o valor que está sendo lido começar a aumentar. Quando o valor se estabiliza o robô segue nessa direção. Se por algum motivo o valor da função alterar de forma incorreta (ou seja, ele começar a desviar levemente da fonte de luz) o robô torna a girar ate encontrar uma nova direção otima. Esse processo repete-se indefinidamente.

Na execução do trabalho tivemos alguns problemas relacionados a medições erradas e a influencia do ambiente nos sensores, esses problemas foram superados com relativo sucesso após utilizarmos técnicas que elimnam a luz ambiente. Assim,todas as tarefas que foram proposta no trabalho foram realizadas com sucesso.

A apresentação do dia 03/10 foi satisfatoria onde todas as tarefas propostas puderam ser apresentadas sem problemas significativos.

Seguir linha:

http://www.youtube.com/watch?v=Hk6Kqx-b6yU

Seguir luz polarizada:

http://www.youtube.com/watch?v=x-mP7e5WTEI

Ação ao encontra um bloco amarelo:

http://www.youtube.com/watch?v=JJqH--5Ogk8

Ação ao encontrar um bloco vermelho:

http://www.youtube.com/watch?v=1yk-nSCh4mU

#include sencdr5.icb
#include sencdr6.icb
#use "knob.ic"
#include "funcoes.ic"
 
#define QSLEEP 30L
 
// constantes para o menu
char m0[4][20] = {"Menu: Seguir Luz ","Menu: Seguir linha ","Menu: Calibrar","Menu: Sair "};
char m1[7][30] = {"Acao: Preto", "Acao: Vermelho", "Acao: Azul", "Acao: Amarelo","Acao: Verde","Tempo Arraste", "Acao: INICAR PROGRAMA" };
char m2[4][30] = {"[ ] Desviar Esq.  ", "[ ] Desviar Dir.  ", "[ ] Empurrar", "[ ] Parar"};
char m3[2][30] = {"Seguir Luz - Vertical","Seguir Luz - Horizontal"};
char m4[2][30] = {"Calibrar Cor","Calibrar Presenca"};
 
// tempo de arraste
int tempoArraste = 2;
 
// define sensibilidade padrao
persistent int sensibilidade = 15;
 
// define o vetor de acoes para as cores de blocos
char vetorAcao[5];
 
int main(){            
 
    // define acoes padrao    
    vetorAcao[0] = 2; // preto = empurrar
    vetorAcao[1] = 0; // vermelho = contornar a esquerda
    vetorAcao[2] = 2; // azul = empurrar
    vetorAcao[3] = 1; // amarelo = contornar a direita
    vetorAcao[4] = 3; // verde = parar    
 
    // estado incial do programa
    printf("Press START\n");
    while(!start_button());
 
    // roda menu principal
    while(1) {
        int sel;
        printf("Rode o knob p. selec.\n");
        msleep(2000L);
 
        // seleciona uma opcao
        sel = select_string(m0,4);
 
        // opcao para seguir luz
        if(sel==0){            
            seguirLuz(select_string(m3,2));
        }
        // opcao para seguir reta e reconhecer blocos
        else if(sel == 1){
            selecionarAcao(vetorAcao);
            seguirReto(vetorAcao, sensibilidade, tempoArraste ); 
        }
        // opcao para calibrar robo
        else if(sel == 2 ) 
          if(select_string(m4,2) == 0)
            calibrar();
          else
            sensibilidadePresenca();
        // opcao para terminar o programa
        else if(sel == 3 )break;
    }
 
    printf("Terminou\n");
}
 
// selecina as acoes para cada cor de bloco
void selecionarAcao( char vetorAcao[] ){
    int sel,sel2,i;
 
    while(1){
        sel = select_string(m1,7);
 
        if( sel == 6 ) 
          break;
 
        if( sel == 5 ){
            setarTempoArraste();
            msleep(100L);
        }        
 
        // escolhe acao 
        if( sel != 6 && sel != 5 ){
 
            // marca a opcao ja selecionada com X
            for( i=0; i < 4; i++ ){
                m2[i][1] = ' ';          
            }            
            m2[vetorAcao[sel]][1] = 'X';            
            sel2 = select_string(m2,4);                
            vetorAcao[sel] = sel2;            
        }        
    }
}
 
// possibilita setar a sensibilidade de presenca
void sensibilidadePresenca()
{
    int k;
    while(!start_button())
      {
        k = knob();
        printf("valor distancia: %d - PRESS START TERMINAR\n",k);
        msleep(100L);
    }
    sensibilidade = k;
 
}
 
// possibilita setar o tempo de arraste de um bloco
void setarTempoArraste()
{
    int k;
    while(!start_button())
      {
        k = knob()/25;
        printf("valor distancia: %d - PRESS START TERMINAR\n",k);
        msleep(100L);
    }
    tempoArraste = k;    
}

/* 
ESQUERDA = par
DIREITA  = impar
 
ANALOG 2 = SENSOR DE LUZ DIFERENCIAL
ANALOG 3 = LDR PEQUENO
ANALOG 4 = LDR GRANDE
ANALOG 5 = ENCODER DIREITA
ANALOG 6 = ENCODER ESQUERDA
 
DIGITAL 11 = SENSOR OPTICO CENTRAL
DIGITAL 14 = SENSOR OPTICO ESQUERDA
DIGITAL 15 = SENSOR OPTICO DIREITA
 
*/
 
 
#include sencdr5.icb
#include sencdr6.icb
 
//constantes PD
#define constanteP 0.4
#define constanteD 0.1
 
//define constantes
#define NLUZ 0
#define TEMPOMEDICAO 120L
#define DIFERENCADESEJADA 20
 
//define quanto cada valor de sensor sera lido para ser considerado valido 
#define NMEDIAMEDICAO 5
 
// realiza a media das medicoes de luz
int VALORMEDICAO = 0;
int diferencaMedicao = 0;
int diferencaMedia = 0;
int mediaLuz = 0;
int PORTALEITURALUZ = 4;
 
// vetores identidade para identificacao de cores
float vCores[35] = {
    21.397,1.511,6.886,17.357,8.992,18.495,21.944, //preto      
      29.868,16.800,7.373,16.451,20.409,26.571,20.120, // vermelho
      52.419,2.938,17.944,49.441,20.164,50.136,52.467, // azul
      31.463,14.679,19.683,16.695,25.978,24.036,26.820, // amarelo
      41.000,4.000,32.00,25.000,33.000,27.000,40.000 // verde  
};
 
// vetor que contera a medicao do bloco
float vetorMedicao[7];
 
//valor para luz = branco
int LUZ = 28;
 
// funcao para seguir reto e desviar de blocos
int seguirReto( char vetorAcao[], int sensibilidade, int tempoArraste )
{ 
    // declara variaveis
    float power0 = 0.0,power1 = 0.0; // potencia dos motores
    int estado = 0; // estado da maquina de estados
    int vel0 = 7; // velocidade da roda 0
    int vel1 = 7; // velocidade da roda 0
    int sensorE = 0; // leitura dos sensores
    int sensorD = 0; // leitura dos sensores
    int sensorC = 0; // leitura dos sensores
    int cor = -1; // identificacao da cor
    int i,j;
    int D = 0, F = 1; // constante para D = dentro e F = fora (da linha)
    int idThread; // guarda id da thread para mata-la
 
    poke(0x1009, 0x3c);
    LUZ = 28;
 
    // inicia thead para medir e elimniar o meio ambiente
    idThread = start_process(medeAmbiente());
 
    // excuta leitura inicial
    sensorE = digital(14);
    sensorD = digital(15);  
    sensorC = digital(11);  
 
    // define estado inicial
    estado = 0;   
    while(1){
        // parada dos motores
        if(stop_button()){
            alloff();
            break;
        }
 
        // le sensores
        sensorE = digital(14);
        sensorD = digital(15);  
        sensorC = digital(11);  
 
        // vefica estado na maquina de estados
        estado = verificaEstado( sensorE, sensorD, sensorC, estado );
 
        // verifica se existe objeto no caminho e 
        // se houver muda para estado de reconhecimento
        if( diferencaMedia > sensibilidade )
          estado = -2;
 
        // segue reto 
        if( estado == 2 ){    
            power0 = andar(vel0,1,0,power0);          
            power1 = andar(vel1,1,1,power1);   
        }
        // segue direita rapido
        else if( estado == -1  ){
            power0 = andar(vel0+3,1,0,power0);          
            power1 = andar(vel1-3,1,1,power1);   
        }
        // segue esquerda rapido
        else if( estado == -1 ){
            power0 = andar(vel0-3,1,0,power0);          
            power1 = andar(vel1+3,1,1,power1);   
        }
        // segue direita devagar
        else if( estado == 0  ){
            power0 = andar(vel0+1,1,0,power0);          
            power1 = andar(vel1,1,1,power1);   
        }
        // segue esquerda devagar
        else if( estado == 1 ){
            power0 = andar(vel0,1,0,power0);          
            power1 = andar(vel1+1,1,1,power1);   
        }   
        // estado de reconhecimento de blocos
        else if( estado == -2 ){
 
            // analisa cor
            printf("Analisando... \n", cor);
            cor = medirCor();            
 
            // imprime a cor identificada
            printf("Cor: ");
            if( cor == 0 )printf("Preto\n");
            else if( cor == 1 )printf("Vermelho\n");
            else if( cor == 2 )printf("Azul\n");
            else if( cor == 3 )printf("Amarelo\n");
            else if( cor == 4 )printf("Verde\n");
 
            msleep(2000L);
 
            // contornar a esquerda(0) ou direita(1)
            if( vetorAcao[cor] == 0 || vetorAcao[cor] == 1){
                int count = 0;    
                int parar = 0;
                int condicaoParar = 0;
                int aux_encoder_counts = (encoder5_counts+encoder6_counts)/2;         
                int direcao;
 
                // define direcao 
                if( vetorAcao[cor] == 0 ) direcao = 1;
                else direcao = -1;
 
                // retorna um pouco para desviar do bloco:
                power0 = 0.0;
                power0 = 1.0;
                count = 0;
                while(count < 70 ){
                    power0 = andar(-vel0,1,0,power0);          
                    power1 = andar(-vel1,1,1,power1);   
                    count += (encoder5_counts+encoder6_counts)/2 - aux_encoder_counts;
                    aux_encoder_counts = (encoder5_counts+encoder6_counts)/2;
                    printf("Count = %d\n", count);
                    msleep(10L);
                }               
                alloff();                
                msleep(1000L);
 
                // gira para esquerda(1) ou direita(-1):
                count = 0;
                power0 = 0.0;
                power1 = 0.0;
                aux_encoder_counts = (encoder5_counts+encoder6_counts)/2;
                while(count < 50 ){
                    power0 = andar( -direcao*vel0,1,0,power0);          
                    power1 = andar( direcao*vel1,1,1,power1);   
                    count += (encoder5_counts+encoder6_counts)/2 - aux_encoder_counts;
                    aux_encoder_counts = (encoder5_counts+encoder6_counts)/2;
                    msleep(10L);
                }
                alloff();
 
                msleep(1000L);
 
                // anda em circulo desviando do bloco:
                parar = 0;
                condicaoParar = 0;
                // circulo pela esquerda
                if( direcao == 1){
                    vel0 = 10;
                    vel1 = 6;
                }
                // circulo pela direita
                else{
                    vel0 = 6;
                    vel1 = 10;
                }
 
                power0 = 0.0;
                power1 = 0.0;                
                count = 0;
                aux_encoder_counts = (encoder5_counts+encoder6_counts)/2;
                while(!parar){
                    sensorE = digital(14);
                    sensorD = digital(15);  
                    sensorC = digital(11);          
                    count += (encoder5_counts+encoder6_counts)/2 - aux_encoder_counts;
                    aux_encoder_counts = (encoder5_counts+encoder6_counts)/2;
                    printf("Count = %d\n", count);
                    msleep(10L);
 
                    printf("%d %d %d\n", sensorE, sensorD, sensorC );
 
                    if( sensorE = F && sensorD == F && sensorC == F && count > 40 )
                      condicaoParar = 1;
 
                    if( condicaoParar == 1 && ( sensorE == D && sensorD == D ) ){
                        parar = 1;
                    }
 
                    power0 = andar( vel0,1,0,power0);          
                    power1 = andar( vel1,1,1,power1);                
                }                                
                vel0 = 7;
                vel1 = 7;
 
                msleep(1000L);
                alloff();
 
                // gira para esquerda ou direita para retomar a linha
                count = 0;
                power0 = 0.0;
                power1 = 0.0;
                aux_encoder_counts = (encoder5_counts+encoder6_counts)/2;
                while(count < 60 ){
                    power0 = andar(-direcao*vel0,1,0,power0);          
                    power1 = andar(direcao*vel1,1,1,power1);   
                    count += (encoder5_counts+encoder6_counts)/2 - aux_encoder_counts;
                    aux_encoder_counts = (encoder5_counts+encoder6_counts)/2;
                    msleep(10L);
                }
                alloff();
 
                // retorna estado inicial
                estado = 0;
 
                // imprime acao
                if( direcao == 1 )printf("Contornou esq.! Pressione START!\n");                                
                else printf("Contornou dir.! Pressione START!\n");                                
 
            }
            // empurrar bloco
            else if( vetorAcao[cor] == 2 ){
 
                printf("Empurrou! Pressione START!\n");                                
                for( i = 0; i < tempoArraste*100; i++ ){
                    power0 = andar(vel0,1,0,power0);          
                    power1 = andar(vel1,1,1,power1);   
                    msleep(6L);
                }
                alloff();
            }
            // parar
            else if( vetorAcao[cor] == 3 ){
                printf("Parou! Pressione START!\n");
            }
 
            // aguarda com resposta ate apertar start
            while(!start_button());
            break;
        }
        msleep(2L);
    }
 
    // mata processo que executa eliminacao do ambiente
    kill_process( idThread );
}
 
 
void seguirLuz( int polarizacao )
{   
    // declara variaveis
    float power0 = 0.0,power1 = 0.0; // potencia dos motores
    int vel0 = 7; // velocidade de linha reta
    int vel1 = 7; // velocidade de linha reta
    int velRot0 = 2; // velocidade de rotacao
    int velRot1 = 2; // velocidade de rotacao
    int sensorLuz = 0; // valor do sensor diferencial
    int sensorLuzAntigo = 0; // valor antigo do diferencial
    int diferenca = 0; // direnca calculada
    int maiorValor = -255;
    int flagDiminuiu = 0;
    int i,j;
    int count = 5;    
    int aux_encoder5_counts = encoder5_counts;
    int direcao = 0;
    int idT;
    alloff();
 
    // inicia thread para medir luz polarizada
    idT = start_process(medeLuzPolarizada());    
    msleep(1000L);
 
    // executa circulo ou reto dependendo da direcao
    while(1){        
        // para programa
        if( stop_button() )break;
 
        // busca valor medido dependendo do tipo de polarizacao
        if( polarizacao == 1 )sensorLuz = 138-mediaLuz-(analog(4)/2);
        else sensorLuz = mediaLuz-160-(analog(4)/2);
 
        printf("%d\n", sensorLuz);
 
        sensorLuzAntigo = 0;
 
        // roda ate encontrar menor medicao
        if( sensorLuzAntigo > sensorLuz ){
 
            flagDiminuiu = 0;
            sensorLuzAntigo = sensorLuz;
            msleep(200L);            
            while(1){
                power0 = andar(velRot0,-1,0,power0);          
                power1 = andar(velRot1,1,1,power1);   
 
                if( polarizacao == 1 )sensorLuz = 138-mediaLuz-(analog(4)/2);
                else sensorLuz = mediaLuz-160-(analog(4)/2);
 
                //se for negativo -> aumenta
                //se for positivo -> diminui
                if( sensorLuz > 0 ){
                    diferenca = sensorLuzAntigo - sensorLuz;
 
                    if( diferenca < 0 )
                      flagDiminuiu = 1;
 
                    if( diferenca < 0 && flagDiminuiu == 1 )
                      {
                        break;                        
                    }
                }
 
                sensorLuzAntigo = sensorLuz;
 
                msleep(2L);            
 
                printf("%d\n", sensorLuz );
 
 
            }                            
            power0 = 25.0;
            power1 = 25.0;            
        }
        sensorLuzAntigo = sensorLuz;        
 
        //segue reto pois encontrou fonte de luz
        power0 = andar(vel0,1,0,power0);          
        power1 = andar(vel1,1,1,power1);           
        msleep(100L);            
    }    
 
    // mata processo que le luz polarizada
    kill_process(idT);
}
 
// funcao para fazer o robo andar utilizando controle PD
float andar( int velocidade, int direcao, int nMotor, float power )
{
    // direcao = 1 -> andar para frente
    // direcao = -1 -> andar para tras
 
    if( nMotor == 0 ) 
      power -= (float)direcao * controleMotor(encoder6_velocity,velocidade);
    else
      power -= (float)direcao * controleMotor(encoder5_velocity,velocidade); 
 
    // limita valor devido a biblioteca nova
    if(power >50.0) power = 50.0;
    if(power < -50.0) power = -50.0;
 
    motor(nMotor, (int)power );
 
    return power;
}
 
// para os motores
void parar( float *power0, float *power1 )
{
    *power0 = 0.0;
    *power1 = 0.0;
}
 
//faz o controle PD dos motores utilizando a velociadade
float controleMotor(int velocidadeMotor,int velocidadeObjMotor)
{
 
    float power;
 
    //faz om que motor mantenha um certa velocidade
    if(velocidadeMotor != velocidadeObjMotor)
      {
        power = constanteP*((float)(velocidadeMotor - velocidadeObjMotor)) ;            
 
    }    
    return power;
}
 
// verifica estado 
int verificaEstado( int sensorE, int sensorD, int sensorC, int estado)
{   
    int D = 0; // dentro da faixa
    int F = 1; // fora da faixa
 
    // se nao esta reconhecendo blocos, verifica maquina de estados
    if( estado != - 2 ){
        if( sensorE == F && sensorD == D && sensorC == F ){
            estado = 0;
        }
        else if( sensorE == F && sensorD == D && sensorC == D ){
            estado = 0;
        }
        else if( sensorE == D && sensorD == F && sensorC == F ){
            estado = 1;
        }
        else if( sensorE == D && sensorD == F && sensorC == D ){
            estado = 1;
        }
        else if( sensorE == D && sensorD == D && sensorC == F ){
            estado = 2;
        }
        else if( sensorE == F && sensorD == F && sensorC == D ){
            estado = 2;
        }
        else if( sensorE == D && sensorD == D && sensorC == D ){
            estado = 2;
        }
    }    
    return estado;
}
 
// executa em uma thread para eliminar o ambiente
void medeAmbiente()
{   
    int Media[NMEDIAMEDICAO];
    int ligadoDesligado = 1; //
    int medicaoAnterior = analog(PORTALEITURALUZ);
 
    int i,j;
    int auxdiferencaMedia;
    // inicializa media
    for(j=0;j<NMEDIAMEDICAO;j++)
      {
        Media[j] = 0;   
    }
 
    while(1){        
        // liga e desliga a luz
        if(ligadoDesligado == 1)
          poke(0x1008, LUZ);
        else
          poke(0x1008, NLUZ);
 
        msleep(TEMPOMEDICAO);
        VALORMEDICAO = analog(PORTALEITURALUZ);
 
        diferencaMedicao = medicaoAnterior - VALORMEDICAO; 
        medicaoAnterior = VALORMEDICAO;
 
        // faz a media da diferena de medicao  
        if( i%NMEDIAMEDICAO == (NMEDIAMEDICAO-1) )
          {
            i = 0;
        }
 
        if(diferencaMedicao>0)
          Media[i] = diferencaMedicao;
        else
          Media[i] = -diferencaMedicao;
 
        auxdiferencaMedia =0;
        for(j=0;j<NMEDIAMEDICAO;j++)
          {
            auxdiferencaMedia += Media[j];
        }
        diferencaMedia = auxdiferencaMedia/NMEDIAMEDICAO;
 
 
        i++;
 
        ligadoDesligado *= -1;
    }
 
}
 
// funcao que identifica a cor do objeto
int medirCor()
{
    int i,j;
    int prodEscalar = 0;
    int normaX = 0, normaY = 0;
    float maxValor = 0.0, resultado = 0.0 ;
    int indiceMaior = -1;
 
    float proporcao[7];
    float media = 0.0;
    float desvioPadrao = 0.0;
    int indiceMenor = -1;
    float valorMenor = 10000.0;
 
    alloff();            
    PORTALEITURALUZ = 4;
    msleep(2000L);
 
    for( i = 0; i < 7; i++ ){
 
        if( i == 0 ) LUZ = 28;
        else if( i == 1 )LUZ = 4;
        else if( i == 2 )LUZ = 8;
        else if( i == 3 )LUZ = 16;
        else if( i == 4 )LUZ = 12;
        else if( i == 5 )LUZ = 20;
        else if( i == 6 )LUZ = 24;
        msleep(2500L);
        vetorMedicao[i] = (float)diferencaMedia;;
    }
 
    PORTALEITURALUZ = 4;
    LUZ = 0;
 
    for( j=0; j < 5; j++ ){
        // calcula a proporcao
        for( i =0; i < 7; i++ ){
            if(vCores[j*7+i] != 0.0)
              proporcao[i] = vetorMedicao[i] / vCores[j*7+i];
            else
              proporcao[i] = 0.0;
 
        }
 
        // calcula a media
        media = 0.0;
        for( i =0; i < 7; i++ ){
            media += proporcao[i];
        }
        media /= 7.0;
 
        // calcala a desvio padrao
        desvioPadrao = 0.0;
        for( i =0; i < 7; i++ ){
            desvioPadrao += (proporcao[i]-media)*(proporcao[i]-media);
        }
 
        desvioPadrao = desvioPadrao/7.0;
        desvioPadrao = sqrt(desvioPadrao);
 
 
        if( valorMenor > desvioPadrao ){
            valorMenor = desvioPadrao;
            indiceMenor = j;
        }
    }
 
    LUZ = 0;   
 
    return indiceMenor;
}
 
// funcao para medir luz polarizada, e executada em uma thread
void medeLuzPolarizada(){
    int vetorMedia[NMEDIAMEDICAO];
    int i;
    int iCircular = 0;
    int soma = 0;
 
    // calcula a media inicial
    for(i = 0; i < NMEDIAMEDICAO; i++ ){
        vetorMedia[i] = analog(2);
    }
 
    while(1){
 
        // le sensor        
        vetorMedia[iCircular] = analog(2);
        iCircular++;
        iCircular = iCircular % NMEDIAMEDICAO ;
 
        // calcula nova media
        soma = 0;
        for(i = 0; i < NMEDIAMEDICAO; i++ ){
            soma += vetorMedia[i];
        }
        soma /= NMEDIAMEDICAO;                                
 
        mediaLuz = soma;
    }   
}
 
// funcao para calibrar cores de blocos, caso se queira
void calibrar()
{
    char  cores[5][10] = {"preto","vermelho","azul","amarelo","verde"}; 
    int i,j,idThread;
    int k;
    poke(0x1009, 0x3c);
    LUZ = 28;
    PORTALEITURALUZ = 3;
 
    // inicia thead para medir meio ambiente
    idThread = start_process(medeAmbiente());
    for(i =0 ; i < 5 ; i++)
      {
        printf("calibrar Cor: %s PRESS START\n",cores[i]);
        while(!start_button());
        printf("calibrando... \n");
 
        for( j = 0; j < 7; j++ ){
 
            if( j == 0 ) LUZ = 28;
            else if( j == 1 )LUZ = 4;
            else if( j == 2 )LUZ = 8;
            else if( j == 3 )LUZ = 16;
            else if( j == 4 )LUZ = 12;
            else if( j == 5 )LUZ = 20;
            else if( j == 6 )LUZ = 24;
 
            msleep(2000L);
            vCores[i*7 + j] = (float)diferencaMedia;
 
        }
 
        printf("%s:",cores[i]);
        for(k=0;k< 7;k++)
          printf("%d ",(int)vCores[i*7 + k]);
        while(!start_button());
        printf("\n");
    }
    PORTALEITURALUZ = 4; 
    LUZ = 28;
    kill_process( idThread );    
}

 
/****************************** 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(100L);
            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:
 */