Subversion Repositories svnkaklik

// Soutezni robot pro Istrobot 2004
// $Archive: /Lego/Callis/Callis_log.nqc $
// $Date: 9.05.04 6:34 $
// $Revision: 1 $

#define THRESHOLD  50         // rozhodovaci uroven mezi cernou a bilou
#define ML         OUT_A      // Motory
#define MR         OUT_C
#define FL         OnFwd(ML)  // Vpred
#define FR         OnRev(MR)
#define BL         OnRev(ML)  // Vzad
#define BR         OnFwd(MR)
#define RSENSOR    SENSOR_3   // Senzory na caru
#define LSENSOR    SENSOR_1
#define BUMPER     SENSOR_2   // Senzor na cihlu
#define MEZERA     38         // jak se muze jet dlouho bez cary
#define COUVEJ     70         // kolik se ma couvat po detekci diry
#define CIKCAK     40         // kolik se ma jet cik/cak
#define PRES_DIRU  35         // jak predpokladame velkou diru

#define L 0  // left
#define R 1  // right
#define S 2  // straight

#define STOP    Off(ML + MR); Wait(10);    // zastav  8

int movement;     // smer minuleho pohybu
int line;         // na ktere strane byla detekovana cara
int dira;         // pocitadlo pro nalezeni preruseni cary
int n;            // pomocna promena pro cyklus FOR

task main()
{
  PlaySound (SOUND_DOUBLE_BEEP);
  Wait(100); // 1s
  SetSensor(RSENSOR,SENSOR_LIGHT);   // senzor na caru modry pravy
  SetSensor(LSENSOR,SENSOR_LIGHT);   // senzor na caru modry levy
  SetSensor(BUMPER,SENSOR_TOUCH);    // sensor na prekazku
  SetEvent(L, LSENSOR, EVENT_TYPE_HIGH);
  SetEvent(R, RSENSOR, EVENT_TYPE_HIGH);

  SetPower (ML,OUT_HALF);     // vykon motoru
  SetPower (MR,OUT_HALF);
  movement=R;
  line=S;
  dira=0;
  
  cikcak();    // toc se, abys nasel caru
  FL; FR;      // vpred
  
  CreateDatalog(1000);

  // Sledovani cary
  while(true)
  {

    monitor( EVENT_MASK(L) | EVENT_MASK(R) )
    {
      FL; FR;    // rovne
    }
    catch (EVENT_MASK(L))
    {
      line=L;    // zaznamenej, kdes videl caru
      n=-dira;
      AddToDatalog(n);
      dira=0;    // nuluj pocitadlo diry, protoze jsme videli caru
    }
    catch (EVENT_MASK(R))
    {
      line=R;    // zaznamenej, kdes videl caru
      n=dira;
      AddToDatalog(n);
      dira=0;    // nuluj pocitadlo diry, protoze jsme videli caru
    }

    // oba senzory mimo caru

    if (0==dira) // v prvnim cyklu po ztrate cary zacneme zahybat
                 // v ostatnich cyklech nedelame nic (pro urychleni snimani)
    {
       switch(line)  // kdyz nevidis caru, tak jed tam, kdes ji videl naposled
       {
         case L:
              Off(ML);       // doleva
              movement=L;    // poznamenej kam jedes
           break;
         case R:
              Off(MR);       // doprava
              movement=R;    // poznamenej kam jedes
           break;
       };
       dira++;   // zvets pocitadlo diry, aby to pristi cyklus tudy neslo
       continue; // co nejrychleji se vrat na snimani cary
    }

    if (MEZERA < dira++)  // nejedeme uz moc dlouho bez cary?
    {
      dira=0;
      PlaySound (SOUND_FAST_UP);
      switch(movement)  // kam se jelo ?
      {
       case L:
            Off(ML);  // couvej doprava
            BR;
            Wait(COUVEJ+10);
            movement=R;
         break;
       case R:
            BL;       // couvej doleva
            Off(MR);
            Wait(COUVEJ);
            movement=L;
         break;
      }
      PlaySound(SOUND_CLICK);
      STOP;                    // konec couvani
      FL; FR;                  // rovne pres diru
      Wait(PRES_DIRU);
      STOP;
      cikcak();                // najdi caru
      FR; FL;                  // vpred
    } // dira
    
  } // while(true)
}


void cikcak()   // Hledani cary
{
  n=CIKCAK/2;       // poprve hned zatoc opacne, nez se jelo pred detekci diry
  switch(movement)  // zmenime smer zataceni
  {
   case L:
        FL; BR;       // doprava
        movement=R;        // poznamenej kam jedem
        line=L;            // kdyz prejedem, tak bude cara vlevo
     break;
   case R:
        FR; BL;       // doleva
        movement=L;        // poznamenej kam jedem
        line=R;            // kdyz prejedem, tak bude cara vpravo
     break;
  }

  while(true)   // jed cik-cak, dokud nenajdes caru
  {
    if ((THRESHOLD < RSENSOR) || (THRESHOLD < RSENSOR))   // je cara ?
    {
         STOP;
         break;
    };
    if (CIKCAK < n++)  // Jedeme uz dost dlouho cik? Pak jed cak.
    {
       n=0;
       STOP;
       switch(movement)     // zmenime smer zataceni
       {
         case L:
              FL; BR;       // doprava
              movement=R;        // poznamenej kam jedem
              line=L;            // kdyz prejedem, tak bude cara vlevo
           break;
         case R:
              FR; BL;       // doleva
              movement=L;        // poznamenej kam jedem
              line=R;            // kdyz prejedem, tak bude cara vpravo
           break;
       }
    }
  }
}