Skip to content

Instantly share code, notes, and snippets.

@bonyiii
Created October 1, 2015 19:10
Show Gist options
  • Save bonyiii/f92586e6902ffa261386 to your computer and use it in GitHub Desktop.
Save bonyiii/f92586e6902ffa261386 to your computer and use it in GitHub Desktop.
lego
#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);
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment