Subversion Repositories svnkaklik

#define THRESHOLD  44     // rozhodovaci uroven mezi cernou a bilou
#define FTHRESHOLD 100    // Rozhodovaci uroven dalkoveho sensoru
#define MLEFT     OUT_C   // Motory
#define MRIGHT    OUT_A
#define RSENSOR   SENSOR_1   // Sensory na caru
#define LSENSOR   SENSOR_3
#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
int derivace;     // jak moc se bude zachovavat predesly smer

void Straight()   // rovne
{
    OnRev(MLEFT);
    OnFwd(MRIGHT);
    movement=S;
}

void Left()    // doleva
{
    Off(MLEFT);
    OnFwd(MRIGHT);
    movement=L;
}

void Right()   // doprava
{
    OnRev(MLEFT);
    Off(MRIGHT);
    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

  movement=S;
  line=S;
  dira=0;

  start cara;
  start cihla;
  SetPower (MLEFT,OUT_FULL);
  SetPower (MRIGHT,OUT_FULL);
}

task cara()
{
  Straight();
  while(true)
  {
       if (THRESHOLD < RSENSOR)
       {
          line=R;
          dira=0;
          switch(movement)
          {
            case L:
               Right();
              break;
            default:
              Straight();
          };
       }
       else
       if (THRESHOLD < LSENSOR)
       {
          line=L;
          dira=0;
          switch(movement)
          {
            case R:
               Left();
              break;
            default:
              Straight();
          };
       }
       else
       {
       if (20 < dira++)
       {
         PlaySound (SOUND_FAST_UP);
         switch(movement)
         {
            case L:
               line=R;
               Off(MLEFT);     //couvej
               OnRev(MRIGHT);
              break;
            case R:
               line=L;
               OnFwd(MLEFT);    // couvej
               Off(MRIGHT);
             break;
         };
         Wait(50);
         Straight();
         Wait(5);
         dira=0;
       };

       switch(line)
       {
          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(MLEFT);           // stop
      Off(MRIGHT);
      Wait(5);
      OnRev(MLEFT);           // do leva
      OnRev(MRIGHT);
      Wait(30);
      Off(MLEFT);           // stop
      Off(MRIGHT);
      Wait(5);
      OnRev(MLEFT);           // rovne
      OnFwd(MRIGHT);
      Wait(70);
      Off(MLEFT);           // stop
      Off(MRIGHT);
      Wait(5);
      OnFwd(MLEFT);           // do prava
      OnFwd(MRIGHT);
      Wait(25);
      Off(MLEFT);           // stop
      Off(MRIGHT);
      Wait(5);
      OnRev(MLEFT);           // rovne
      OnFwd(MRIGHT);
      Wait(80);
      Off(MLEFT);           // stop
      Off(MRIGHT);
      Wait(5);
      OnFwd(MLEFT);           // do prava
      OnFwd(MRIGHT);
      Wait(25);
      Off(MLEFT);           // stop
      Off(MRIGHT);
      Wait(5);
      OnRev(MLEFT);           // rovne
      OnFwd(MRIGHT);
      Wait(60);
      OnRev(MLEFT);           // do leva
      OnRev(MRIGHT);
      Wait(20);
      while(THRESHOLD > RSENSOR); // toc se dokud neni cara
      Off(MLEFT);           // stop
      Off(MRIGHT);
      Wait(30);
      line=R;
      start cara;
    }
    lastlevel = BUMPER;
    lastlevel -= FTHRESHOLD;
  }
}