robocircuit.c

#define SPEED 75
#define	INPUT_BLACKCOLOR 1
#define	INPUT_BLUECOLOR 2
#define	INPUT_GREENCOLOR 3
#define	INPUT_YELLOWCOLOR 4
#define	INPUT_REDCOLOR 5
#define	INPUT_WHITECOLOR 6


//************ ÚLTIMO CÓDIGO *****************

ColorSensorReadType csr;
bool shouldFinish = false;
bool shouldTurn = false;
bool shouldStop = false;
bool waitingToStop = false;

int globalRed = 0;

bool teste=false;



int obsNum = 0;

task walk()
{
    SetSensorLowspeed(IN_4);

     int totalRight = 0;
     int totalLeft = 0;
     OnFwd(OUT_AC, 75);
     while (true)
     {
           //Caso seja branco ou seja, robo saiu da linha, deve encontrar a linha novamente
           if (csr.ColorValue == 6)
           {
           
              if(totalLeft < 9){
                //Inverte o motor C
                OnFwd(OUT_A,50);
                OnRev(OUT_C,50);
                totalLeft += 1;
                Wait(100);
              }
              else if(totalRight < (2* (totalLeft) -3) ){
                //Inverte o motor A
                OnFwd(OUT_C,50);
                OnRev(OUT_A,50);
                totalRight += 1;
                Wait(100);
              }
              else
              {
              totalRight = 0;
              totalLeft = 0;
              }
           }
           else if(csr.ColorValue != INPUT_WHITECOLOR){
           
                totalRight = 0;
                totalLeft = 0;
               if(shouldStop == true){
                            Off(OUT_AC);
                            Wait(3000);
                            shouldStop = false;
               }
            else if(shouldTurn == true ){
               Off(OUT_AC);
               Wait(100);
               OnRev(OUT_C,75);
               OnFwd(OUT_A,75);
               Wait(500);
               
               shouldTurn = false;
             }
             else if(shouldFinish == true){
               Off(OUT_AC);
               Wait(10000);
           }

           Off(OUT_AC);
           OnFwd(OUT_AC,75);
           Wait(100);
     }

    if(SensorUS(IN_4) <= 15){
       
       do{
       
          //Direita
          OnRev(OUT_A,75);
          OnFwd(OUT_C,75);
          Wait(1000);
       
          //Anda
          OnFwd(OUT_AC,75);
          Wait(1000);
       
          //Esquerda
          OnRev(OUT_C,75);
          OnFwd(OUT_A,75);
          Wait(1000);
       
       }while(SensorUS(IN_4) < 30);
       
        //Direita
        OnRev(OUT_A,75);
        OnFwd(OUT_C,75);
        Wait(1000);

        //Anda
        OnFwd(OUT_AC,75);
        Wait(1000);

        //Esquerda
        OnRev(OUT_C,75);
        OnFwd(OUT_A,75);
        Wait(1000);
       
        //Anda
        OnFwd(OUT_AC,75);
        Wait(1000);
        
        //Esquerda
        OnRev(OUT_C,75);
        OnFwd(OUT_A,75);
        Wait(1000);
       
        while(SensorUS(IN_4) < 30){
          //Direita
          OnRev(OUT_A,75);
          OnFwd(OUT_C,75);
          Wait(1000);

          //Anda
          OnFwd(OUT_AC,75);
          Wait(1000);
          
          //Esquerda
          OnRev(OUT_C,75);
          OnFwd(OUT_A,75);
          Wait(1000);
        }
        
         //Direita
          OnRev(OUT_A,75);
          OnFwd(OUT_C,75);
          Wait(1000);

          //Anda
          OnFwd(OUT_AC,75);
          Wait(1000);

          //Esquerda
          OnRev(OUT_C,75);
          OnFwd(OUT_A,75);
          Wait(550);

        while(csr.ColorValue == 6){
           //Anda
           OnFwd(OUT_AC,75);
           Wait(100);
       }
       
        //Direita
        OnRev(OUT_A,75);
        OnFwd(OUT_C,75);
        Wait(600);
    }
  }
}


task color(){

     int localGreenCount = 0;
     int localRed = 0;
     while(true){
    
      SysColorSensorRead(csr);
      if (csr.Result == NO_ERR) {

            switch(csr.ColorValue){
            case INPUT_BLACKCOLOR:


                 //ClearScreen();
                 //NumOut(0, LCD_LINE1, csr.ColorValue);
                 //TextOut(50,LCD_LINE1,"PRETO");
                 teste = false;
                 break;
            case INPUT_BLUECOLOR:

                 //ClearScreen();
                 //NumOut(0, LCD_LINE1, csr.ColorValue);
                 //TextOut(50,LCD_LINE1,"AZUL");
                                  teste = false;
                 break;
            case INPUT_GREENCOLOR:

                 int countGreen = 0;
                 //ClearScreen();
                 //TextOut(0,LCD_LINE1,"VERDE * ");
                 //NumOut(50, LCD_LINE1,countGreen);
                                  teste = false;

                 int i=0;
                 while(i <= 15){
                    i++;
                    SysColorSensorRead(csr);
                    if(csr.ColorValue == INPUT_GREENCOLOR){
                       countGreen +=1;
                       Wait(40);
                    }
                 }

                 if(countGreen > 14)
                     shouldStop = true;

                 break;
            case INPUT_YELLOWCOLOR:

                 //ClearScreen();
                 //NumOut(0, LCD_LINE1, csr.ColorValue);
                 //TextOut(50,LCD_LINE1,"AMARELO");
                                  teste = false;
                 break;
            case INPUT_REDCOLOR:
                 
                 int j=0;
                 
                 localRed++;
                 
                 if(localRed > 300){
                             globalRed++;
                             localRed = 0;
                 }
                 
                 if(localRed > 150 && localRed < 250 && globalRed > 2){
                     shouldFinish = true;
                 }
                 
                 if(globalRed == 2){
                     shouldTurn = true  ;
                 }
                 

                 break;
            case INPUT_WHITECOLOR:


                 break;
            }
            
            ClearScreen();
            TextOut(0,LCD_LINE1,"VERMELHO");
            NumOut(50, LCD_LINE1, globalRed);
            TextOut(0,LCD_LINE2,"LOCAL");
            NumOut(50, LCD_LINE2, localRed);
            

      }
  }


}

task main()
{
  //Inicia o sensor de cor na porta S3
  SetSensorColorFull(S3);

  csr.Port = S3;
  
  Precedes(color,walk);
  

}