SAS - Seek and Shoot

O segundo trabalho prático requeria a construção de um robô móvel com um sistema de controle cinemático implementado e deveria ser capaz de se localizar em um campo com duas fontes de luz polarizadas inversamente, seguir linha e identificar cores de blocos, aplicando também uma ação para cada cor reconhecida. Para sua construção foi adquirido, além dos sensores já utilizados, diversos outros sensores e componentes eletrônicos necessários para a confecção desses (resistências, fios, termo-retrátil, etc). Dentre os sensores, podemos citar a confecção de duas chaves opto-reflexivas, um LED RGB, um sensor foto-diodo, um sensor break-beam (utilizando um LED azul e um LDR) e um sensor diferencial para detecção de luz polarizada. As tarefas foram ordenadas da seguinte maneira:

• Caracterização do sensor óptico ativo (LED RGB + Foto diodo)
• Influência da distância e cor do objeto
• Tarefas para a apresentação:

• Localização de fontes de luz polarizadas
• Seguir linhas marcadas no campo
• Identificar blocos por suas cores
• Construir um menu interativo para dar opções de execução das tarefas acima

Como objetivo futuro deixado pelo trabalho anterior, pretendia-se construir uma estrutura que fosse mais firme e tivesse uma velocidade razoável. Percebeu-se que era impossível melhorar esses dois aspectos de maneira significante sem alterar grande parte do modelo estrutural.

A figura acima mostra a diferença na disposição dos motores, reduções de engrenagens utilizadas e shaft encoders. Para melhorar a velocidade foi preciso montar os motores de forma que as engrenagens estivessem ligadas diretamente aos seus eixos, ao invés da montagem anterior que utilizava pinhões. Com isso obteve-se um resultado impressionante quanto ao aumento da velocidade, porém a estabilidade do robô ficou totalmente comprometida. Era necessário uma maior resolução dos pulsos gerados pelos shaft encoders, para que assim pudesse desenvolver um controle PD compatível com a velocidade adquirida. Por isso, ligamos uma engrenagem no eixo do motor a uma engrenagem no eixo do shaft encoder e essa redução fez com que o número de pulsos gerados fosse multiplicado.

Mesmo assim o número de pulsos gerados não era suficiente para o desenovolvimento de um controle PD satisfatório. Testes revelaram que cada centímetro percorrido pelo robô correspondia a aproximadamente 3,42 pulsos do shaft encoder. Para o próximo trabalho, pretende-se, primeiramente, através de pequenas alterações na estrutura gerar o número adequado de pulsos que proporcione um controle PD mais preciso. Podemos afirmar sem sombra de dúvidas que o fator que mais prejudicou o trabalho foi a falta de precisão no controle cinemático abordado acima, afinal todas as outras tarefas dependiam de uma certa estabilidade no deslocamento.

Para a realização da tarefa de seguir linha, construímos duas chave opto-reflexivas que foram posicionadas próximas ao eixo das rodas. Esses dois sensores foram utilizados para determinar a posição do robô em relação à linha do campo a ser seguida. Dessa maneira, fica possível reconhecer se o robô desviou-se da linha pela direita ou pela a esquerda. Essa montagem apresentou desempenho questionável, pois a movimentação do robô já passava a ser realizada em formato ‘S’ após desviar-se a primeira vez da linha. Se os sensores fossem colocados na parte frontal do robô, poderia-se detectar o desvio da linha assim que seu alinhamento à linha fosse comprometido.

A identificação de cor dos blocos foi feita utilizando a combinação de um LED RGB e um foto-diodo. O LED RGB foi escolhido por economizar muito espaço, pois utilizamos um LED apenas ao invés de utilizar 3 LEDs coloridos. Para a medição da luz optou-se por utilizar o foto-diodo, pois este apresentava uma eficiência melhor para o espectro de luz visível em comparação com sensores LDR comuns. Para focar melhor a emissão e recepção da luz e portanto obter medidas mais independentes às interferências externas, utilizamos termo-retrátil.

