Tabela de conteúdos

GRUPO 3 - Breakway Builders - TP 2

Membros da Equipe

Introdução

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.

Tarefas a serem realizadas

  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

Montagem e Aperfeiçoamento do Robô

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:

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

Caracterização do sensor

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:

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ô.

Influência da distância e cor do objeto

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.

Tarefas para apresentação

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.

Tarefa seguir linha

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.)

Tarefa detectar objeto e detectar cor

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.

Tarefa seguir luz polarizada

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.

Conclusão

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.

Apresentação

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

Vídeos

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

Codigo Fonte do Programa

Arquivo main.ic

#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;    
}

Arquivo funcoes.ic

/* 
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 );    
}

Knob.ic

 
/****************************** 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:
 */