Subversion Repositories svnkaklik

// Soutezni robot pro Istrobot 2004
// Objeti cihly vpravo
// $Archive: /Lego/Callis/Callis_R.nqc $
// $Date: 27.04.04 20:52 $
// $Revision: 3 $

#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


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;
       }
    }
  }
}

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

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

  // Sledovani cary
  while(true)
  {
    if(BUMPER)  // je cihla?
    {
      PlaySound(SOUND_DOWN);
      objizdka();     // objed cihlu
    }

    if (THRESHOLD < RSENSOR)    // Cara pod pravym senzorem
    {
      dira=0;    // nuluj pocitadlo diry, protoze jsme videli caru
      line=R;    // zaznamenej, kdes videl caru
      FL; FR;    // rovne
      continue;
    }

    if (THRESHOLD < LSENSOR)    // Cara pod levym senzorem
    {
      dira=0;    // nuluj pocitadlo diry, protoze jsme videli caru
      line=L;    // zaznamenej, kdes videl caru
      FL; FR;    // rovne
      continue;
    }

    // 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 objizdka()
{
  BL; BR;                 // zacouvej
  Wait(15);
  STOP;

  cikcak();               // vyrovnej se na caru
  cikcak();               // radeji 3x, aby se vyrovnaly setrvacne sily
  cikcak();
  
  FL; BR; Wait(23);               // vpravo
  STOP;
  FL; FR; Wait(65);               // rovne
  STOP;
  BL; FR; Wait(23);               // vlevo
  STOP;
  FL; FR; Wait(75);               // rovne
  STOP;
  BL; FR; Wait(23);               // vlevo
  STOP;
  FL; FR;
  Wait(20);                       // kousek rovne
  // rovne, dokud nenarazis na caru
  while(THRESHOLD > LSENSOR);     // levej senzor, protoze cihla je levo
  STOP;
  FL; BR;                         // toc se doprava, dokud nenarazis na caru
  while(THRESHOLD > RSENSOR);     // pravej senzor, protoze cara je urcite vpravo
  STOP;
  line=L;      // caru predpokladame vlevo, kdybysme ji prejeli
  dira=0;
  FL; FR;                         // vpred
}