vizsga

#define ENGINE_LEFT OUT_A
#define ENGINE_RIGHT OUT_C
#define ENGINE_BOTH OUT_AC

#define SENS_TOUCH IN_2
#define SENS_US_FRONT IN_3
#define SENS_US_LEFT IN_1
#define SENS_COLOR IN_4

int us_left_was, us_left_new;

task read_us() {
  SetSensorLowspeed(SENS_US_LEFT);
  while(true) {
    us_left_was = SensorUS(SENS_US_LEFT);
    Wait(100);
    us_left_new = SensorUS(SENS_US_LEFT);
  }
}

task maze() {
  SetSensorLowspeed(SENS_US_FRONT);
  SetSensorLowspeed(SENS_US_LEFT);
  SetSensorTouch(SENS_TOUCH);

  int us_front, us_left, pressed;
  
  while(true) {
    us_left = SensorUS(SENS_US_LEFT);
    us_front = SensorUS(SENS_US_FRONT);
    pressed = Sensor(SENS_TOUCH);
    NumOut(0, LCD_LINE5, us_left_new, true);
    NumOut(0, LCD_LINE2, us_left_was, false);

    OnRevSync(ENGINE_BOTH, 100, 0);

    if(pressed) {
      OnFwdSync(ENGINE_BOTH, 60, 0);
      Wait(800);
      OnRevSync(ENGINE_BOTH, 100, 100);
      Wait(800);
    }
    
    if(us_front < 15) {
      OnFwdSync(ENGINE_BOTH, 60, 0);
      Wait(800);
      OnRevSync(ENGINE_BOTH, 50, 100);
      Wait(800);
    }


    if(us_left < 30) {
      if( us_left_was - us_left_new > 0 && us_left_was - us_left_new < 20) {
        OnRevSync(ENGINE_BOTH, 100, -20);
        Wait(200);
      } else {
        if(us_left_new - us_left_was > 0 && us_left_new - us_left_was < 20) {
          OnRevSync(ENGINE_BOTH, 100, 20);
          Wait(200);
        }
      }
    }

    if(us_left > 60 && us_left < 200) {
      Wait(400);
      OnRevSync(ENGINE_BOTH, 100, 100);
      Wait(800);
    }

  }

}

task main() {
  SetSensorTouch(SENS_TOUCH);
  while(!Sensor(SENS_TOUCH)) {}
  
  Wait(1000);
  Precedes(maze, read_us);
}