Para identificar a presença de blocos foi montado um sensor break-beam. Inicialmente foi experimentado a utilização de um foto-diodo e um led infravermelho. No entanto, pudemos evidenciar em testes que o foto-diodo não era sensível o suficiente para detectar as cores de todos os LEDs que tinhamos disponíveis (azul, verde, vermelho e infra-vermelho). Após algumas tentativas, optou-se por utilizar um LED de cor azul e um CdS, de forma que quando o feixe é interrompido é considerado que existe um bloco entre os braços que compõem a parte dianteira do robô.

A localização do robô no campo de competição do laboratório é feita utilizando um sensor diferencial de luz polarizada, composto por dois senosres LDR (CdS). Este sensor diferencial foi reaproveitado do repositório de peças extras disponível. Os sensores LDR foram montados com um tubo de borracha e um filtro polarizador com o objetivo de obter uma melhor resposta em relação às fontes de luz.

Para fazer o controle do robô estabelecemos como objetivo a velocidade das rodas. Caso, a velocidade de uma das rodas estivesse acima ou abaixo do esperado, a potência do seu motor correspondente era diminuída ou aumentada respectivamente. O termo derivativo do controle PD se refere à aceleração do robô. As constantes proporcional e derivativa utilizadas são 0.5 e -0.15 respectivamente.

O reconhecimento de cores é feito sobre blocos de isopor pintados, colocados sobre a trajetória que deveria ser realizada pelo robô. Ela é feita através do módulo ilustrado pela figura abaixo. Ele é composto por um foto-diodo (TIL-78) e por um LED RGB. Para reduzir a interferência da luz externa e isolar o diodo do LED, ambos foram envoltos por termo-retráteis. Em seguida, em prol de se eliminar alta reflexibilidade das peças LEGO onde os sensores estão embutidos, ambos foram inseridos dentro de um termo-retrátil maior e alinhados de forma que a luz emitida pelo LED não fosse capturada pelo foto-diodo. A eficácia dessa medida foi comprovada em testes.

Após feita a montagem foi feita a caracterização do sensor, na qual foram obtidas medidas considerando a leitura da luz ambiente refletida pelo bloco e a reflexão das cores básicas RGB (vermelho, verde e azul) e suas combinações CMY (ciano, magenta e amarelo) emitidas pelo LED RGB, como ilustrado pela figura abaixo. Para aumentar a precisão de leitura, optou-se por utilizar a medida do ambiente como maneira de se eliminar a interferência externa, subtraindo-se de cada leitura o valor obtido antes de se utilizar o LED. Além disso as cores CMY possibilitaram detectar com maior precisão não só blocos de isopor, mas também peças LEGO.

Estas medidas foram feitas sob todos os blocos de isopor, considerando diferentes distâncias (3mm, 6mm e 15mm), ausência e presença de luz ambiente, bem como a possível interferência que poderia ser causada pelos motores ligados. Os gráficos abaixo relatam apenas os dados obtidos apenas para o bloco de isopor vermelho, no entanto, o restante dos dados obtidos se encontra anexo à este documento (dados coletados).

Sem Luz

Com Luz Ambiente

Como não detectamos nenhuma alteração relevante dos dados com a ligação dos motores, resolvemos não incluir a ilustração dos dados obtidos. No entanto, os dados coletados em anexo ilustram o observado.

Para identificar a presença de um obstáculo (bloco) foi implementado um sensor break-beam, semelhante a uma garra, ilustrado pela figura abaixo:

Assim quando o feixe de luz é interrompido, o robô para e, em seguida, aciona o mecanismo de reconhecimento de cores para determinar a cor do bloco e portanto a ação correspondente que deverá ser tomada.

O sistema de localização baseado em fontes de luz polarizadas é muito importante para a movimentação do robô no campo de competição do laboratório e ainda interfere muito na execução de diversas tarefas que serão requeridas na competição. Primeiramente é feito um reconhecimento da luz ambiente e esse fator é armazenado para retirar a interferência da luz ambiente nas medições. Após isso, o robô faz 32 rotações de mesmo ângulo até completar 360 graus, sendo que a cada rotação é feita uma medição da intensidade de luz recebida pelo sensor diferencial. Após coletar esses dados, o robô se movimenta na direção que mais o aproxima da fonte de luz destino especificada enquanto o valor lido no sensor indicar que ele continua se aproximando. Caso esse valor diminua significantemente, ele faz uma nova coleta de dados realizando suas 32 rotações e então define a nova direção de maior crescimento na função que o aproxima da fonte de luz polarizada definida.

