whatevernigga.3.0

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

         //inline garra;
        // {

         // if(Sensor(IN_4) <= 10)
          //{
          // rotatemotor(OUT_B, 75, 90);
          //
         // }

        // }

        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(Sensor(IN_2)<=30 && Sensor(IN_3)<=30) //os dois sensores vendo preto

          {
          OnFwd(OUT_AC, 75);
          }

          else if(Sensor(IN_2)<=30) // sensor 2, ao ver preto, desacelera lado DIREITO do robô
          {
          OnRev(OUT_A, 50);
          OnFwd(OUT_C, 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(SensorUS(IN_1)<20)
          {
           RotateMotor(OUT_A, 75, 790); // forward 90 degrees
           RotateMotor(OUT_C, -75, 790);
           OnFwd(OUT_AC, 75);
           Wait(500);

           RotateMotor(OUT_C, 75, 790);
           RotateMotor(OUT_A, -75, 790);
           OnFwd(OUT_AC, 75);
           Wait(500);

           RotateMotor(OUT_C, 75, 790);
           RotateMotor(OUT_A, -75, 790);
           OnFwd(OUT_AC, 75);
           Wait(500);

           RotateMotor(OUT_A, 75, 790); // forward 90 degrees
           RotateMotor(OUT_C, -75, 790);
           OnFwd(OUT_AC, 75);
           Wait(500);
          }

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

          }


 }