====== GRUPO 3 - Breakway Builders - TP 2 ======

===== Membros da Equipe =====

  * Douglas Martins 

  * Elerson Rubens

  * Rodrigo Maffra

  * Roger Melo

===== 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 =====
  - Caracterização do sensor
  - Influência da distância e cor do objeto
  - 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.

{{:cursos:introrobotica:2011-2:grupo03:coroas.jpg|}}

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.

{{:cursos:introrobotica:2011-2:grupo03:sensoresopticos.jpg|}}

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

{{:cursos:introrobotica:2011-2:grupo03:sensoresir.jpg|}}

Figura 3 - Sensores Infra-Vermelhos Opto-Reflexivos

{{:cursos:introrobotica:2011-2:grupo03:carrocompleto.jpg|}}

Figura 4 - Carro Completo

{{:cursos:introrobotica:2011-2:grupo03:reconhecendoblocos.jpg|}}

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

{{:cursos:introrobotica:2011-2:grupo03:g1.png|}}

Gráfico 1

{{:cursos:introrobotica:2011-2:grupo03:g3.png|}}

Gráfico 2

{{:cursos:introrobotica:2011-2:grupo03:g2.png|}}

Gráfico 3

{{:cursos:introrobotica:2011-2:grupo03:g4_1.png|}}

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 

{{:cursos:introrobotica:2011-2:grupo03:g5.png|}}

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

{{:cursos:introrobotica:2011-2:grupo03:g6.png|}}

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

{{:cursos:introrobotica:2011-2:grupo03:g7.png|}}

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

{{:cursos:introrobotica:2011-2:grupo03:medicoes_cor2.png|}}

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 ====

<code c>

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

</code>


==== Arquivo funcoes.ic ====

<code c>

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

</code>

==== Knob.ic ====

<code c>


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

</code>