A implementação que faz com que o robô se desloque sobre uma linha, foi baseada em uma máquina de estados ilustrada pela figura abaixo:

Nesta máquina de estado, o estado inicial (0) representa que ambos os sensores estão sobre a linha preta e que o robô deve se mover em linha reta. Qualquer variação no alinhamento do robô em relação a sua rota ideal faz com que o estado seja alterado para um dos outros dois possíveis: caso o robô saia a esquerda (1), caso o robô saia a direita(2). Nestes estados, o controle reage alterando a velocidade objetivo de uma das rodas afim de compensar o erro. Quando ambos os sensores voltam para a trajetória a pertubação é desfeita e uma nova é criada afim de alinhar o robô.

Neste trabalho foi construído um robô móvel dotado de um controle cinemático utilizando os modelos de controle proporcional-derivativo (PD), capaz de: se orientar em um campo com duas fontes de luz polarizadas, seguir uma linha, detectar blocos coloridos e tomar ações diferentes de acordo com as cores dos blocos encontrados. Descrevemos os passos da montagem e as dificuldades encontradas em cada passo, bem como os resultados da execução das tarefas. E como ressaltado anteriormente, fica claro que o problema que mais afetou o desempenho nas tarefas está relacionado à dificuldade de implementar um controle PD estável, a partir de uma resolução de pulsos muito baixa e incompatível com a velocidade do robô.

#include fencdr5.icb
#include fencdr6.icb
 
#define SAMPLES 100
#define LIGHT_SAMPLES 10
#define PORT_LIGHT_PHOTO_DIODE 2
#define PORT_LIGHT_DIF_CDS 3
#define PORT_BLOCK_SENSOR 4
#define LIM_LEFT -30
#define LIM_RIGHT 30
#define BLOCKS_AMOUT 5
 
#define PUSH 0
#define RIGHT_PASS 1
#define LEFT_PASS 2
#define STOP 3
 
#define PV 0.5
#define PD 0.001
#define DV 0.15
 
//#define DEBUG 1
 
#ifdef DEBUG
int data[800];
#endif
 
/* RGB led colors */
char color[] = {
    0, //none
      16,//red
      8, //green
      4, //blue
      24,//rg
      20,//rb
      12 //gb
  };
 
/* Block color responses from .6mm (enviroment-measured) */
int blocks[30] = {
    80,5,13,86,93,19,    //red
      17,41,21,59,39,63,   //green
      5,10,61,16,66,72,    //blue
      111,34,21,143,130,55,//yellow
      5,5,14,11,20,20      //black
  };
 
/* Block colors */
char color_names[6][10] = {"vermelho","verde","azul","amarelo","preto"};
 
int LAST_STATE = 0;
int LEFT_LINE_SENSOR = 11;
int RIGHT_LINE_SENSOR = 14;
int OUT_LINE = 1;
int IN_LINE = 0;
int right_vT = 10;
int left_vT = 10;
 
int SENSOR_CONTROL, STOP_ALL;
int BLOCK_FUNCTIONS[] = {1,2,0,3,3};
 
float k=PV, k1=PD, //proportional constants
k2=-DV;       //derivative constant
 
float v0, v1, lastv0, lastv1;
int acum0, acum1, i;
int s0, s1;
int push_time=3;
/**
 * @author alpm, andrebo, thvmm
 * 
 * walking straight counts:
 * @50/39 for 800L: 80 counts:24cm => 3.33 counts = 1cm
 * @41/41 for 500L: 60 counts:17.5 => 3.42 counts = 1cm
 * turn 90 degrees
 * axis width: 16cm => 2*pi*r/4 = 12.56cm on each weel => 42.96 counts
 */
 
