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=5;
    movement=L;
}

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

  while(true)
  {
       if (THRESHOLD < RSENSOR) {line=R; dira=0;};
       if (THRESHOLD < LSENSOR) {line=L; dira=0;};
  }
}


task cara()
{
Left();
 while(true)
 {
   switch(line)
   {
      case S:
          if (3 < dira++)
          {
            PlaySound(SOUND_DOUBLE_BEEP);
          };
          break;
      case L:
          switch(movement)
          {
//            case S:
            case R:
                    Left();
                 break;
            case L:
                    Right();
                 break;
          };
          break;

      case R:
          switch(movement)
          {
//            case S:
            case L:
                    Right();
                 break;
            case R:
                    Left();
                 break;
          };
          break;
   };
   line=S;
   Wait(10);
 }
}

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