Projeto de código Seguidor de Linha com Lego NXT 2.0
Autor: Santos | Publicado: 2015-05-07T12:26:00-03:00

Este projeto foi desenvolvido por mim e José Adolfo durante a disciplina de Introdução a Programação 2014.2 na Univasf(Universidade Federal do Vale do São Francisco). Trata-se de um código em NXC (Not Exactly C) no ambiente Bricx que deveria fazer um robô Lego NXT 2.0 seguir uma linha composta de duas cores: azul e verde, com objetivo de encontrar a cor amarela. O robô deveria ficar numa área demarcada pela cor vermelha, iniciando de qualquer posição dentro dessa região. O detalhe é que não era necessário seguir a linha, sendo apenas necessário encontrar a cor amarela e assim feito tocar uma música predefinida ou personalizada. Entretanto, encontrar a cor amarela se faz mais fácil seguindo a linha composta já que essa leva até a cor alvo.

Videos de testes:


Teste 1




Teste 2



O código segue abaixo:


#define P_ST   IN_1
#define P_SC   IN_2
#define PRETO   INPUT_BLACKCOLOR
#define AZUL   INPUT_BLUECOLOR
#define VERDE   INPUT_GREENCOLOR
#define AMARELO   INPUT_YELLOWCOLOR
#define VERMELHO  INPUT_REDCOLOR
#define BRANCO   INPUT_WHITECOLOR
#define LIMITE     4
#define DIV        4

enum ESTAGIOS { INICIO = 0, SEGUIR, FINAL };

void girarEsquerdaMotC( char vel ) {
     OnFwd( OUT_A, vel ); OnRev( OUT_C, 30 );
}
void girarDireitaMotA( char vel ) {
     OnFwd( OUT_C, vel ); OnRev( OUT_A, 30 );
}

task main() {
     int corAnterior = PRETO;
     char estagio = 0, relogio;

     SetSensorTouch( P_ST );
     SetSensorColorFull( P_SC ); 

     until(Sensor(P_ST) != 0); 
     while(1) {
          if( Sensor(P_SC) == VERMELHO ) {
               relogio = 1;
               while(Sensor(P_SC) == VERMELHO && relogio > 0 && relogio < 10) {
                    OnRev(OUT_AC, 80);
                    Wait(300);
                    relogio += 1;
               }
               relogio = 0;
               if(Sensor(P_SC) == BRANCO)
                    relogio = 1;
               while(Sensor(P_SC) == BRANCO && relogio > 0 && relogio < 100) {
                    OnFwd(OUT_C, 70);
                    OnRev(OUT_A, 70);
                    Wait(10);
                    relogio += 1;
               }
               relogio = 0;
          }else if( Sensor(P_SC) == AMARELO ) {
               estagio = FINAL;
          }
  
          if( estagio == INICIO ) {
               int cor = Sensor(P_SC);
               if( cor == VERDE || cor == AZUL ) {
                    corAnterior = cor;    
                    while( cor == corAnterior ) {
                         OnFwd(OUT_AC, 100);
                         cor = Sensor(P_SC);
                    }
                    if( Sensor(P_SC) == BRANCO ) {
                         if( corAnterior == VERDE ) {
                              girarDireitaMotA(100);
                              while(Sensor(P_SC) == VERDE || Sensor(P_SC) == BRANCO );
                         } else if( corAnterior == AZUL ) {
                              girarEsquerdaMotC(100);
                              while(Sensor(P_SC) == AZUL || Sensor(P_SC) == BRANCO );
                         }
                    }
                    estagio = SEGUIR;
               }else {
                    OnFwd(OUT_AC, 100);
               }
          }else if( estagio == SEGUIR ) {
               int vGiro = 0, cmp = 0, motorE = OUT_C, motorD = OUT_A;
               char flag = 0;
   
               cmp = Sensor(P_SC);   
               while( cmp == AZUL || cmp == VERDE ) {
                    if( !flag ) {
                         Wait(100);
                         flag = 1;
                    }

                    OnFwd(OUT_AC, 100);
                    corAnterior = cmp;
                    cmp = Sensor(P_SC);
               }

               if( cmp == BRANCO && corAnterior == VERDE ) {
                    motorE = OUT_A;
                    motorD = OUT_C;
               }   

               vGiro = 75 + 230 * (LIMITE - cmp) / DIV;   
               if( vGiro > 100 ) vGiro = 100;
               if( vGiro < -100 )vGiro = -100;
               if( cmp == AMARELO ) vGiro = 0;

               if( vGiro < 0 ) {
                    OnRev(motorE, -vGiro);
               } else {
                    OnFwd(motorE, vGiro);
               }

               vGiro = 75 - 230 * (LIMITE - cmp) / DIV;
               if( cmp == AMARELO ) vGiro = 0;
               if( vGiro > 100 ) vGiro = 100;
               if( vGiro < -100 ) vGiro = -100;

               if( vGiro < 0 ) {
                    OnRev(motorD, -vGiro);
               } else {
                    OnFwd(motorD, vGiro);
               }
          }else if( estagio == FINAL ) {
               Off(OUT_AC);   
               PlayTone( TONE_G6, 280); Wait(240); PlayTone( TONE_G6, 280); Wait(240); PlayTone( TONE_G6, 330); Wait(240); PlayTone( TONE_D7, 360); Wait(260); PlayTone( TONE_C7, 320); Wait(250); PlayTone( TONE_D7, 740); Wait(760);
               StopAllTasks();
          }
     }
}


O Bricx é uma central de controle que conta com ferramentas, dentre elas, uma IDE. Mas que funciona apenas no Windows. Para Linux e Mac você pode utilizar o NBC que é o compilador, sem IDE.