is-lv5

/* 1. zadatak
 */

void naprijed(int x)
{
       OnFwd(OUT_A + OUT_C);
       Wait(x);
       //Off(OUT_A + OUT_C);
}

/* 2. zadatak
 */
void pomak_lijevo(int x)
{
      OnFwd(OUT_C);
      Wait(x);
      //Off(OUT_C);
}

/* 3. zadatak
 */
void pomak_desno(int x)
{
      OnFwd(OUT_A);
      Off(OUT_C);
      Wait(x);
}

void pomak_desno_rev(int x)
{
  Off(OUT_C);
  OnRev(OUT_A);
  Wait(x);
}

/* 4. zadatak
 */
void okret_lijevo(int x)
{
      OnFwd(OUT_C);
      OnRev(OUT_A);
      Wait(x);
      //Off(OUT_A + OUT_C);
}
/* 5. zadatak
 */
void okret_desno(int x)
{
      OnFwd(OUT_A);
      OnRev(OUT_C);
      Wait(x);
      //Off(OUT_A + OUT_C);
}


/*6.
Napisati kod za oèitavanje optièkog senzora u tzv. Row obliku i funkciju koja oèitava
vrijednost optièkog senzora
*/
void opticki_senzor()
{
     SetSensorType(SENSOR_1, SENSOR_TYPE_LIGHT);
     SetSensorMode(SENSOR_1, SENSOR_MODE_RAW);
     int x =  SENSOR_1;  //vrijednost optickog senzora spremljena u varijablu x
}

/* Napisati kod za oèitavanje senzora dodira u tzv. Bool obliku i funkciju koja oèitava
vrijednost senzora dodira.
*/
void touch_senzor(){
     SetSensorType(SENSOR_1,SENSOR_TYPE_TOUCH);
     SetSensorMode(SENSOR_1, SENSOR_MODE_BOOL);
     int x =  SENSOR_1;  //vrijednost touch senzora spremljena u varijablu x
}

void nazad(int x)
{
     OnRev(OUT_A + OUT_C);
     Wait(x);
     //Off(OUT_A + OUT_C);
}

task main()
{
/*
  Naprijed(100);    //100 sekundi
  Pomak_lijevo(85);
  Pomak_desno(85);
  Okret_lijevo(85);   //85 sekundi - 90 stupenjva
  Okret_desno(85);

  Opticki_senzor();
  Touch_senzor();

  // 8. zadatak
  Naprijed(500);      //kretanje naprijed 5s
  Pomak_lijevo(300);  //kretanje uljievo 3s
  Pomak_desno(300);   //kretanje udesno 3s
  Okret_lijevo(200);  //okret ulijevo 2s
  Okret_desno(200);   //okret udesno 2s
  Nazad(300);            //kretanje unazad 3s
  */
  //Naprijed(215);

  /*
 SetSensor(SENSOR_2,SENSOR_LIGHT);
 SetSensorMode(SENSOR_2,SENSOR_MODE_RAW);
 int x = 0;
 int d = 0;
 while(1)
 {
   if(SENSOR_2>790){
     if (x == 0 && ++d >= 5) {
       okret_lijevo(3);
     } else {
       if(x == 1) {
         x = 0;
         d = 0;
       }
       pomak_lijevo(3);
     }
   } else{
     if (x == 1 && ++d >= 5) {
       okret_desno(3);
     } else {
       if(x == 0) {
         x = 1;
         d = 0;
       }
       pomak_desno(3);
     }
   }
  }
  */
   SetSensorType(SENSOR_3,SENSOR_TYPE_TOUCH);
   SetSensorMode(SENSOR_3, SENSOR_MODE_BOOL);
   SetSensorType(SENSOR_1,SENSOR_TYPE_TOUCH);
   SetSensorMode(SENSOR_1, SENSOR_MODE_BOOL);

   int i = 0;
   int mode = 0;
   int j = 0;
   // mode 0 - naprijed
   // mode 1 - pomak desno
   while(1) {
       if (SENSOR_1 == 1) {
        nazad(20);
        okret_lijevo(10);
        j = 0;
      } else if (SENSOR_3 == 1) {
        if (mode == 1 && SENSOR_1 == 0) {
           while (i-- > 0) {
             pomak_desno_rev(3);
           }
           mode = 0;
        } else {
            nazad(20);
            okret_lijevo(10);
            i = 0;
        }
        j = 0;
      } else if (mode == 0 && ++i < 8) {
        if (j >= 3) {
          okret_desno(3);
        } else {
          naprijed(3);
        }
      } else {
        if (mode == 0) {
          mode = 1;
          i = 0;
          if (j >=3) {
             j = 0;
          }
        }
        pomak_desno(6);
        if (++i >= 4) {
          mode = 0;
          i = 0;
          ++j;
        }
      }
   }
}