int main(){
    int pid;
    int v0 = 60;
    int v1 = 50;
    int function, l;
    alloff();
 
 
    while(!start_button()){
        l = knob();
        if(l < 85){
            function = 0;
            printf("\n\nFuncao: Seguir Luz");
        } else if(l>= 85 && l<=170) {
            function = 1;
            printf("\n\nFuncao: Blocos");
        } else {
            function = 2;
            printf("\n\nFuncao: Teste Blocos");
        } msleep(100L);
    }
    start_press();
    switch(function){
        case 0: beep(); lightFollow(_selectLight()); break;
        case 1: calibrate(); beep(); start_process(check_sensor()); goAheadLine(); break;
        case 2: beep(); start_process(check_sensor()); goAheadLine(); break;
    }
 
    return 0;
}
 
void goAhead3(){
    while(1){
        while(SENSOR_CONTROL || STOP_ALL);
        if(digital(LEFT_LINE_SENSOR) == OUT_LINE && digital(RIGHT_LINE_SENSOR) == IN_LINE){
            motor(0, 25);
            motor(3, 50);
        } else if(digital(RIGHT_LINE_SENSOR) == OUT_LINE && digital(LEFT_LINE_SENSOR) == IN_LINE){
            motor(0, 50);
            motor(3, 25);
        } else if(digital(RIGHT_LINE_SENSOR) == IN_LINE && digital(LEFT_LINE_SENSOR) == IN_LINE){
            motor(0, 50);
            motor(3, 50);
        }
        msleep(300L);
    }
}
 
int calibrate(){
    int i, lastFunction;
    int functions[BLOCKS_AMOUT];
 
    for(i=0; i<BLOCKS_AMOUT; i++){
        functions[i] = (i%4);
    }
 
    for(i=0; i<BLOCKS_AMOUT; i++){
        //Define funcionalidade para o bloco
        lastFunction = _selectFunction(functions, i, color_names[i]);
        functions[lastFunction] = 1;
        if(lastFunction == PUSH){
            push_time = _selectTime();
        }
        _printFunction(lastFunction);
        msleep(1000L);
        start_press();
    }
 
}
 
int _selectTime(){
    int l;
    do {
        l = (20*knob()/255);
        printf("\n\nTempo: %d s", l);
        msleep(1000L);
    } while(!start_button());
 
    return l;
}
 
int _selectLight(){
    int l;
    do {
        l = (1*knob()/255);
        printf("\n\nLuz: %d", l);
        msleep(100L);
    } while(!start_button());
 
    return l;
}
 
 
/**
 * Seleciona a funcionalidade para um bloco. Recebe o historico das 
 * funcionalidades ja escolhidas como parametro para nao escolher 
 * repetir.
 */
int _selectFunction(int selectedFunctions[], int block, char name[]){
    int lastFunction, lastKnob;
    int i;
 
    printf("\n\nBloco %s: ", name);
 
    lastKnob = knob();
 
    while(!start_button()){
        //Funcionalidade: Empurrar
        if(knob() < 64 && lastKnob != knob()){
            printf("\n\nBloco %s: ", name);
            printf("Empurrar");
            lastFunction = PUSH;
        }
        //Funcionalidade: Passar a direita
        if(knob() >= 64 && knob() < 128 && lastKnob != knob()){
            printf("\n\nBloco %s: ", name);
            printf("Passar a direita");
            lastFunction = RIGHT_PASS;
        }
        //Funcionalidade: Passar a esquerda
        if(knob() >= 128 && knob() < 192 && lastKnob != knob()){
            printf("\n\nBloco %s: ", name);
            printf("Passar a esquerda");
            lastFunction = LEFT_PASS;
        }
        //Funcionalidade: Parar
        if(knob() >= 192 && knob() < 256 && lastKnob != knob()){
            printf("\n\nBloco %s: ", name);
            printf("Parar");
            lastFunction = STOP;
        }
        lastKnob = knob();
        msleep(500L);
    }
 
    BLOCK_FUNCTIONS[block] = lastFunction;
    return lastFunction;   
}
 
/**
 * Recebe o codigo da funcao e imprime o nome da mesma.
 */
