Robot FINAL

#define UMBRAL 65
#define DIST_PARED 24

int cont=0;

sub Espiral(){
int a,b,i;
    a=50;
    b=50;
    for(i=0;i<5;i++){

    OnFwd(OUT_B,a);
    OnFwd(OUT_A,b);
    Wait(200);


    if  (SENSOR_2<=UMBRAL){
        break;
    }

    b=b-10;
    }
}



sub Giro180(){  //Función Utilizada para girar el robot:
        Off(OUT_AB);    //Detiene ambas ruedas.
        OnFwd(OUT_A,50);
        OnRev(OUT_B,47);        //Mueve ambas ruedas en distintas direcciones para conseguir el giro
        Wait(400);                      //durante La cantidad de tiempo indicada
}

sub Pared(){

                        TextOut(20,30, "// PARED \\ ") ;
                        PlayTone(1500,50);
                        Off(OUT_AB);
                        Wait(1000);
                        OnRev(OUT_AB,50);       //Se efectúa un leve retroceso para preparar un giro mas correcto.
                        Wait(400);
                        Giro180();              //Llamada a la función para efectuar el giro.
                        Wait(1000);

}

sub NegroBlanco(){
    TextOut(20,30, "// (=_=) \\ ") ;
                        if(SENSOR_2<=UMBRAL){   //Se compara el lector del sensor de luz en valor de UMBRAL:
                                Off(OUT_AB);            //Si es menor, asumira que es negro y moverá una sola rueda.
                                OnFwd(OUT_A,40);
                                OnFwd(OUT_B,0);
                                cont=0;
                        }
                        else{   //De lo contrario, asumira que es blanco y moverá la otra rueda.
                                cont++;
                                Off(OUT_AB);
                                OnFwd(OUT_B,40);
                                OnFwd(OUT_A,0);

                                if(cont>200){
                                            TextOut(20,30, "// (O_O) \\ ") ;
                                            PlayTone(5500,50);
                                            Espiral();

                                            }
                                }
}
task main(){

        SetSensorLight (IN_2);
        SetSensorLowspeed (IN_4); //Se inicializan los sensores.

        while (TRUE){   //Se declara un ciclo infinito para que el robot efectúe las acciones indicadas indefinidamente.
               if (SensorUS(IN_4)<=DIST_PARED) { //Se comprueba la distancia con la pared; De ser menor a la programada:
                        Pared();
                        }
                else {
                       NegroBlanco();
                        }

                        }
                  }