Acrob robot constructs a map of a maze - obstacles around the robot (Jozef Belko)
Contents
Úvod
Zadaním môjho projeku bolo vytvoriť robota, ktorý bude mapovať blúdisko pomocou IR alebo US senzorov. Po zmapovaní má vypísať na obrazovku ako blúdisko vyzerá. Následne dostane na vstupe súradnice dvoch bodov, pričom jeho ďalšou úlohou je aby sa dostal z prvého z týchto bodov do druhého.
Robot
Používam robota Acrob, ktorého súčasťou su dva servo motory. Tie používa na pohyb. K robotovi som pripojil dva IR senzory, ktorých úlohou je zisťovať či sú alebo nie sú v okolí robota steny blúdiska. Na robota som namontoval tiež dva senzory na sledovanie čiary.
Prostredie
Blúdisko je tvorene poliami v tvare štvorca, ktoré môžu byť z ľubovolných strán ohraničené stenami. Znamená to, že je možné, že robot nebude schopný zmapovať celé pole kvôli nedosiahnuteľnosti niektorých polí. Tiež je ale možné že blúdisko nebude obsahovať žiadne steny. Na podložke blúdiska budú vertikálne a horizontálne čiary prechadzajúce stredmi jednotlivých polí blúdiska. V strede každeho poľa sa teda prekrížia.
Riešenie
Robot sleduje čiary na podložke pomocou, čoho sa pohybuje priamočiaro. A IR senzormi zisťuje či a kde okolo neho sú na danom poli steny. V prvom kole hlavnej funkcie zistí či sa po jeho stranách nachadzajú steny, potom sa otočí a získa tieto informácie aj o zvyšných dvoch stranách. Potom zistí, z ktorého poľa môže získavať ďalšie infomácie o mape, ak takéto pole existuje, nájde k nemu cestu, presunie sa tam a pomocou IR senzorov získa informácie o danom poli. Toto sa opakuje kým je možné získavať ďalšie dáta. Ak je celé blúdisko zmapované čaka na vstup, ktorým sú suradnice bodov, medzi ktorými bude hľadať cestu. Nakoniec cestu medzi týmito bodmi prejde. Akcie robota su rozdelené do jednotlivých funkcií.
Funkcia snimaj Táto funkcia vezme data z analogoveho vstupu z IR senzorov a určí čí sa okolo robota nachádzajú steny blúdiska.
void snimaj(){
Serial.print( "Snimam " );
int senzor1=analogRead(AnalogSenzorPIN1); //GP2D120
int senzor2=analogRead(AnalogSenzorPIN2); //2Y0A21
if(senzor1>200){
pole[pos_x][pos_y][(smer+1)%4]=1;
}else{
pole[pos_x][pos_y][(smer+1)%4]=2;
}
if(senzor2>300){
pole[pos_x][pos_y][(smer+3)%4]=1;
}else{
pole[pos_x][pos_y][(smer+3)%4]=2;
}
Serial.println( "- hotovo" );
}
Funkcia vpred Funkcia vpred slúži na presunutie robota o jedno pole blúdiska dopredu. Využíva funkcie fw(), left() a right(), vďaka ktorým stiha sledovat čiaru na podložke.
void vpred(){
Serial.print( "Idem vpred " );
boolean pom=true;
if(smer==0 && pos_y>0){
pos_y--;
}else if(smer==1 && pos_x<2*mapsize){
pos_x++;
}else if(smer==2 && pos_y<2*mapsize){
pos_y++;
}else if(smer==3 && pos_x>0){
pos_x--;
}else{
pom=false;
}
if(pom){
navsteva[pos_x][pos_y]=true;
while(true){
line_data1 = analogRead(line_senzor1); // read the value from the sensor
line_data2 = analogRead(line_senzor2);
if(line_data1<treshold1 && line_data2>treshold2){
right();
}else if(line_data1>treshold1 && line_data2<treshold2){
left();
}else if(line_data1<treshold1 && line_data2<treshold2){
fw();
}else if(line_data1>treshold1 && line_data2>treshold2){
break;
}
}
fw();fw();fw();
}
Serial.println( "- hotovo" );
}
Funkcia otoc Vďaka tejto funkcií sa robot vie otočiť o 90°. Tiež využíva funkcie fw(), bw(), left() a right(). Preberá jeden parameter, ktorý určuje či ide o otočenie vpravo alebo vľavo.
void otoc(int kam){
Serial.print( "Otacam sa " );
if(kam==1){
smer++;
if(smer==4){
smer=0;
}
}else{
smer--;
if(smer==-1){
smer=3;
}
}
while(true){
line_data1 = analogRead(line_senzor1); // read the value from the sensor
line_data2 = analogRead(line_senzor2);
if(line_data1<treshold1 && line_data2>treshold2){
left();
}else if(line_data1>treshold1 && line_data2<treshold2){
right();
}else if(line_data1<treshold1 && line_data2<treshold2){
bw();
}else if(line_data1>treshold1 && line_data2>treshold2){
break;
}
}
if(kam==1){
right();right();right();right();right();
}else{
left();left();left();left();left();
}
if(line_data1<treshold1 && line_data2<treshold2){
fw();fw();fw();
}else if(line_data1>treshold1 && line_data2<treshold2){
right();fw();fw();fw();
}else if(line_data1<treshold1 && line_data2>treshold2){
left();fw();fw();fw();
}else if(line_data1>treshold1 && line_data2>treshold2){
fw();
if(line_data1>treshold1 && line_data2<treshold2){
right();fw();fw();
}else if(line_data1<treshold1 && line_data2>treshold2){
left();fw();fw();
}
}
Serial.println( "- hotovo" );
}
Funkcia novy_ciel Táto funkcia prechádza polia blúdiska a vyberá prvé, z ktorého je možné získať nové informácie o blúdisku. Prebera jeden parameter, ktorým je pole o dvoch prvkoch, ktoré reprezentujú súradnice vysledného poľa.
void novy_ciel(byte *ciel){
Serial.print( "Hladam ciel " );
for(int i=0;i<mapsize*2;i++){
for(int j=0; j<mapsize*2;j++){
if( navsteva[pos_x][pos_y]==true){
if(pole[i][j][0]==2 && j>0 && navsteva[i][j-1]==false){
ciel[0]=i;
ciel[1]=j-1;
Serial.println( "- hotovo" );
return;
}
if(pole[i][j][1]==2 && i>0 && navsteva[i-1][j]==false){
ciel[0]=i-1;
ciel[1]=j;
Serial.println( "- hotovo" );
return;
}
if(pole[i][j][2]==2 && j<mapsize*2 && navsteva[i][j+1]==false){
ciel[0]=i;
ciel[1]=j+1;
Serial.println( "- hotovo" );
return;
}
if(pole[i][j][3]==2 && i<mapsize*2 && navsteva[i+1][j]==false){
ciel[0]=i+1;
ciel[1]=j;
Serial.println( "- hotovo" );
return;
}
}
}
}
ciel[0]=255;
ciel[1]=255;
Serial.println( "- hotovo" );
}
Funkcia hladat_cestu
Posednou funkciou je funkcia hladat_cestu, ktorá pomocou prehľadávania do šírky nájde z aktuálnej pozície na pozíciu poľa, z ktorého ide získať informacie o blúdsku alebo na pozíciu, ktorú mu určil užívateľ na vstupe. Prebera dva parametre ako sučastné súradnice, dva ako súradnice destinácie a jeden parameter (pole), v ktorom vráti cestu k cieľu.
void hladat_cestu(int zx, int zy, int cx, int cy, byte *cesta){
Serial.println( "Hladam cestu k cielu " );
int aktx=zx,akty=zy;
byte pozicia=0;
byte pocet=0;
byte queue[mapsize*2*mapsize*2][2];
char visited[mapsize*2][mapsize*2];
visited[zx][zy]=zx*10+zy+1;
queue[0][0]=zx;
queue[0][1]=zy;
pocet++;
while(pocet>0){
byte aktx=queue[pozicia][0];
byte akty=queue[pozicia][1];
pocet--;
pozicia++;
if(visited[aktx][akty]>0){
visited[aktx][akty]*=-1;
if(aktx==cx && akty==cy){
break;
}
if(pole[aktx][akty][0]==2){
if(akty>0 && visited[aktx][akty-1]==0){
queue[pozicia+pocet][0]=aktx;
queue[pozicia+pocet][1]=akty-1;
pocet++;
visited[aktx][akty-1]=10*aktx+akty+1;
}
}
if(pole[aktx][akty][1]==2){
if(aktx>0 && visited[aktx-1][akty]==0){
queue[pozicia+pocet][0]=aktx-1;
queue[pozicia+pocet][1]=akty;
pocet++;
visited[aktx-1][akty]=10*aktx+akty+1;
}
}
if(pole[aktx][akty][2]==2){
if(akty<mapsize*2-1 && visited[aktx][akty+1]==0){
queue[pozicia+pocet][0]=aktx;
queue[pozicia+pocet][1]=akty+1;
pocet++;
visited[aktx][akty+1]=10*aktx+akty+1;
}
}
if(pole[aktx][akty][3]==2){
if(aktx<mapsize*2-1 && visited[aktx+1][akty]==0){
queue[pozicia+pocet][0]=aktx+1;
queue[pozicia+pocet][1]=akty;
pocet++;
visited[aktx+1][akty]=10*aktx+akty+1;
}
}
}
}
if(aktx!=cx && akty!=cy){
cesta[0]=0;
}else{
pocet=1;
while(visited[aktx][akty]!=(10*aktx+akty+1)*-1){
if(visited[aktx][akty]==(aktx*10+akty+1)*-1-10){
cesta[pocet]=1; //smer 3 - hore
}else if(visited[aktx][akty]==(aktx*10+akty+1)*-1+10){
cesta[pocet]=3; //smer 1 - dole
}else if(visited[aktx][akty]==(aktx*10+akty+1)*-1-1){
cesta[pocet]=0; //smer 0 - vlavo
}else if(visited[aktx][akty]==(aktx*10+akty+1)*-1+1){
cesta[pocet]=2; //smer 2 - vpravo
}
pocet++;
pozicia=((visited[aktx][akty]*-1)-1)%10;
aktx=((visited[aktx][akty]*-1)-1)/10;
akty=pozicia;
}
cesta[0]=pocet;
}
}
Záver
Robot je schopný zmapovať blúdisko s rozmermi do asi 10x10 polí. Avšak dosť často sa mýli pri otáčaní. Tiež bohužiaľ nefunguje načítanie uživatelských dát. Robota v blúdisku možno vidieť na obrázku.
