Skip to content

Instantly share code, notes, and snippets.

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