/**************************************************************************
/  NIUTINBOT.NXC
/  by NIUTIN TEAM
/           - Eduardo Jorge Carrasco Hernández
/           - Jorge Hernández Ramírez
/           - Emilio Macías Conde
/           - Enrique Ismael Mendoza Robaina
/           - Javier Antonio Monzón Santana
/
/  WEB:    http://www.niutin.es
/  BLOG:   http://www.canarias7.es/blogs/niutin
/  E-MAIL: niutininformatica@gmail.com
/          niutin@niutin.es
/
/ Descripción:
/   Código fuente de nuestro robot sigue línea negra y evita obstáculos.
/*************************************************************************/

#define negro  30
#define G90    50
#define velocidad 55
#define velgiro 30
#define velgiroesq 20
#define rango 15
#define DER 0
#define IZQ 1

int Control=0; // Control global de las Tareas
int Direccion=1; // Variable para Direccion (Der: 0; Izq: 1)
int Giros=G90; // 90º inicialmente
int FactorGiro = 1.2; // Factor para Aumentar Giro
int USLateral=S2; // Sensor UltraSonido Lateral
int USFrontal=S4; // Sensor UltraSonido Frontal
int avance=0; // Indica cuanto ha avanzado. Sirve para calcular el espera cuando ya no hay objeto
mutex Cerrojo;

int valluz(){
   int min=1023;
   for (int i=0; i<4; i++){   // 10
      int valsensor=SENSOR_3;
      if(valsensor<min)
         min=valsensor;
   }
   return min;
}

int valmax(){
   int max=0;
   for (int i=0; i<5; i++){  // 10
      int valsensor=SensorUS(USLateral);
      if(valsensor>max)
         max=valsensor;
   }
   return max;
}

int valmin(){
   int min=256;
   for (int i=0; i<5; i++){   // 10
      int valsensor=SensorUS(USLateral);
      if(valsensor<min)
         min=valsensor;
   }
   return min;
}

void espera(int tiempo){
   int i;
   i=0;
   while ( ((SENSOR_3>negro+rango) || (SENSOR_3<negro-rango) ) && (i<tiempo*10) ){
      i++;
   }
}

void gira90(int Dir){
      if (Dir==0) { // Gira a la Derecha
         OnRev(OUT_B,velgiroesq); //Giramos sobre el mismo
         OnFwd(OUT_C,velgiroesq);
         // Mientras No 90º
         while (!((SENSOR_3>negro+rango) || (SENSOR_3<negro-rango))) {
      }
      Off(OUT_BC);
      OnRev(OUT_B,velgiroesq*2); //Giramos sobre el mismo
      OnFwd(OUT_C,velgiroesq*2);
      while (SensorUS(USLateral)>20) {}
      Off(OUT_BC);
   }
   else
   { // Gira a la Izquierda
      OnRev(OUT_C,0); //Giramos sobre el mismo
      OnFwd(OUT_B,velgiroesq*4);
      espera(90);
      Off(OUT_BC);

   }
   // Hacemos que no sean 90º exactos para que no se pege mucho al objeto
   // debido al mal funcionamiento de los motores.
}

task lee_ultrasonido()
{
   while(true) {
       Acquire(Cerrojo);
       if (SensorUS(USFrontal)<26) { // 30
          Control=1;

          // Giramos a la derecha cuando encuentre la linea mientras rebasa el obstáculo
          Direccion=0;
          Giros=1000;  // Aumentamos el giro para asegurarnos que el camino que toma es la derecha

          Off(OUT_BC);
          gira90(DER);
          while ((valluz()>negro+rango) || (valluz()<negro-rango)) {  // Mientras blanco (no negro)
             avance=0;
             while (((valluz()>negro+rango) || (valluz()<negro-rango)) && (valmin()<=30)){
                OnFwdReg(OUT_BC, 55,OUT_REGMODE_SYNC); // Hacia adelante.
                avance++;
             }
             OnFwdReg(OUT_BC, 55,OUT_REGMODE_SYNC); // Hacia adelante. 25
             espera(12+avance); // Es un poco heurístico (ensayo-error) Lo que hacemos es calcular el avance mediante el tamaño del objeto que conocemos por el tiempo que el robot lo ha visualizado.
             Off(OUT_BC);
             if ((valluz()>negro+rango) || (valluz()<negro-rango))
                gira90(IZQ);
             Off(OUT_BC);
             while (((valluz()>negro+rango) || (valluz()<negro-rango))&&(valmax()>30)){
                OnFwdReg(OUT_BC, 55,OUT_REGMODE_SYNC); //25
             }
          }

          Off(OUT_BC);

      }
      Release(Cerrojo);
   }
}

