lego

gistfile1.txt

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

#define SENS_TOUCH IN_4
#define SENS_US IN_2
#define SENS_COLOR IN_1

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

enum State { FORWARD, TURN };

int current_state;

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;
    }
  TextOut(0, LCD_LINE5, color_name, true);
  }
}

task read_us() {
  SetSensorLowspeed(SENS_US);

  int us;

  while(true) {
    us = SensorUS(SENS_US);

    if (us <= 50) {
       current_state = TURN;
    } else {
       current_state = FORWARD;
    }
  }
}

task engine() {
   while(true) {
    switch(current_state) {
      case FORWARD:
        OnRev(ENGINE_BOTH, 100);
        break;
      case TURN:
        OnFwd(ENGINE_LEFT, 100);
        break;
    }
  }
}

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