void _printFunction(int f){
    switch(f){
        case 0:
          printf("\n\nEmpurrar");
        break;
        case 1:
          printf("\n\nPassar a direita");
        break;
        case 2:
          printf("\n\nPassar a esquerda");
        break;
        case 3:
          printf("\n\nParar");
        break;
    }
}
 
/* Segue luz polarizada especificada */
void lightFollow(int light){
    int l, env, m, k;
 
    env = analog(PORT_LIGHT_PHOTO_DIODE);
 
    while(1){
        if(light == 0) m = -32768;
        else m = 32768;
        k = 0;
        for(i=0; i<32; i++){
            l = analog(PORT_LIGHT_DIF_CDS) - env;
            if((light == 0 && l <= m) || (light == 1 && l > m)){
                m = l;
                k = i;
            }
            printf("\n\nm:%d l:%d",m,l);
            turn(5);
            msleep(50L);
        }
        turn(5*k);
        do {
            l = analog(PORT_LIGHT_DIF_CDS) - env;
            goAhead(50);
        } while ((light == 0 && l <= m+10) || (light == 1 && l > m-10));
 
    }
}
 
/* Segue em linha reta */
void goAhead(int target){
    lastv0 = 0.;
    lastv1 = 0.;
    acum0 = 0;
    acum1 = 0;
    encoder5_counts = 0;
    encoder6_counts = 0;
 
    v0 = 0.0;
    v1 = 0.0;
 
    while(1){
        control((target < 0));
        if(v0 >= 0.0)
          acum0 += encoder6_counts;
        else
          acum0 -= encoder6_counts;
 
        if(v1 >= 0.0)
          acum1 += encoder5_counts;
        else
          acum1 -= encoder5_counts;
 
        lastv0 = (float)encoder6_counts;
        lastv1 = (float)encoder5_counts;
        encoder5_counts = 0; 
        encoder6_counts = 0;
 
        #ifdef DEBUG
          printf("\n\na0:%d v0:%d    a1:%d v1:%d",acum0,(int)v0,acum1,(int)v1);
        #endif
 
 
        if(
           (target >= 0 && (acum0 >= target || acum1 >= target)) ||
           (target <  0 && (acum0 <= target || acum1 <= target))
           ) break;
        msleep(100L);
    }
 
    alloff();
}
 
/* Segue em linha reta por tempo determinado */
void goAheadTime(int target){
    lastv0 = 0.;
    lastv1 = 0.;
    acum0 = 0;
    acum1 = 0;
    encoder5_counts = 0;
    encoder6_counts = 0;
    i=0;
    v0 = 0.0;
    v1 = 0.0;
 
    while(1){
        control(0);
        if(v0 >= 0.0)
          acum0 += encoder6_counts;
        else
          acum0 -= encoder6_counts;
 
        if(v1 >= 0.0)
          acum1 += encoder5_counts;
        else
          acum1 -= encoder5_counts;
 
        lastv0 = (float)encoder6_counts;
        lastv1 = (float)encoder5_counts;
        encoder5_counts = 0; 
        encoder6_counts = 0;
 
        #ifdef DEBUG
          printf("\n\na0:%d v0:%d    a1:%d v1:%d",acum0,(int)v0,acum1,(int)v1);
        #endif
 
 
        if(
           (i/10 >= target)
           ) break;
        i++;
        msleep(100L);
    }
 
    alloff();
}
 
/* Gira em torno do eixo */
void turn(int target){
    lastv0 = 0.;
    lastv1 = 0.;
    acum0 = 0;
    acum1 = 0;
    encoder5_counts = 0;
    encoder6_counts = 0;
 
    v0 = 0.0;
    v1 = 0.0;
 
    while(1){
        turnControl((target < 0));
        if(v0 >= 0.0)
          acum0 += encoder6_counts;
        else
          acum0 -= encoder6_counts;
 
        if(v1 >= 0.0)
          acum1 += encoder5_counts;
        else
          acum1 -= encoder5_counts;
 
        lastv0 = (float)encoder6_counts;
        lastv1 = (float)encoder5_counts;
        encoder5_counts = 0; 
        encoder6_counts = 0;
 
        #ifdef DEBUG
          printf("\n\na0:%d v0:%d    a1:%d v1:%d",acum0,(int)v0,acum1,(int)v1);
        #endif
 
        if(
           (target >= 0 && (acum0 >= target || acum1 <= -target)) ||
           (target <  0 && (acum0 <= target || acum1 >= -target))
           ) break;
        msleep(50L);
    }
 
    alloff();
}
 
