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

}