task hacia_adelante()
{
   while(true){
     Acquire(Cerrojo);
     if(Control==0) { // Si hay que moverse
       TextOut(0, LCD_LINE1, "Voy a seguir");
       OnFwdReg(OUT_BC, velocidad,OUT_REGMODE_SYNC); // Hacia adelante
     }
     Release(Cerrojo);
   }
}

void corregir_trayectoria()
{
   Acquire(Cerrojo);
   TextOut(0, LCD_LINE2, "Voy a corregir"); // Texto por Pantalla
   if (Direccion==1) { // Gira a la Izquierda
      ResetRotationCount(OUT_B); // Contador de Giros a 0
      OnRev(OUT_C,velgiro); //Giramos sobre el mismo
      OnFwd(OUT_B,velgiro);
      // Mientras Blanco y No 90º
      while (((SENSOR_3>negro+rango) || (SENSOR_3<negro-rango)) && (MotorRotationCount(OUT_B)<Giros)) {}
      if (MotorRotationCount(OUT_B)>=Giros) { // Si llego a 90º
         Giros=Giros+(Giros*FactorGiro); // G180 Giramos hasta el otro extremo
         Direccion=1-Direccion; // Cambios de dirección
      }
      else {
         Off(OUT_BC);
         Giros=G90; // Inicializamos los giros
         Control=0; // Arranca Hacia Adelante
      }
   }
   else { // Gira a la Derecha
      ResetRotationCount(OUT_C); // Contador de Giros a 0
      OnFwd(OUT_C,velgiro);  //Giramos sobre el mismo
      OnRev(OUT_B,velgiro);
      // Mientras Blanco y No 90º
      while (((SENSOR_3>negro+rango) || (SENSOR_3<negro-rango)) && (MotorRotationCount(OUT_C)<Giros)) {}
      if (MotorRotationCount(OUT_C)>=Giros) { // Si llego a 90º
         Giros=Giros+(Giros*FactorGiro); // Giramos hasta el otro extremo
         Direccion=1-Direccion; // Cambios de dirección
      }
      else {
         Off(OUT_BC);
         Giros=G90; // Inicializamos los giros
         Control=0; // Arranca Hacia Adelante
      }
   }
   Release(Cerrojo);
}

task leer_sensor()
{
   while(true){
      Acquire(Cerrojo);
      if((SENSOR_3>negro+rango) || (SENSOR_3<negro-rango)) { // Si no lee Negro
          if(SENSOR_3>67){
            StopAllTasks();
         }
         Control=1; // Para Hacia Adelante

         NumOut(0, LCD_LINE3, SENSOR_3); // Imprimimos el Valor
         Release(Cerrojo);
         corregir_trayectoria();
         Acquire(Cerrojo);
      }
      else
      {
         Control=0; // Arranca Hacia Adelante

      }
      Release(Cerrojo);
   }
}

task main()
{
   SetSensorLight(S3);
   ResetSensor(USLateral);
   ResetSensor(USFrontal);
   SetSensorLowspeed(USLateral);
   SetSensorLowspeed(USFrontal);
   start hacia_adelante;
   start leer_sensor;
   start lee_ultrasonido;
}