Lego NXT 2.0 Line Follower code project
Autor: Santos | Publicado: 2015-05-07T13:09:00-03:00

This project was developed by me and José Adolfo during the course of Introduction to Programming 2014.2 at Univasf. It is a code in NXC (Not Exactly C) in the environment Bricx and this code was suposed to make the Lego NXT 2.0 robot follow a bicolor line: blue and green, with the main target being to find the yellow color.
The robot had to be in an area demarcated with red, starting from any position inside that region. The trick was that it was not necessary to follow the bicolor line, being only necessary to find the yellow color at the end and then to play a predefined or custom music. However, to find the yellow color is easier following the line, since that color is in the end of that line.

Test videos:


Test 1




Test 2



And the code is bellow:


#define P_TS    IN_1
#define P_CS    IN_2
#define BLACK   INPUT_BLACKCOLOR
#define BLUE    INPUT_BLUECOLOR
#define GREEN   INPUT_GREENCOLOR
#define YELLOW  INPUT_YELLOWCOLOR
#define RED INPUT_REDCOLOR
#define WHITE   INPUT_WHITECOLOR
#define LIMIT   4
#define DIV     4

enum STAGES { BEGIN = 0, FOLLOW, END };

void turnLeftMotC char vel ) {
     OnFwd( OUT_A, vel ); OnRev( OUT_C, 30 );
}
void turnRightMotA char vel ) {
     OnFwd( OUT_C, vel ); OnRev( OUT_A, 30 );
}

task main() {
     int prevColor = BLACK;
     char stage = 0, mytimer;

     SetSensorTouch( P_TS );
     SetSensorColorFull( P_CS ); 

     until(Sensor(P_TS) != 0); 
     while(1) {
          if( Sensor(P_CS) == RED ) {
               mytimer = 1;
               while(Sensor(P_CS) == RED && mytimer > 0 && mytimer < 10) {
                    OnRev(OUT_AC, 80);
                    Wait(300);
                    mytimer  = 1;
               }
               mytimer = 0;
               if(Sensor(P_CS) == WHITE)
                    mytimer = 1;
               while(Sensor(P_CS) == WHITE && mytimer > 0 && mytimer < 100) {
                    OnFwd(OUT_C, 70);
                    OnRev(OUT_A, 70);
                    Wait(10);
                    mytimer  = 1;
               }
               mytimer = 0;
          }else if( Sensor(P_CS) == YELLOW ) {
               stage = END;
          }
  
          if( stage == BEGIN ) {
               int cor = Sensor(P_CS);
               if( cor == GREEN || cor == BLUE ) {
                    prevColor = cor;    
                    while( cor == prevColor ) {
                         OnFwd(OUT_AC, 100);
                         cor = Sensor(P_CS);
                    }
                    if( Sensor(P_CS) == WHITE ) {
                         if( prevColor == GREEN ) {
                              turnRightMotA100);
                              while(Sensor(P_CS) == GREEN || Sensor(P_CS) == WHITE );
                         } else if( prevColor == BLUE ) {
                              turnLeftMotC100);
                              while(Sensor(P_CS) == BLUE || Sensor(P_CS) == WHITE );
                         }
                    }
                    stage = FOLLOW;
               }else {
                    OnFwd(OUT_AC, 100);
               }
          }else if( stage == FOLLOW ) {
               int turnVel = 0, cmp = 0, motorE = OUT_C, motorD = OUT_A;
               char flag = 0;
   
               cmp = Sensor(P_CS);   
               while( cmp == BLUE || cmp == GREEN ) {
                    if( !flag ) {
                         Wait(100);
                         flag = 1;
                    }

                    OnFwd(OUT_AC, 100);
                    prevColor = cmp;
                    cmp = Sensor(P_CS);
               }

               if( cmp == WHITE && prevColor == GREEN ) {
                    motorE = OUT_A;
                    motorD = OUT_C;
               }   

               turnVel = 75   230 * (LIMIT - cmp) / DIV;   
               if( turnVel > 100 ) turnVel = 100;
               if( turnVel < -100 )turnVel = -100;
               if( cmp == YELLOW ) turnVel = 0;

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

               turnVel = 75 - 230 * (LIMIT - cmp) / DIV;
               if( cmp == YELLOW ) turnVel = 0;
               if( turnVel > 100 ) turnVel = 100;
               if( turnVel < -100 ) turnVel = -100;

               if( turnVel < 0 ) {
                    OnRev(motorD, -turnVel);
               } else {
                    OnFwd(motorD, turnVel);
               }
          }else if( stage == END ) {
               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();
          }
     }
}


The Bricx is a control center that have tools and one IDE. But it works only on Windows. If you use Linux or Mac you can use the NBC compiler, no IDE with it.