Subversion Repositories svnkaklik

// Robot na MiniSumo pro Model Hobby 2004
// $Archive: /Lego/Matador/ModelHobby2004.nqc $
// $Date: 16.03.05 10:47 $
// $Revision: 1 $

#define THRESHOLD  65      // rozhodovaci uroven mezi cernou a bilou
#define FTHRESHOLD 100     // Rozhodovaci uroven dalkoveho sensoru
#define POHON      OUT_A   // Motory
#define MULETA     OUT_C
#define VPRED      OnFwd(POHON)
#define VZAD       OnRev(POHON)
#define NAHORU     OnRev(MULETA)
#define DOLU       OnFwd(MULETA)
#define PREDNI     SENSOR_1   // Sensory na okraj
#define ZADNI      SENSOR_3
#define BUMPER     SENSOR_2   // Dalkovy sensor

task main()
{
  PlaySound (SOUND_DOUBLE_BEEP);
  Wait(400); // 5s podle pravidel - prvni prodleva procedury zapas
  SetSensor(PREDNI,SENSOR_LIGHT);   // senzor na caru predni
  SetSensor(ZADNI,SENSOR_LIGHT);   // senzor na caru zadni
  // sensor na prekazku
  SetSensor(BUMPER,_SENSOR_CFG(SENSOR_TYPE_LIGHT, SENSOR_MODE_RAW));

  SetTxPower(TX_POWER_HI);    // aby daleko videl

  SetPower (POHON,OUT_FULL);     // vykon motoru
  SetPower (MULETA,OUT_HALF);

  start radar;
  start zapas;
  start nevypadni;
  start toro;
}

task toro()
{
  int i=1;

  while(true)
  {
     if (i++ & 1) {DOLU;} else {NAHORU;};
     Wait(50);
     Off(MULETA);
     Wait(200);
  }
}

task zapas()
{
  Wait(100);       // napodruhe pojedeme dele dozadu
  while(true)
  {
     VZAD;         // mateni telem
     Wait(50);
     Off(POHON);
     Wait(100);
     VPRED;
     Wait(50);
     Off(POHON);
     Wait(100);
     Off(POHON);
     Wait(100);
  }
}

task nevypadni()
{

  while(true)
  {
     if (THRESHOLD > PREDNI)    // Cara vpredu
     {
        stop zapas;
        stop radar;
        PlaySound (SOUND_FAST_UP);
        VZAD;
        Wait(50);
        start radar;
        start zapas;
     }
     else
     if (THRESHOLD > ZADNI)    // Cara vzadu
     {
        stop zapas;
        stop radar;
        PlaySound (SOUND_FAST_UP);
        VPRED;
        Wait(50);
        start radar;
        start zapas;
     }
  } // while(true)
}

task radar() // dalkovy sensor
{
  int lastlevel;

  lastlevel = 0;
  while(true)
  {
    SendMessage(0);
    if(lastlevel > BUMPER)  // vidime robota?
    {
      stop zapas;
      PlaySound(SOUND_DOWN);
      VPRED;
      Wait(50);
    }
    lastlevel = BUMPER;
    lastlevel -= FTHRESHOLD;
  }
}