/* Segue linha preta */
void goAheadLine(){
 
    int state = 0;
    int vT = 10;
    int dif, w = 1, p;
 
    right_vT = left_vT = vT;
    v0 = 0.0;//full batery:17.0;
    v1 = 0.0;
 
    encoder5_counts = 0; 
    encoder6_counts = 0;
    acum0 = 0;acum1 = 0;
 
    while(1){ //andar pra frente pra sempre
 
        findLine();
 
        if(SENSOR_CONTROL == 1){
            alloff();
            #ifdef DEBUG
              data[i++] = -10;
            #endif
        }
        while(SENSOR_CONTROL || STOP_ALL == 1); //wait until 'lock' is released
 
        if(i%3 == 0){
            control(0);
            acum0 += encoder6_counts;
            acum1 += encoder5_counts;
 
            lastv0 = (float)encoder6_counts;
            lastv1 = (float)encoder5_counts;
            encoder5_counts = 0; 
            encoder6_counts = 0;
        }
 
 
        if(stop_button())break;
 
        msleep(100L);          
    }
}
 
/* Máquina de estados de busca de linha - Controla velocidades */
void findLine(){
    int fix = 2;
 
    #ifdef DEBUG
      data[i] = LAST_STATE;
    #endif
    if(digital(LEFT_LINE_SENSOR) == OUT_LINE && digital(RIGHT_LINE_SENSOR) == IN_LINE){
        if(LAST_STATE == 0) {
            left_vT += fix;
            LAST_STATE = 1;
        }
    } else if(digital(RIGHT_LINE_SENSOR) == OUT_LINE && digital(LEFT_LINE_SENSOR) == IN_LINE){
        if(LAST_STATE == 0) {
            right_vT += fix;
            LAST_STATE = 2;
        }
    } else if(digital(RIGHT_LINE_SENSOR) == IN_LINE && digital(LEFT_LINE_SENSOR) == IN_LINE){
        //Os 2 estao no preto
        switch(LAST_STATE){
            case 0:
              break;
            //Ultimo estado: saiu para esquerda
            case 1:
              LAST_STATE = 3;
            left_vT -= fix;
            right_vT += fix;
            break;
            //Ultimo estado: saiu para direita
            case 2:
              LAST_STATE = 4;
            right_vT -= fix;
            left_vT += fix;
            break;
            //Ultimo estado: veio da esquerda, precisa corrigir
            case  3:
              LAST_STATE = 0;
            right_vT -= fix;
            break;
            //Ultimo estado: veio da direita, precisa corrigir
            case 4:
              LAST_STATE = 0;
            left_vT -= fix;
            break;
 
        }
    }   
    control(0);
}
 
/* @thread - Detecção de bloco, cores e despacho de tarefa */
void check_sensor(){
    int c;
    while(SENSOR_CONTROL == 0){
        if(analog(PORT_BLOCK_SENSOR) > 30){
            alloff();
            SENSOR_CONTROL = 1;
 
            c = detect_color();
            printf("\n\n%d %s", c, color_names[c]);
            execute_task(c);
            SENSOR_CONTROL = 0;
        }
    }
}
 
/* Executa tarefa */
void execute_task(int c){
    int function = BLOCK_FUNCTIONS[c];
    switch(function){
        case STOP:
          STOP_ALL = 1; break;
        case PUSH:
          goAheadTime(push_time); break;
        case LEFT_PASS:
          goAhead(-30);
        turn(45);
        goAhead(65);
        turn(-45);
        goAhead(140);
        turn(-45);
        goAhead(65);
        turn(45);
        break;
        case RIGHT_PASS:
          goAhead(-30);
        turn(-45);
        goAhead(65);
        turn(45);
        goAhead(140);
        turn(45);
        goAhead(65);
        turn(-45);
        break;   
    }
}
 
