#define S1_ON_TRACK SENSOR_1 > black - treshold //Egyes szemzor a pálya felett van #define S3_ON_TRACK SENSOR_3 > black - treshold //Hármas szemzor a pálya felett van #define ON_THE_TRACK S3_ON_TRACK && !(S1_ON_TRACK) //A robot a pályán van #define TURN_90_TIME 75 //90°-os fordulathoz szükséges idő #define COLL_FWD_T 80 //Két fal melletti "tapogatózás" közti idő int black, white, treshold; //Kezdeti kalibráláshoz szükséges változók int searching; sub calibrate() //Színek bekalibrálása { black = SENSOR_3; //A 3-as szenzor alatti a fekete white = SENSOR_1; //Az 1-es szenzor alatti a fehér treshold = abs(black - white) / 2; //A kettő közti részt megfelezzük //efelett pályának tekintjük, alatta nem } void turn_right() //Jobbra fordulás { OnFwd(OUT_A); OnRev(OUT_C); } void turn_left() //Balra fordulás { OnFwd(OUT_C); OnRev(OUT_A); } void forward() //Egyenesen előre { OnFwd(OUT_A+OUT_C); } void reverse() //Hátra { OnRev(OUT_A+OUT_C); } void halt() //Megállás { Off(OUT_A+OUT_C); } void turn_90_left() //90°-os fordulat balra { turn_left(); //Balra fordulunk Wait(TURN_90_TIME); //a megfelelő ideig halt(); } void turn_90_right() //90°-os fordulat balra { turn_right(); //Jobbra fordulunk Wait(TURN_90_TIME); //a megfelelő ideig halt(); } task follow_track() //Pálya követése { start collision; //Figyeljük az ütközést while(true) { forward(); //Előre until(!(ON_THE_TRACK)); //amíg a pályán vagyunk if(!(S1_ON_TRACK)) //Ha balra tértünk le { turn_right(); //jobbra fordulunk } else //Különben (ha jobbra) pedig { turn_left(); //balra } until(ON_THE_TRACK); //míg újra nem leszünk rajt a pályán } } task collision() //Ütközés detektálása és akadály megkerülése { searching = 0; //Még nem keressük a pályát while(true) { until(SENSOR_2 == 1); //Ha ütközünk stop follow_track; //abbahagyjuk a pálya követését start blinking; //és villogással jelezzük, hogy kerülünk do { reverse(); //Visszatolatunk Wait(10); //egy kicsit halt(); turn_90_right(); //Jobbra 90° forward(); //Előre Wait(COLL_FWD_T); //"tapogatózási-időnyit" if(searching == 0) //Ha még nem keressük a pályát { searching = 1; //akkor jelezzük, hogy mostantól keressük start search_track; //és el is indítjuk } turn_90_left(); forward(); ClearTimer(0); until(SENSOR_2 == 1 || Timer(0)>=5); //Ha ütközünk vagy //½ másodpercig tudunk egyenesen haladni halt(); //akkor megállunk } while(SENSOR_2 == 1); //Amíg ütközés miatt állunk meg //Ha nem, akkor túljutottunk az első sarkon turn_90_left(); forward(); //Előre, while(true) { until(SENSOR_2 == 1); //amíg nem ütközünk halt(); reverse(); //Tolassunk Wait(10); //egy kicsit halt(); turn_90_right(); //Jobbra 90° forward(); //Előre Wait(COLL_FWD_T); //egy tapogatózásnyit turn_90_left(); forward(); } } } task search_track() //Pálya keresése { until(!(S1_ON_TRACK) && !(S3_ON_TRACK)); //Amíg mindkét szenzor nincs a pálya fölött stop collision; //Akadálykikerülés leállítása stop blinking; //Villogás leállítása Off(OUT_B); //A lámpa biztosan legyen kikapcsolva halt(); start follow_track; //Kövessük a pályát (mivel mindkét érzékelő a pálya fölött van, jobbra fogunk fordulni) } task blinking() //Villogás { while(true) { On(OUT_B); //Lámpa be Wait(25); //¼ másodpercig marad így Off(OUT_B); //aztán kikapcs Wait(25); //szintén ¼ másodpercig } } task main() { SetSensor(SENSOR_1, SENSOR_LIGHT); //Kezdeti SetSensor(SENSOR_2, SENSOR_TOUCH); //szenzor SetSensor(SENSOR_3, SENSOR_LIGHT); //beállítások ClearSensor(SENSOR_1); //Biztos ami biztos ClearSensor(SENSOR_2); //a szenzorok ClearSensor(SENSOR_3); //nullázása calibrate(); //Kezdeti kalibrálás start follow_track; //Kezjünk el menni a pályán }