ROBOTIKA 1. BEADANDÓ
2001/2002 tanév 1. félév

Kanyarkövetés dokumentáció

Hegedűs Hajnalka – Gasparin Balázs – Ali György

A feladat

Olyan robotot kellett készítenünk, amely képes követni tetszőleges alakú kanyarokat tartalmazó vonalat egy sík terepen. A vonal szélessége tetszőleges lehet. A programnak alkalmazkodnia kell a vonal színéhez és a mindenkori fényviszonyokhoz.

A program vázlata

Először a mozgatáshoz szükséges makrókat definiáltuk. Utána következett a kalibrálásért felelős rész, majd pedig a vezérlés egy végtelen ciklusban.

A robot

A robotot két motor hajtja, az egyik az oldalirányú, a másik az előre-hátra mozgatásért felel. A mozgást differenciálmű és lánckerék alkalmazásával oldottuk meg.

A tájékozódáshoz a robot három fényérzékelőt használ, ezek közül a középső a követendő vonal fölött, a két szélső pedig két oldalt, a vonal melletti rész fölött kell, hogy legyen. A robot folyamatosan ellenőrzi az érzékelők állapotát, és ha valamelyik elhagyja a neki kijelölt tartományt, a robot iránykorrekciót hajt végre.

A robot mozgatása

Négyféle mozgást definiáltunk: hátra, előre, jobbra, balra mozgásokat. Az A motor végzi az előre-hátra mozgatást, a B az oldalirányú forgatást.

Szenzorok, kalibráció

Három szenzort használtunk, amik a robot elején helyezkedtek el. A kalibráció úgy történik, hogy a robot mindkét irányba elfordul, és közben mindhárom szenzorra megméri a minimális és a maximális értéket.

Azért volt szükséges mindhárom érzékelő kalibrálása, mert nem biztos, hogy azonos fényességet azonos értékkel reprezentálnak. A mért minimum és maximum közti tartományt három részre osztottuk. A program azt ellenőrzi, hogy az aktuális fényérték nagyobb vagy kisebb-e, mint a konstansban megadott tartományhatárok. Pl. a KOZEPS makró írja le azt a feltételt, hogy mikortól tekintünk sötétnek egy színt a középső érzékelőn, a KOZEPV pedig, hogy mikor tekintjük világosnak. A két érték közötti tartomány az esetleges becsillanások vagy a kocsi árnyéka által okozott fényerőcsökkenésből adódó tévedések kiküszöbölésére szolgál.

A kalibrációnál először a jobb és a középső érzékelőket állítjuk be. Ezt úgy tesszük, hogy addig fordul balra a robot, amíg a maximum és a minimum közti eltérés nem lesz nagyobb 5-nél. Majd ugyanezzel a módszerrel a bal érzékelőt is kalibráljuk.

Vezérlés

A vezérléshez az egyszerű reaktív modellt választottuk. Alapértelmezésben a robot előre megy, az irányváltoztató manővereket feltételek váltják ki. Ha a manővert kiváltó feltétel hamissá válik, akkor a robot visszaáll az előre haladó állapotba.

A manőverek:

• Tolatás: akkor van szükség rá, ha egyik érzékelő sem jelez sötétet. Ilyenkor valószínű, hogy a robot leszaladt a vonalról. Ilyenkor a tolatás nem áll meg rögtön, amint az egyik érzékelő sötétet jelez, mivel így a robot könnyen hamis útra tévedhetne. Csak konstans idő elteltével kezd újra előre menni.
• Kanyarodás balra: akkor történik, ha a középső érzékelő és a jobb oldali is sötétet jelez.
• Kanyarodás jobbra: hasonló az előzőhöz, csak itt a bal oldali érzékelő jelez sötétet a jobb helyett.

Néhány felvétel a robot működéséről

1. felvétel, 2. felvétel

A program

 
#define HATRA { Off(OUT_B); OnFwd(OUT_A); }
#define ELORE { Off(OUT_B); OnRev(OUT_A); }
#define JOBBRA { Off(OUT_A); OnFwd(OUT_B); }
#define BALRA { Off(OUT_A); OnRev(OUT_B); }
 
#define BAL_ERZ SENSOR_3
#define KOZEPSO_ERZ SENSOR_2
#define JOBB_ERZ SENSOR_1
 
