labyrinth.nxc

int sonar=0;

void backup() {
  OnRev(OUT_BC, 50);
  Wait(200);
}
void pivotRight(int speed, int time) {
  OnFwd(OUT_C, speed);
  Wait(time);
  Coast(OUT_B);
  Coast(OUT_C);
}
task main() {
    SetSensorTouch(S1);
    SetSensorTouch(S2);
    SetSensorLowspeed(S4);
    SetSensorLight(IN_3);
    bool barva=true;
    while (barva) {
      OnFwd(OUT_B, 60);
      OnFwd(OUT_C, 60);
      if(Sensor(IN_3)>53){
        Wait(100);
        barva=false;
      }
      if (Sensor(S1) == 1 && Sensor(S2) == 0) {
        OnFwd(OUT_B, 20);
      }
      if (Sensor(S2) == 1 && Sensor(S1) == 0) {
        TextOut(0, LCD_LINE1, "sync", true);
        OnFwd(OUT_C, 20);
      }
      if (Sensor(S2) == 1 && Sensor(S1) == 1) {
        backup();
        pivotRight(50, 500);
        Wait(500);
        sonar = SensorUS(S4);
        Wait(500);
        if (sonar < 24) {
          TextOut(0, LCD_LINE1, "wrongway");
          NumOut(1, LCD_LINE2, sonar);
          backup();
          Coast(OUT_B);
          Coast(OUT_C);
          Wait(1000);
          //otoceni o 180 stupnu
          pivotRight(100, 800);
          Coast(OUT_B);
          Coast(OUT_C);
          Wait(1000);
        }
      }
    }
}