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
{
    SetPower (MLEFT,OUT_FULL);
    SetPower (MRIGHT,OUT_FULL);
    OnRev(MLEFT);
    OnFwd(MRIGHT);
    derivace=5;
    movement=S;
}

void Left()    // doleva
{
    SetPower (MLEFT,OUT_LOW);
    SetPower (MRIGHT,OUT_FULL);
    Off(MLEFT);
    OnFwd(MRIGHT);
    derivace=3;
    movement=L;
}

void Right()   // doprava
{
    SetPower (MLEFT,OUT_FULL);
    SetPower (MRIGHT,OUT_LOW);
    OnRev(MLEFT);
    Off(MRIGHT);
    derivace=3;
    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;

  Straight();
  while(true)
  {
       if (THRESHOLD < RSENSOR)
       {
           line=R;
           dira=0;
           Right();
           if (THRESHOLD < RSENSOR)
           {
             while(THRESHOLD < RSENSOR);
           }
           else

       };
       if (THRESHOLD < LSENSOR)
       {
         if (L==movement)
         {
            Straight();
         }
         else
         {
           line=L;
           dira=0;
           Left();
         };
       };
  }
}


task cara()
{
Left();
 while(true)
 {
          if (8 < dira++)
          {
            PlaySound(SOUND_DOUBLE_BEEP);
            if (L==line) line=R; else line=L;
            Off(MLEFT);
            Off(MRIGHT);
            Wait(500);
            dira=0;
          };

   switch(line)
   {
      case S:
          break;
      case L:
            Left();
          break;

      case R:
            Right();
          break;
   };
//   line=S;
   Wait(5);
   Straight();
   Wait(5);
 }
}

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

  lastlevel = 0;
  while(true)
  {
    SendMessage(0);
    if(lastlevel > BUMPER)
    {
      PlaySound(SOUND_FAST_UP);
      stop cara;
      Off(MLEFT);
      Off(MRIGHT);
      Wait(300);
      start cara;
    }
    lastlevel = BUMPER;
    lastlevel -= FTHRESHOLD;
  }
}