#define KOZEPS (KOZEPSO_ERZ <= kozepso_min)
#define BALS (BAL_ERZ <= bal_min)
#define JOBBS (JOBB_ERZ <= jobb_min)
#define KOZEPV (KOZEPSO_ERZ >= kozepso_max)
#define BALV (BAL_ERZ >= bal_max)
#define JOBBV (JOBB_ERZ >= jobb_max)
 
task main()
{
 int bal_min, jobb_min, kozepso_min, bal_max, jobb_max, kozepso_max;
 int ert;
 
 SetSensor(BAL_ERZ, SENSOR_LIGHT);
 SetSensor(KOZEPSO_ERZ, SENSOR_LIGHT);
 SetSensor(JOBB_ERZ, SENSOR_LIGHT);
 
 kozepso_min = KOZEPSO_ERZ;
 kozepso_max = KOZEPSO_ERZ;
 jobb_min = JOBB_ERZ;
 jobb_max = JOBB_ERZ;
 
 //kalibracio 
 BALRA;
 until(jobb_max - jobb_min > 5)
 {
 ert = JOBB_ERZ;
 if(ert > jobb_max) jobb_max = ert;
 else if(ert < jobb_min) jobb_min = ert;
 
 ert = KOZEPSO_ERZ;
 if(ert > kozepso_max) kozepso_max = ert;
 else if(ert < kozepso_min) kozepso_min = ert;
 }
 
 Wait(50);
 ert = JOBB_ERZ;
 if(ert > jobb_max) jobb_max = ert;
 else if(ert < jobb_min) jobb_min = ert;
 
 ert = KOZEPSO_ERZ;
 if(ert > kozepso_max) kozepso_max = ert;
 else if(ert < kozepso_min) kozepso_min = ert;
 
 
 JOBBRA;
 bal_min = BAL_ERZ;
 bal_max = BAL_ERZ;
 until(bal_max - bal_min > 5)
 {
 int ert;
 
 ert = BAL_ERZ;
 if(ert > bal_max) bal_max = ert;
 else if(ert < bal_min) bal_min = ert;
 
 ert = KOZEPSO_ERZ;
 if(ert > kozepso_max) kozepso_max = ert;
 else if(ert < kozepso_min) kozepso_min = ert;
 }
 
 Wait(70);
 ert = JOBB_ERZ;
 if(ert > jobb_max) jobb_max = ert;
 else if(ert < jobb_min) jobb_min = ert;
 
 ert = KOZEPSO_ERZ;
 if(ert > kozepso_max) kozepso_max = ert;
 else if(ert < kozepso_min) kozepso_min = ert;
 
 bal_max -= (bal_max-bal_min)/3;
 bal_min += (bal_max-bal_min)/2;
 jobb_max -= (jobb_max-jobb_min)/3;
 jobb_min += (jobb_max-jobb_min)/2;
 kozepso_max -= (kozepso_max-kozepso_min)/3;
 kozepso_min += (kozepso_max-kozepso_min)/2;
 
 
 
 //raall a csikra
 BALRA;
 until(KOZEPS);
 
 //elindul
 ELORE;
 SelectDisplay(DISPLAY_WATCH);
 
 while(true)
 {
 if (!BALS && !KOZEPS && !JOBBS) {
 // Tolat
 HATRA;
 until(BALS || KOZEPS || JOBBS);
 Wait(15);
 ELORE;
 
 } else if(!BALS && KOZEPS && !JOBBS) {
 // Elore
 
 } else if( BALS && !KOZEPS && JOBBS) {
 // Elore
 
 } else if( BALS && KOZEPS && JOBBS) {
 // Elore
 
 } else if(!BALS && !KOZEPS && JOBBS) {
 // Jobbra
 JOBBRA;
 until( KOZEPS || JOBBV);
 ELORE;
 
 } else if( BALS && !KOZEPS && !JOBBS) {
 // Balra
 BALRA;
 until(BALV || KOZEPS );
 ELORE;
 
 } else if(!BALS && KOZEPS && JOBBS) {
 // Jobbra elore
 JOBBRA;
 until( KOZEPV || JOBBV);
 ELORE;
 
 } else if( BALS && KOZEPS && !JOBBS) {
 // Balra elore
 BALRA;
 until(BALV || KOZEPV );
 ELORE;
 
 }
 } 
}