Skip to content

Instantly share code, notes, and snippets.

@bonyiii bonyiii/vizsga
Created Oct 22, 2015

Embed
What would you like to do?
#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);
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.