/* CaneRobot1.nqc      Febbraio 2005
   Questo programma utilizza:
   Morore_A per andare avanti e girare andando indietro
   Morore_B per far muovere le tenaglie
   Morore_C per far muovere la bocca e le orecchie
   Sensore_1 (contatto) per controllare il paraurti
   Sensore_2 (non collegato)
   Sensore_3 (luce) per sensore di prossimità IR
*/
   
int ultimoValore;   // Memorizza il valore di luce  precedente

task main()
{
  // Setta TX power alta per far funzionare il sensore di prossimità IR
  SetTxPower (TX_POWER_HI);
  SetSensorType(SENSOR_1, SENSOR_TYPE_TOUCH);
  SetSensorMode (SENSOR_1, SENSOR_MODE_BOOL);
  SetSensorType(SENSOR_3, SENSOR_TYPE_LIGHT);
  SetSensorMode(SENSOR_3, SENSOR_MODE_RAW);

  OnRev(OUT_A);
  OnFwd(OUT_B);
  start trasmetti_segnale_IR;
  start verifica_segnale_IR;
  start verifica_paraurti;

}

task trasmetti_segnale_IR()
{
  while(true)
    {SendMessage(0); Wait(10);}
}

task verifica_segnale_IR()
{
  while(true)
  {
    ultimoValore = SENSOR_3;
    if(SENSOR_3 > ultimoValore + 150)
    {
      PlaySound (SOUND_DOUBLE_BEEP);
      Off(OUT_B);
      OnFwd(OUT_A); Wait(150);
      OnRev(OUT_A);
      OnFwd(OUT_B);
    }
  }
}

task verifica_paraurti()
{
  while(true)
  {
    if(SENSOR_1 == 1)
    {
      PlaySound (SOUND_DOUBLE_BEEP);
      PlaySound (SOUND_DOUBLE_BEEP);
      Off(OUT_B);
      apri_bocca();
      OnFwd(OUT_A); Wait(300);
      OnRev(OUT_A);
      OnFwd(OUT_B);
    }
  }
}



sub apri_bocca()
{
  while(true)
  {
  OnRev(OUT_C); Wait(20);
  Off(OUT_C);
  OnFwd(OUT_C); Wait(20);
  Off(OUT_C);
  }
}