definitivo

// definir a distância do objeto
#define 	INPUT_REDCOLOR   5
#define 	INPUT_GREENCOLOR   3
#define 	INPUT_BLACKCOLOR   1
#define 	INPUT_WHITECOLOR   6


        /* sub verde(int pwr)
         {
         SetSensor(S2, SENSOR_COLORFULL);
         SetSensor(S3, SENSOR_COLORFULL);
         SetSensorLowspeed(IN_1);
         SetSensorLowspeed(IN_4);

         OnRev(OUT_AC, 75);
         Wait(1000);

         }

        sub garra (int pwr)
        {

                           while(true)
                           {
                           if(Sensor(IN_2)<=30 && Sensor(IN_3)<=30)
                           {
                           OnFwd(OUT_AC, 75);
                           }

                           else if(SensorUS(IN_1)<10)
                           {
                            RotateMotor(OUT_C, 75, 600);
                            OnRev(OUT_AC, 75);
                           }

                           else if(SensorUS(IN_4)<7)
                           {
                           RotateMotor(OUT_B, 75, 50);
                           RotateMotor(OUT_A, 75, 600);
                           OnFwd(OUT_AC, 75);
                           }
                           }


        }
          */


        task main()
        {
        SetSensor(S2, SENSOR_COLORRED);
        SetSensor(S3, SENSOR_COLORRED);
        SetSensorLowspeed(IN_1);
        SetSensorLowspeed(IN_4);
        //white==48; apenas para decorar
       // black==18;
       //green==18;

          while(true)
          {

          if(Sensor(IN_2)>=30 && Sensor(IN_3)>=30)  // ao ver branco, andar pra frente
          {
          OnFwd(OUT_AC, 75);

           if(SensorUS(IN_1)<13)
          {
           OnFwd(OUT_C, 75); // forward 90 degrees
           OnRev(OUT_A, 75);
           Wait(900);
           OnFwd(OUT_AC, 75);
           Wait(250);

           RotateMotor(OUT_C, 50, 790);
           OnFwd(OUT_AC, 75);
           Wait(250);

           RotateMotor(OUT_C, 50, 790);
           OnFwd(OUT_AC, 75);
           Wait(250);

           if(Sensor(IN_2)>=30 && Sensor(IN_3)>=30)
           {
           RotateMotor(OUT_A, 50, 790);
           }
          }

          }

         else if(Sensor(IN_2)<=30 && Sensor(IN_3)<=30) //os dois sensores vendo preto

          {
          SetSensor(S2, SENSOR_COLORFULL);  //transforma os sensores de luz em sensor de cor
          SetSensor(S3, SENSOR_COLORFULL);
          SetSensorLowspeed(IN_1);
          SetSensorLowspeed(IN_4);

          OnRev(OUT_AC, 75);
          Wait(100);
          OnFwd(OUT_AC, 75);

          if(Sensor(IN_2) == 3)  //DIREITA vendo verde
          {
          OnRev(OUT_A, 50);
          OnFwd(OUT_C, 50);
          }

          else if(Sensor(IN_3) == 3)   // ESQUERDA vendo verde
          {
           OnRev(OUT_C, 50);
           OnFwd(OUT_A, 50);
          }

          }

          else if(Sensor(IN_2)<=30) // sensor 2, ao ver preto, desacelera lado DIREITO do robô
          {
          OnFwd(OUT_C, 50);
          OnRev(OUT_A, 50);
          }

          else if(Sensor(IN_3)<=30) // sensor 3, ao ver preto, desacelera lado ESQUERDO do robô
          {
          OnFwd(OUT_A, 50);
          OnRev(OUT_C, 50);

          }



          /*if (Sensor(IN_2) == 40 && Sensor(IN_3) == 40)
          {
           garra (60);
          }
            */
}
}