/* MioRobot1.nqc      Dicembre 2005
/* Questo programma utilizza:
   Morore_A per far muovere il braccio di lancio
   Morore_B per far accendere la LEGOlamp (luce per sensore di prossimità)
   Morore_C per andare avanti e girare andando indietro
   Sensore_1 (contatto) per la mano da stringere
   Sensore_2 (luce) per leggere la luce riflessa (sensore di prossimità)
   Sensore_3 (contatto) per controllare il paraurti
*/

// Definizioni delle costanti del programma
#define LANCIO 227            // tempo di lancio
#define RETROMARCIA 300       // tempo di retromarcia
#define LUCE_BASSA 600        // valore basso della luce letta
#define LUCE_ALTA 720         // valore alto della luce letta
   
task main()
{
// Task principale. Configurazione sensori
  SetSensorType(SENSOR_1, SENSOR_TYPE_TOUCH);
  SetSensorMode (SENSOR_1, SENSOR_MODE_BOOL);
  SetSensorType(SENSOR_2, SENSOR_TYPE_LIGHT);
  SetSensorMode(SENSOR_2, SENSOR_MODE_RAW);
  SetSensorType(SENSOR_3, SENSOR_TYPE_TOUCH);
  SetSensorMode (SENSOR_3, SENSOR_MODE_BOOL);
// Task principale. Il robot inizia ad avanzare
  OnFwd(OUT_C);
// Task principale. Chiamata dei task
  start verifica_paraurti;
  start verifica_sensore_prossimita;
  start verifica_stretta_di_mano;
}

task verifica_paraurti()
/* Questo task verifica il paraurti.
   Il programma controlla in continuazione l'INPUT 3,
   se viene premuto il paraurti, il robot emette 4 beep
   e inverte la marcia per il tempo di RETROMARCIA
*/
{
  while(true)
  {
    if(SENSOR_3 == 1)
    {
      PlaySound (SOUND_DOUBLE_BEEP);
      PlaySound (SOUND_DOUBLE_BEEP);
      OnRev(OUT_C); Wait(RETROMARCIA);
      OnFwd(OUT_C);
    }
  }
}

task verifica_sensore_prossimita()
/* Questo task permette di realizzare un sensore di prossimità
   utilizzando un sensore di luce e LegoLamp.
   Il programma controlla in continuazione l'INPUT 2 (valore RAW)
   il valore della luce emesso da LegoLamp e letto dal sensore di
   luce; quando il valore RAW è compreso tra LUCE_BASSA e LUCE_ALTA
   (valori da impostare in ogni situazione), il robot emette 2 beep
   e inverte la marcia per il tempo di RETROMARCIA
      N.B. Per far funzionare il sensore di prossimità è necessario che
           l'illuminazione nel locale sia uniforme e i valori LUCE_BASSA
           e LUCE_ALTA siano impostati correttamente.
           In caso di malfunzionamenti, scollegare l'INPUT 2
           e l'OUTPUT B.
*/
{
  while(true)
  {
    OnFwd(OUT_B);
    // Se letto un valore compreso tra LUCE_BASSA e LUCE_ALTA
    // (ostacolo in avvicinamento)
    if ((SENSOR_2 >= LUCE_BASSA) && (SENSOR_2 <= LUCE_ALTA))
    {
      PlaySound (SOUND_DOUBLE_BEEP);
      OnRev(OUT_C); Wait(RETROMARCIA);
      OnFwd(OUT_C);
    }
  }
}

task verifica_stretta_di_mano()
/* Questo task verifica il braccio da stringere.
   Il programma controlla in continuazione l'INPUT 1,
   se viene stretta la mano, il robot emette un suono e effettua
   un 1° lancio; dopo 2 sec. effettua il 2° lancio
*/
{
  while(true)
  {
    if(SENSOR_1 == 0)
    {
      PlaySound (SOUND_UP);
      PlaySound (SOUND_DOWN);
      OnFwd(OUT_A); Wait(LANCIO);
      Off(OUT_A); Wait(100);
      PlaySound (SOUND_CLICK); Wait(100);
      PlaySound (SOUND_CLICK); Wait(100);
      PlaySound (SOUND_CLICK);
      OnFwd(OUT_A); Wait(LANCIO);
      Off(OUT_A);
    }
  }
}