/* Detecta cor por desvio padrão */
int detect_color(){
    int measures[7], i, j;
    int min=0, n1;
    float pi, z, scores[BLOCKS_AMOUT];
 
    poke(0x1009, 0x3c);
 
    for(i=0; i<7; i++){
        poke(0x1008, color[i]);
        measures[i] = 0;
        for(j=0; j<LIGHT_SAMPLES; j++){
            msleep(250L);
            measures[i] += analog(PORT_LIGHT_PHOTO_DIODE);
        }
        measures[i] /= LIGHT_SAMPLES;
 
        if(i>0){
            measures[i] = measures[0]-measures[i];
        }
    }
    #ifdef DEBUG
      printf("\n\n%d %d %d %d %d %d %d",measures[0],measures[1],measures[2],measures[3],measures[4],measures[5],measures[6]);
    start_press();
    #endif
    poke(0x1008, 0);
 
    for(i=0; i<BLOCKS_AMOUT; i++){
        pi = 0.;
        for(j=0; j<6; j++){
            n1 = measures[j+1]-blocks[i*6+j];
            z = (float)(n1)*(float)(n1);
            pi += z;
            #ifdef DEBUG
              printf("\n\nn1:%d pi:%f",n1,z);
            start_press();
            #endif
        }
 
        /* Stores the angle (normalized dot product) of the vectors */
        scores[i] = sqrt(pi);
        #ifdef DEBUG
          printf("\n\ni:%d score:%f",i,scores[i]);
        start_press();
        #endif
        if(scores[i] < scores[min]){
            min = i;
        }
    }
    #ifdef DEBUG
      printf("\n\nmin:%d",min);
    start_press();
    #endif
 
    return min;
}
 
/* Função de controle em linha reta */
void control(int invert){
    if(invert){
        v0 -= k * (float)(right_vT - encoder6_counts) + k1*((float)acum1-(float)acum0) + (k2 * ((float)encoder6_counts-lastv0));
        v1 -= k * (float)(left_vT - encoder5_counts) + k1*((float)acum0-(float)acum1) + (k2 * ((float)encoder5_counts-lastv1));
    } else {
        v0 += k * (float)(right_vT - encoder6_counts) + k1*((float)acum1-(float)acum0) + (k2 * ((float)encoder6_counts-lastv0));
        v1 += k * (float)(left_vT - encoder5_counts) + k1*((float)acum0-(float)acum1) + (k2 * ((float)encoder5_counts-lastv1));
    }
 
    if(v0 > 100.0) v0 = 100.0;
    if(v0 < -100.0) v0 = -100.0;
    if(v1 > 100.0) v1 = 100.0;
    if(v1 < -100.0) v1 = -100.0;
    motor(3, (int)v1);           
    motor(0, (int)v0);
}
 
/* Função de controle pra rotação em torno do eixo */
void turnControl(int invert){
    if(invert){
        v0 -= k * (float)(right_vT/2 - encoder6_counts) + k1*((float)acum1-(float)acum0) + (k2 * ((float)encoder6_counts-lastv0));
        v1 += k * (float)(left_vT/2 - encoder5_counts) + k1*((float)acum0-(float)acum1) + (k2 * ((float)encoder5_counts-lastv1));
    } else {
        v0 += k * (float)(right_vT/2 - encoder6_counts) + k1*((float)acum1-(float)acum0) + (k2 * ((float)encoder6_counts-lastv0));
        v1 -= k * (float)(left_vT/2 - encoder5_counts) + k1*((float)acum0-(float)acum1) + (k2 * ((float)encoder5_counts-lastv1));
    }
 
    if(v0 > 100.0) v0 = 100.0;
    if(v0 < -100.0) v0 = -100.0;
    if(v1 > 100.0) v1 = 100.0;
    if(v1 < -100.0) v1 = -100.0;
    motor(3, (int)v1);           
    motor(0, (int)v0);
}