LEGO PATHFINDING

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

#define SENS_TOUCH IN_1
#define SENS_US IN_3
#define SENS_COLOR IN_4

bool button_pressed() {
  int pressed1, pressed2;
  pressed1 = Sensor(SENS_TOUCH);
  Wait(10);
  pressed2 = Sensor(SENS_TOUCH);
  return pressed1 && pressed2;
}

enum State { FORWARD, HALT, PATHFIND };

int current_state;
long angle = 0;

task read_color() {
  SetSensorColorFull(SENS_COLOR);

  int color;
  string color_name;

  while(true) {
    color = Sensor(SENS_COLOR);

    switch(color) {
    case INPUT_BLACKCOLOR:
      color_name = "Fekete";
      break;
    case INPUT_BLUECOLOR:
      color_name = "Kek";
      break;
    case INPUT_GREENCOLOR:
      color_name = "Zold";
      break;
    case INPUT_YELLOWCOLOR:
      color_name = "Sarga";
      break;
    case INPUT_REDCOLOR:
      color_name = "Piros";
      break;
    case INPUT_WHITECOLOR:
      color_name =  "Feher";
      break;
    }
    //if(color == INPUT_WHITECOLOR || color == INPUT_GREENCOLOR){
    if(color == INPUT_GREENCOLOR){
      current_state = FORWARD;
    } else {
      current_state = HALT;
      current_state = PATHFIND;
    }
  }
}

task engine() {
   while(true) {
    switch(current_state) {
      case FORWARD:
        OnRevSync(ENGINE_BOTH, 50, 0);
        angle = 0;
        break;
      case PATHFIND:
        RotateMotor(ENGINE_LEFT, 50, angle);
        RotateMotor(ENGINE_RIGHT, -50, angle);
        Wait(20);
        RotateMotor(ENGINE_LEFT, -50, (angle * 2));
        RotateMotor(ENGINE_RIGHT, 50, (angle * 2));
        angle += 10;
        break;
      case HALT:
        OnFwdSync(ENGINE_BOTH, 0, 0);
    }
  }
}

task rotate_count() {
  long rotate_count=0;
  ResetTachoCount(ENGINE_LEFT);
  while(true) {
    if(current_state == HALT) { break; }
    rotate_count += MotorTachoCount(ENGINE_LEFT);
    NumOut(0, LCD_LINE5, (rotate_count / -20), true);
  }
}

task main() {
  Precedes(rotate_count, read_color, engine);
}