Subversion Repositories svnkaklik

#define THRESHOLD  44      // rozhodovaci uroven mezi cernou a bilou
#define FTHRESHOLD 100     // Rozhodovaci uroven dalkoveho sensoru
#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   // Sensory na caru
#define LSENSOR    SENSOR_1
#define BUMPER     SENSOR_2   // Dalkovy sensor

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

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

void Straight()   // rovne
{
    FL;
    FR;
    movement=S;
}

void Left()    // doleva
{
    Off(ML);
    FR;
    movement=L;
}

void Right()   // doprava
{
    FL;
    Off(MR);
    movement=R;
}

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
  // sensor na prekazku
  SetSensor(BUMPER,_SENSOR_CFG(SENSOR_TYPE_LIGHT, SENSOR_MODE_RAW));
  SetTxPower(TX_POWER_HI);    // aby daleko videl

  SetPower (ML,OUT_FULL);     // vykon motoru
  SetPower (MR,OUT_FULL);
  movement=S;
  line=S;
  dira=0;

  start cara;
  start cihla;
}

task cara()              // sledovani cary
{
  Straight();
  while(true)
  {
       if (THRESHOLD < RSENSOR)    // Cara vpravo
       {
          line=R;    // cara zrejme bude na druhe strane
          dira=0;    // nuluj pocitadlo diry
          switch(movement)
          {
            case L:
               Right();
              break;
            default:
              Straight();
          };
       }
       else
       if (THRESHOLD < LSENSOR)    // Cara vlevo
       {
          line=L;    // zaznamenej, kdes videl caru
          dira=0;    // nuluj pocitadlo diry
          switch(movement)
          {
            case R:
               Left();
              break;
            default:
              Straight();
          };
       }
       else   // sensory mimo caru
       {
         if (20 < dira++)  // nejedeme uz moc dlouho bez cary?
         {
           PlaySound (SOUND_FAST_UP);
           switch(movement)
           {
             case L:
                line=R;   // cara zrejme bude na druhe strane
                Off(ML);  //couvej
                BR;
               break;
             case R:
                line=L;   // cara zrejme bude na druhe strane
                BL;       // couvej
                Off(MR);
               break;
           };
           Wait(50);   // zpet, ale ne uplne a kousek rovne
           Straight();
           Wait(5);
           dira=0;    // myslime si, ze diru jsme vyresili
         };

         switch(line)  // kdyz nevidis caru, tak jed tam, kdes ji videl naposled
         {
           case L:
                Left();
             break;
           case R:
                Right();
             break;
         };
       }
  } // while(true)
}


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

  lastlevel = 0;
  while(true)
  {
    SendMessage(0);
    if(lastlevel > BUMPER)
    {
      PlaySound(SOUND_DOWN);
      stop cara;
      Off(ML);           // zatim pouze zastav
      Off(MR);
      Wait(300);
      start cara;
    }
    lastlevel = BUMPER;
    lastlevel -= FTHRESHOLD;
  }
}