|
Page 1 of 1
|
[ 11 posts ] |
|
aStar (a-star, A-Stern) Algorithm for NXT: TamTam.c
Author |
Message |
Ford Prefect
Guru
Joined: Sat Mar 01, 2008 12:52 pm Posts: 1030
|
 aStar (a-star, A-Stern) Algorithm for NXT: TamTam.c
hi everyone,
here's my a-star for NXT to solve navigation in labyrinths - or simply in your living rooms.
It uses an a-star algorithm, modified by Dijkstra (the estimated distance to destination is always =0; this simplifies the node structure, enlarges the possible size of the map and excelerates the execution speed).
This is a "virtual solution"; in future this file will be added to the Navigation project (algorithm is to be enclosed to navigator.h file, see different thread).
_________________ regards, HaWe aka Ford #define S sqrt(t+2*i*i)<2 #define F(a,b) for(a=0;a<b;++a) float x,y,r,i,s,j,t,n;task main(){F(y,64){F(x,99){r=i=t=0;s=x/33-2;j=y/32-1;F(n,50&S){t=r*r-i*i;i=2*r*i+j;r=t+s;}if(S){PutPixel(x,y);}}}while(1)}
Last edited by Ford Prefect on Sat Mar 22, 2008 2:08 pm, edited 1 time in total.
|
Sun Mar 16, 2008 12:00 pm |
|
 |
Ford Prefect
Guru
Joined: Sat Mar 01, 2008 12:52 pm Posts: 1030
|
newest version, nearly Prefect
(You'll need in addition the file RobotC+.h, see below)
 |  |  |  | Code: ///////////////////////////////////////////////////////////////////////////// // TamTam.c // Version 1.0.7 "Dijkstra" // virtuelle Wegsuche auf dem Display //*************************************************************************** // der A* (A-Stern, a-star) - Algorithmus fuer den NXT // zur Wegsuche und Navigation an Hindernissen vorbei //*************************************************************************** // hier in der Variante nach Dijkstra // (keine geschaetzten heuristischen Kosten H, also H=0 => F=G! // // 1.0.4 am Schluss werden Routenanweisungen erstellt // 1.0.1 erkennt, wenn kein Weg zum Ziel existiert // 0.8.9 Suchrichtung optimiert: direkte Wege werden sehr schnell gefunden // 0.3.8 Kurse optimiert: Kurs-Aenderungen verursachen hoehere Kosten // 0.2.9 schneiden von Hindernis-Kanten wird vermieden // (c) Helmut W. 2008 /////////////////////////////////////////////////////////////////////////////
#include "RobotC+.h"
#define println nxtDisplayTextLine #define printXY nxtDisplayStringAt //******************************************************************** short xStart, yStart, xZiel, yZiel;
const short MadlyEnormous=10000; const short occ=1;
const short MapSize=30; // 30x30 Felder, je 30cm plus "Rand" const short limit=MapSize/2;
short Map[MapSize+1][MapSize+1]; // besetzt=1 / frei=0; short xPrev[MapSize+1][MapSize+1]; // Abszisse von Vorgaenger (x) short yPrev[MapSize+1][MapSize+1]; // Ordinate von Vorgaenger (y) short dPrev[MapSize+1][MapSize+1]; // Richtung zu Vorgaenger (d= 0-7) short F[MapSize][MapSize]; // gesamte Wege-Kosten F short List[MapSize][MapSize]; // gehoert zu Liste: undef=0 offen=1 geschlossen=2
//********************************************************************
short GetMap(short x, short y) // Zustand des Feldes: 0=frei, 1= Hindernis { return Map[x+limit][y+limit];}
void SetMap(short x, short y, short val) { Map[x+limit][y+limit]=val;}
short GetxPrev(short x, short y) // gespeicherter Vorgaenger: x-Koord. { return xPrev[x+limit][y+limit];}
void SetxPrev(short x, short y, short xPtr) { xPrev[x+limit][y+limit]=xPtr;}
short GetyPrev(short x, short y) // gespeicherter Vorgaenger: y-Koord. { return yPrev[x+limit][y+limit];}
void SetyPrev(short x, short y, short yPtr) { yPrev[x+limit][y+limit]=yPtr;}
short GetdPrev(short x, short y) // gespeicherter Vorgaenger: Richtung { return dPrev[x+limit][y+limit];}
void SetdPrev(short x, short y, short d) { dPrev[x+limit][y+limit]=d;}
short GetF(short x, short y) // Wege-Kosten F { return F[x+limit][y+limit];}
void SetF(short x, short y, short val) { F[x+limit][y+limit]=val;}
short GetList(int x, int y) // Listen-Zugehoerigkeit (0, 1, 2) { return List[x+limit][y+limit];}
void SetList(int x, int y, int val) { List[x+limit][y+limit]=val;}
//******************************************************************** //********************************************************************
void SetPixel(short x, short y) // Transformation der Koordinatensysteme { int xPix, yPix;
xPix=limit+1+x; yPix=limit+1+y; nxtSetPixel(xPix, yPix); }
//********************************************************************
void DrawRect(short x, short y, short rad) // Quadrat um den Mittelpunkt zeichnen { int xPix, yPix;
xPix=limit+x+1; // CenterX yPix=limit+y+1; // CenterY nxtDrawRect(xPix-rad, yPix+rad, xPix+rad, yPix-rad); }
//********************************************************************
void ClearPixel(short x, short y) // Karten-Pixel loeschen { int xPix, yPix;
xPix=limit+1+x; yPix=limit+1+y; nxtClearPixel(xPix, yPix); }
//******************************************************************** //********************************************************************
int GetNx(short x, short pos)// 3 2 1 benachbarte x-Koordinaten { // 4 0 int nx; // 5 6 7
nx=MadlyEnormous; // wenn ungueltiger Nachbar if (((pos==1)||(pos==0)||(pos==7))&& (x<limit-1)) {nx=x+1; } else if ((pos==2)||(pos==6)) { nx=x; } else if (((pos==3)||(pos==4)||(pos==5))&&(x>(-limit))) {nx=x-1; }
return nx; }
//********************************************************************
int GetNy(short y, short pos)// 3 2 1 benachbarte y-Koordinaten { // 4 0 int ny; // 5 6 7
ny=MadlyEnormous; // wenn ungueltiger Nachbar if (((pos==1)||(pos==2)||(pos==3))&& (y<limit-1)) { ny=y+1; } else if ((pos==4)||(pos==0)) { ny=y; } else if (((pos==5)||(pos==6)||(pos==7))&&(y>(-limit))) { ny=y-1; }
return ny; }
//********************************************************************
bool CloseToTheEdge(short x, short y, short dir) // 3 2 1 keine Hindernis-Kanten schneiden { // 4 0 bool edge; // 5 6 7 short xm,xp,ym,yp;
edge=false; // nur bei diagonalem Schritt beruecksichtigen!)
xm=(xm> -limit ? x-1 : x); xp=(xp< limit-1 ? x+1 : x); ym=(ym> -limit ? y-1 : y); yp=(yp< limit-1 ? y+1 : y);
if (dir==1) {edge=((GetMap(xp,y)==occ)||(GetMap(x,yp)==occ));} // 0 2 else if (dir==3) {edge=((GetMap(x,yp)==occ)||(GetMap(xm,y)==occ));} // 2 4 else if (dir==5) {edge=((GetMap(xm,y)==occ)||(GetMap(x,ym)==occ));} // 4 6 else if (dir==7) {edge=((GetMap(x,ym)==occ)||(GetMap(xp,y)==occ));} // 6 0
return edge; }
//********************************************************************
void ExpandNode(short xa, short ya) // checkt Nachbarfelder von (xa, ya) { short F, d, k, v, w, z;
for (d=0;d<=7;d++) { v=GetNx(xa,d); // Nachbarn durchgehen: 3 2 1 w=GetNy(ya,d); // 4 0 // 5 6 7 if ((d==0) ||(d==2) ||(d==4) ||(d==6)) // ((p%2)==0 arbeitet fehlerhaft!) {k=10; } // Kosten gerader Schritt: 10 else { k=14; // Kosten diagonaler Schritt: 15 if(CloseToTheEdge(xa,ya,d)) continue; // diagonal ist bei Hinderniskante verboten }
if ((v<MadlyEnormous)&&(w<MadlyEnormous)) // gueltiger Nachbar ?! { z=GetMap(v,w); // Hindernis vorhanden? if ( (z==0)&&(!(GetList(v,w)==2))) // frei und nicht in geschlossener Liste { F=GetF(xa,ya)+k; // F: Kosten zu (v,w) ueber aktuelles Quadrat (xa,ya)
if (!(GetList(v,w)==1)) // wenn NICHT in offener Liste { SetList (v,w,1); // dann in die offene uebernehmen SetxPrev(v,w, xa); // Vorgaenger: ^x-aktuell SetyPrev(v,w, ya); // Vorgaenger: ^y-aktuell SetdPrev(v,w, d); // Richtung zum Vorgaenger (0-7) if(d==GetdPrev(xa,ya)) F-=2; // Kosten geringer, wenn keine Kursaenderung SetF(v,w,F); // Kosten gesamt } // if (Nicht offen) else if (GetList(v,w)==1) // wenn aber bereits in offener Liste { if (F<=GetF(v,w)) // Wegekosten G dorthin vergleichen: // neuer Weg dahin: geringere Kosten als alte Wegekosten ? { // dann ueberschreiben + neuen Weg im Quadrat einspeichern SetxPrev(v,w, xa); // Vorgaenger: ^x-aktuell SetyPrev(v,w, ya); // Vorgaenger: ^y-aktuell SetdPrev(v,w, d); // Richtung zum Vorgaenger (0-7) if(d==GetdPrev(xa,ya)) F-=2;// Kosten geringer, wenn keine Kursaenderung SetF(v,w,F); // Kosten gesamt }// if (neuer Weg besser) } // else if (schon offen)
}// if (nicht besetzt, nicht geschlossen) } // if (gueltiger Nachbar)
}// for
}//ExpandNode
//******************************************************************** //********************************************************************
void A_Stern (short xStart, short yStart, short xZiel, short yZiel) { short i, j, c=0, xAkt, yAkt, xOld, yOld, xd, yd, xDa, yDa, FMin=MadlyEnormous;
xAkt=xZiel; // wir starten rueckwaerts und beginnen die Suche beim Ziel ! yAkt=yZiel; // Das Quadrat, von dem aus gesucht wird, heisst "aktuelles Quadrat"
SetList(xAkt, yAkt, 1); // akt. Quadrat in Offene Liste (=1) SetF (xAkt,yAkt,0); // Kosten F=G+H; G=0, H=0; SetList(xAkt, yAkt, 2); // aktuelles Quadrat in geschlossene Liste uebernehmen xd=abs(xZiel-xStart); yd=abs(yZiel-yStart); // solange das andere Ende (der Start) nicht gefunden wurde:
while (GetList(xStart,yStart)!=2) { FMin=MadlyEnormous; xOld=xAkt; yOld=yAkt; // suche in offener Liste (=1)das Feld mit kleinstem F
if ((xZiel>=xStart)&&(yZiel>=yStart)) { for (i=-limit;i<limit;i++) { for (j=-limit;j<limit;j++) { if ((GetList(i,j)==1)&&(GetF(i,j)<FMin) ) { FMin=GetF(i,j); xAkt=i; // wenn gefunden: nimm es als aktuelles Quadrat yAkt=j; xDa=abs(i-xStart); yDa=abs(j-yStart); if ((xDa<xd)||(yDa<yd)) { SetList(xAkt, yAkt, 2); // aktuelles Quadrat in geschlossene Liste SetPixel(xAkt,yAkt); ExpandNode(xAkt, yAkt); // Nachbarfelder untersuchen c+=1; } } // if } // for j SetList(xAkt, yAkt, 2); // aktuelles Quadrat in geschlossene Liste SetPixel(xAkt,yAkt); ExpandNode(xAkt, yAkt); // Nachbarfelder untersuchen c+=1; } // for i
} // if (Richtung) else if ((xZiel<xStart)&&(yZiel<yStart)){ for (i=limit-1;i>=(-limit);i-=1) { for (j=limit-1;j>=(-limit);j-=1) { if ((GetList(i,j)==1)&&(GetF(i,j)<FMin) ) { FMin=GetF(i,j); xAkt=i; // wenn gefunden: nimm es als aktuelles Quadrat yAkt=j; xDa=abs(i-xStart); yDa=abs(j-yStart); if ((xDa<xd)||(yDa<yd)) { SetList(xAkt, yAkt, 2); // aktuelles Quadrat in geschlossene Liste SetPixel(xAkt,yAkt); ExpandNode(xAkt, yAkt); // Nachbarfelder untersuchen c+=1; } } // if } // for j SetList(xAkt, yAkt, 2); // aktuelles Quadrat in geschlossene Liste SetPixel(xAkt,yAkt); ExpandNode(xAkt, yAkt); // Nachbarfelder untersuchen c+=1; } // for i
} // if (Richtung)
else if ((xZiel<xStart)&&(yZiel>=yStart)) { for (i=limit-1;i>=(-limit);i-=1) { for (j=-limit;j<limit;j+=1) { if ((GetList(i,j)==1)&&(GetF(i,j)<FMin) ) { FMin=GetF(i,j); xAkt=i; // wenn gefunden: nimm es als aktuelles Quadrat yAkt=j; xDa=abs(i-xStart); yDa=abs(j-yStart); if ((xDa<xd)||(yDa<yd)) { SetList(xAkt, yAkt, 2); // aktuelles Quadrat in geschlossene Liste SetPixel(xAkt,yAkt); ExpandNode(xAkt, yAkt); // Nachbarfelder untersuchen c+=1; } } // if } // for j SetList(xAkt, yAkt, 2); // aktuelles Quadrat in geschlossene Liste SetPixel(xAkt,yAkt); ExpandNode(xAkt, yAkt); // Nachbarfelder untersuchen c+=1; } // for i
} // if (Richtung)
else if ((xZiel>=xStart)&&(yZiel<yStart)) { for (i=-limit;i<limit;i+=1) { for (j=limit-1;j>=(-limit);j-=1) { if ((GetList(i,j)==1)&&(GetF(i,j)<FMin) ) { FMin=GetF(i,j); xAkt=i; // wenn gefunden: nimm es als aktuelles Quadrat yAkt=j; xDa=abs(i-xStart); yDa=abs(j-yStart); if ((xDa<xd)||(yDa<yd)) { SetList(xAkt, yAkt, 2); // aktuelles Quadrat in geschlossene Liste SetPixel(xAkt,yAkt); ExpandNode(xAkt, yAkt); // Nachbarfelder untersuchen c+=1; } } // if } // for j SetList(xAkt, yAkt, 2); // aktuelles Quadrat in geschlossene Liste SetPixel(xAkt,yAkt); ExpandNode(xAkt, yAkt); // Nachbarfelder untersuchen
c+=1; } // for i
} // if (Richtung) println(1, "Akt(X,Y)%4d%4d",xAkt,yAkt); // Anzeige von xAkt, yAkt printXY(46, 20, "Wege=%4d",c); // Anzeige der Zahl der Zyklen/Wege
if((xAkt==xOld)&&(yAkt==yOld)&&(FMin==MadlyEnormous)) { println(2,"%s","No Way!"); PlaySound(soundBeepBeep); Wait10Msec(200); break; }
} // while
printXY(46,10, "fertig %s",""); // Anzeige, wenn fertig ClearPixel(xStart,yStart); Wait10Msec(200);
}
//******************************************************************** //********************************************************************
sub DrawMap() // Hindernisse und Start/Ziel zeichnen { short x, y;
nxtDrawRect(0, MapSize+1, MapSize+1, 0); // Umrandung
//========================================================================== x=8; // Hindernis setzen: for (y=-3;y<=5;y++) // Hier kann man sich austoben und beliebige { // Hindernis-Punkte definieren: SetMap(x,y,occ); // a) in Karte eintragen (occ=1=besetzt) SetPixel(x,y); // b) auf Display malen } y=9; for (x=-8;x<=7;x++) // nochmal weitere { // Hindernis-Punkte definieren: SetMap(x,y,occ); // a) in Karte eintragen (occ=1=besetzt) SetPixel(x,y); // b) auf Display malen }
y=-5; for (x=0;x<=5;x++) // nochmal weitere { // Hindernis-Punkte definieren: SetMap(x,y,occ); // a) in Karte eintragen (occ=1=besetzt) SetPixel(x,y); // b) auf Display malen }
y=1; for (x=10;x<=12;x++) // nochmal weitere { // Hindernis-Punkte definieren: SetMap(x,y,occ); // a) in Karte eintragen (occ=1=besetzt) SetPixel(x,y); // b) auf Display malen }
y=6; for (x=8;x<=14;x++) // nochmal weitere { // Hindernis-Punkte definieren: SetMap(x,y,occ); // a) in Karte eintragen (occ=1=besetzt) SetPixel(x,y); // b) auf Display malen }
//==========================================================================
SetPixel(xStart,yStart); // Start als Punkt malen DrawRect(xZiel, yZiel,1); // Ziel als Viereck malen
}
//******************************************************************** //********************************************************************
void WalkThisWay() // virtuelle Fahrt der berechneten Route { short x, y, xn, yn, dir, olddir, turn;
x=xStart; y=yStart; dir=0; olddir=0; turn=0; //printXY(46,10,"Kost=%4d",GetF(xStart,yStart)); println(0, "Turn=%4d",turn); println(1, "Kurs=%4d",dir); println(2, "Pos XY%3d%3d",x,y); wait1Msec(1000);
while (! ((x==xZiel)&&(y==yZiel))) { println(2, "Pos XY%3d%3d",x,y); SetPixel(x, y); wait1Msec(500);
dir=180+(45*GetdPrev(x,y)); if (dir>=360) dir-=360;
turn=dir-olddir; if (turn<-180) turn=turn+360; if (turn>180) turn=360-turn;
println(0, "Turn=%4d",turn); wait1Msec(500);
println(1, "Kurs=%4d",dir); wait1Msec(500);
olddir=dir; xn=GetxPrev(x,y); yn=GetyPrev(x,y); x=xn; y=yn;
} SetPixel(x, y); println(2, "Pos XY%3d%3d",x,y); printXY(46,30, "%s","Ziel"); printXY(46,20, "%s","erreicht!");
}
//********************************************************************
sub initVar() { short i,j;
// verschiedene Start/Ziel-Kombinationen
xStart=0; yStart=0; // Startpunkt definieren xZiel=13; yZiel=4; // Zielpunkt definieren
//xStart=13; yStart=4; // Startpunkt definieren //xZiel=0; yZiel=0; // Zielpunkt definieren
//xStart=0; yStart=4; // Startpunkt definieren //xZiel=13; yZiel=0; // Zielpunkt definieren
//xStart=13; yStart=0; // Startpunkt definieren //xZiel=0; yZiel=4; // Zielpunkt definieren
for(i=0;i<MapSize;i++) { for (j=0;j<MapSize;j++) { Map[i][j]=0; // Felder frei xPrev[i][j]=0; // Vorgaenger Nullpunkt yPrev[i][j]=0; F[i][j]=MadlyEnormous; // Kosten gewaltig List[i][j]=0; // Liste undefiniert } } } //********************************************************************
task main() { eraseDisplay(); initVar(); DrawMap();
A_Stern(xStart,yStart,xZiel, yZiel);
while(true) { eraseDisplay(); DrawMap(); WalkThisWay(); wait1Msec(2000); } } //******************************************************************** //********************************************************************
|  |  |  |  |
_________________ regards, HaWe aka Ford #define S sqrt(t+2*i*i)<2 #define F(a,b) for(a=0;a<b;++a) float x,y,r,i,s,j,t,n;task main(){F(y,64){F(x,99){r=i=t=0;s=x/33-2;j=y/32-1;F(n,50&S){t=r*r-i*i;i=2*r*i+j;r=t+s;}if(S){PutPixel(x,y);}}}while(1)}
Last edited by Ford Prefect on Sat Apr 19, 2008 2:59 pm, edited 19 times in total.
|
Sat Mar 22, 2008 2:08 pm |
|
 |
Ford Prefect
Guru
Joined: Sat Mar 01, 2008 12:52 pm Posts: 1030
|
this is an additional file which is included and needed for some variable definitions a.s.o.:
 |  |  |  | Code: /////////////////////////////////////////////////////////////////////// // RobotC+.h - damit's ein bisserl einfacher wird... ;-) // // Version 0.5.8 ///////////////////////////////////////////////////////////////////////
//=================================================================== // mathematische Konstanten und Funktionen //===================================================================
// Eulersche Zahl E: const float E=2.718281828450945235360287471352662497757247093699959574966967627724076630353547594571;
//********************************************
int min(int a, int b) { int m; m=(a<b ? a:b); return m; }
//********************************************
float min(float a, float b) { float m; m=(a<b ? a:b); return m; }
//********************************************
int max(int a, int b) { int m; m=(a>b ? a:b); return m; }
//********************************************
float max(float a, float b) { float m; m=(a>b ? a:b); return m; }
//********************************************
int round(float f) { if(f>0) return (int)(f + 0.5); else return (int)(f - 0.5); }
//********************************************
// ArcusTangens mit Sonderfaellen; Angabe in Grad! // x=Ankathete y=Gegenkathete Tangens=y/x
float atan2Degrees(float x, float y) { float phi, alpha; //phi=Bogenmass, alpha=Grad;
if (x>0) {phi=atan(y/x);} else if ((x<0)&&(y>=0)) {phi=PI+atan(y/x);} else if ((x<0)&&(y<0)) {phi=-PI+atan(y/x);} else if ((x==0)&&(y>0)) {phi=PI/2;} else if ((x==0)&&(y<0)) {phi=-PI/2;} else if ((x==0)&&(y==0)) {phi=0;}
alpha=radiansToDegrees(phi); return alpha; }
//********************************************
bool IsInRange(int Wert, int Basis, int Toleranz) { return((Wert>=Basis-Toleranz) && (Wert<=Basis+Toleranz)); }
//=================================================================== // Prozeduren fuer Sensoren //===================================================================
bool SMuxPressed(int port, byte ch) // MUX-Port an NXT (S1, S2,... ) // ch: 0=direkt, 1=ch1, 2=ch2, 3=ch3
{ int v; bool pressed;
v=SensorValue(port); if (ch==0) { pressed=(v<100 ); } else if (ch==1) { pressed=( IsInRange(v,775,30) || IsInRange(v,523,25) || IsInRange(v,343,8) || IsInRange(v,275,8)) ; } else if (ch==2) { pressed=( IsInRange(v,620,40) || IsInRange(v,523,25) || IsInRange(v,295,8) || IsInRange(v,275,8)) ; } else if (ch==3) { pressed=( IsInRange(v,363,15) || IsInRange(v,343,8) || IsInRange(v,295,8) || IsInRange(v,275,8)) ; } return pressed;
// Wertetabelle fuer Sensor-Muxer (natuerlich Bauform-abhaengig) //************************************************************* // // rcx ch3 ch2 ch1 raw // RCX-WiderstMux als sensorRawValue definiert // 0 0 0 0 1023 // an 3 Mux-Einaenge UND direktan rcx-Eingang // 0 0 0 1 775 // je 1 Taster angeschlossen // 0 0 1 0 620 // 0 0 1 1 523 // 0 1 0 0 363 // 0 1 0 1 343 // 0 1 1 0 295 // 0 1 1 1 275 // 1 0 0 0 79 // ab hier die Werte, wenn der // 1 0 0 1 64 // 4. Taster direkt am rcx-Sensor-Eingang // 1 0 1 0 74 // gedrueckt wird. // 1 0 1 1 70 // Eine Unterscheidung, ob dann auch // 1 1 0 0 69 // gleichzeitig noch andere Taster // 1 1 0 1 55 // zusaetzlich gedrueckt sind, // 1 1 1 0 55 // ist dann kaum mehr moeglich. // 1 1 1 1 55 //
}
//=================================================================== // globale Prozeduren fue Motor-Steuerung //===================================================================
void motSync (int m, int s) // setzt Sync-Master & Slave { if ((m==0) && (s==0)) { nSyncedMotors = synchNone;} else if ((m==0) && (s==1)) { nSyncedMotors = synchAB;} else if ((m==0) && (s==2)) { nSyncedMotors = synchAC;} else if ((m==1) && (s==0)) { nSyncedMotors = synchBA;} else if ((m==1) && (s==2)) { nSyncedMotors = synchBC;} else if ((m==2) && (s==0)) { nSyncedMotors = synchCA;} else if ((m==2) && (s==1)) { nSyncedMotors = synchCB;} else nSyncedMotors = synchNone; }
//=================================================================== //===================================================================
|  |  |  |  |
_________________ regards, HaWe aka Ford #define S sqrt(t+2*i*i)<2 #define F(a,b) for(a=0;a<b;++a) float x,y,r,i,s,j,t,n;task main(){F(y,64){F(x,99){r=i=t=0;s=x/33-2;j=y/32-1;F(n,50&S){t=r*r-i*i;i=2*r*i+j;r=t+s;}if(S){PutPixel(x,y);}}}while(1)}
|
Sat Mar 22, 2008 2:38 pm |
|
 |
lololol
Rookie
Joined: Sat Jan 10, 2009 6:26 pm Posts: 14
|
 Re: aStar (a-star, A-Stern) Algorithm for NXT: TamTam.c
Nice, i phink it was what i was looking for, in the future i will tell if this works for me. Meanwhile thanks for the help.
|
Mon Nov 09, 2009 7:22 pm |
|
 |
Azhari
Rookie
Joined: Thu Mar 14, 2013 10:32 pm Posts: 25 Location: Malaysia
|
 Re: aStar (a-star, A-Stern) Algorithm for NXT: TamTam.c
Nice and thanks, i'm also doing a* algorithm . It would a nice reference to me as guidence. it would be i use it as reference so i can study and built my own? the problem is.... i don't understand your language 
|
Mon Jul 22, 2013 10:42 pm |
|
 |
mightor
Site Admin
Joined: Wed Mar 05, 2008 8:14 am Posts: 3654 Location: Rotterdam, The Netherlands
|
 Re: aStar (a-star, A-Stern) Algorithm for NXT: TamTam.c
Ford Prefect is not on these forums anymore and he no longer uses ROBOTC. I'm afraid you'll have to use google translate.
= Xander
_________________| Professional Conduit of Reasonableness| (Title bestowed upon on the 8th day of November, 2013) | My Blog: I'd Rather Be Building Robots| ROBOTC 3rd Party Driver Suite: [ Project Page]
|
Tue Jul 23, 2013 1:31 am |
|
 |
Azhari
Rookie
Joined: Thu Mar 14, 2013 10:32 pm Posts: 25 Location: Malaysia
|
 Re: aStar (a-star, A-Stern) Algorithm for NXT: TamTam.c
which language i should use?
|
Tue Jul 23, 2013 5:33 am |
|
 |
mattallen37
Expert
Joined: Thu Sep 29, 2011 11:09 pm Posts: 184 Location: Michigan USA
|
 Re: aStar (a-star, A-Stern) Algorithm for NXT: TamTam.c
German
_________________ Matt
|
Tue Jul 23, 2013 9:38 am |
|
 |
Azhari
Rookie
Joined: Thu Mar 14, 2013 10:32 pm Posts: 25 Location: Malaysia
|
 Re: aStar (a-star, A-Stern) Algorithm for NXT: TamTam.c
|
Tue Jul 23, 2013 11:28 am |
|
 |
Azhari
Rookie
Joined: Thu Mar 14, 2013 10:32 pm Posts: 25 Location: Malaysia
|
 Re:
 |  |  |  | Ford Prefect wrote: newest version, nearly Prefect  (You'll need in addition the file RobotC+.h, see below)  |  |  |  | Code: ///////////////////////////////////////////////////////////////////////////// // TamTam.c // Version 1.0.7 "Dijkstra" // virtuelle Wegsuche auf dem Display //*************************************************************************** // der A* (A-Stern, a-star) - Algorithmus fuer den NXT // zur Wegsuche und Navigation an Hindernissen vorbei //*************************************************************************** // hier in der Variante nach Dijkstra // (keine geschaetzten heuristischen Kosten H, also H=0 => F=G! // // 1.0.4 am Schluss werden Routenanweisungen erstellt // 1.0.1 erkennt, wenn kein Weg zum Ziel existiert // 0.8.9 Suchrichtung optimiert: direkte Wege werden sehr schnell gefunden // 0.3.8 Kurse optimiert: Kurs-Aenderungen verursachen hoehere Kosten // 0.2.9 schneiden von Hindernis-Kanten wird vermieden // (c) Helmut W. 2008 /////////////////////////////////////////////////////////////////////////////
#include "RobotC+.h"
#define println nxtDisplayTextLine #define printXY nxtDisplayStringAt //******************************************************************** short xStart, yStart, xZiel, yZiel;
const short MadlyEnormous=10000; const short occ=1;
const short MapSize=30; // 30x30 Felder, je 30cm plus "Rand" const short limit=MapSize/2;
short Map[MapSize+1][MapSize+1]; // besetzt=1 / frei=0; short xPrev[MapSize+1][MapSize+1]; // Abszisse von Vorgaenger (x) short yPrev[MapSize+1][MapSize+1]; // Ordinate von Vorgaenger (y) short dPrev[MapSize+1][MapSize+1]; // Richtung zu Vorgaenger (d= 0-7) short F[MapSize][MapSize]; // gesamte Wege-Kosten F short List[MapSize][MapSize]; // gehoert zu Liste: undef=0 offen=1 geschlossen=2
//********************************************************************
short GetMap(short x, short y) // Zustand des Feldes: 0=frei, 1= Hindernis { return Map[x+limit][y+limit];}
void SetMap(short x, short y, short val) { Map[x+limit][y+limit]=val;}
short GetxPrev(short x, short y) // gespeicherter Vorgaenger: x-Koord. { return xPrev[x+limit][y+limit];}
void SetxPrev(short x, short y, short xPtr) { xPrev[x+limit][y+limit]=xPtr;}
short GetyPrev(short x, short y) // gespeicherter Vorgaenger: y-Koord. { return yPrev[x+limit][y+limit];}
void SetyPrev(short x, short y, short yPtr) { yPrev[x+limit][y+limit]=yPtr;}
short GetdPrev(short x, short y) // gespeicherter Vorgaenger: Richtung { return dPrev[x+limit][y+limit];}
void SetdPrev(short x, short y, short d) { dPrev[x+limit][y+limit]=d;}
short GetF(short x, short y) // Wege-Kosten F { return F[x+limit][y+limit];}
void SetF(short x, short y, short val) { F[x+limit][y+limit]=val;}
short GetList(int x, int y) // Listen-Zugehoerigkeit (0, 1, 2) { return List[x+limit][y+limit];}
void SetList(int x, int y, int val) { List[x+limit][y+limit]=val;}
//******************************************************************** //********************************************************************
void SetPixel(short x, short y) // Transformation der Koordinatensysteme { int xPix, yPix;
xPix=limit+1+x; yPix=limit+1+y; nxtSetPixel(xPix, yPix); }
//********************************************************************
void DrawRect(short x, short y, short rad) // Quadrat um den Mittelpunkt zeichnen { int xPix, yPix;
xPix=limit+x+1; // CenterX yPix=limit+y+1; // CenterY nxtDrawRect(xPix-rad, yPix+rad, xPix+rad, yPix-rad); }
//********************************************************************
void ClearPixel(short x, short y) // Karten-Pixel loeschen { int xPix, yPix;
xPix=limit+1+x; yPix=limit+1+y; nxtClearPixel(xPix, yPix); }
//******************************************************************** //********************************************************************
int GetNx(short x, short pos)// 3 2 1 benachbarte x-Koordinaten { // 4 0 int nx; // 5 6 7
nx=MadlyEnormous; // wenn ungueltiger Nachbar if (((pos==1)||(pos==0)||(pos==7))&& (x<limit-1)) {nx=x+1; } else if ((pos==2)||(pos==6)) { nx=x; } else if (((pos==3)||(pos==4)||(pos==5))&&(x>(-limit))) {nx=x-1; }
return nx; }
//********************************************************************
int GetNy(short y, short pos)// 3 2 1 benachbarte y-Koordinaten { // 4 0 int ny; // 5 6 7
ny=MadlyEnormous; // wenn ungueltiger Nachbar if (((pos==1)||(pos==2)||(pos==3))&& (y<limit-1)) { ny=y+1; } else if ((pos==4)||(pos==0)) { ny=y; } else if (((pos==5)||(pos==6)||(pos==7))&&(y>(-limit))) { ny=y-1; }
return ny; }
//********************************************************************
bool CloseToTheEdge(short x, short y, short dir) // 3 2 1 keine Hindernis-Kanten schneiden { // 4 0 bool edge; // 5 6 7 short xm,xp,ym,yp;
edge=false; // nur bei diagonalem Schritt beruecksichtigen!)
xm=(xm> -limit ? x-1 : x); xp=(xp< limit-1 ? x+1 : x); ym=(ym> -limit ? y-1 : y); yp=(yp< limit-1 ? y+1 : y);
if (dir==1) {edge=((GetMap(xp,y)==occ)||(GetMap(x,yp)==occ));} // 0 2 else if (dir==3) {edge=((GetMap(x,yp)==occ)||(GetMap(xm,y)==occ));} // 2 4 else if (dir==5) {edge=((GetMap(xm,y)==occ)||(GetMap(x,ym)==occ));} // 4 6 else if (dir==7) {edge=((GetMap(x,ym)==occ)||(GetMap(xp,y)==occ));} // 6 0
return edge; }
//********************************************************************
void ExpandNode(short xa, short ya) // checkt Nachbarfelder von (xa, ya) { short F, d, k, v, w, z;
for (d=0;d<=7;d++) { v=GetNx(xa,d); // Nachbarn durchgehen: 3 2 1 w=GetNy(ya,d); // 4 0 // 5 6 7 if ((d==0) ||(d==2) ||(d==4) ||(d==6)) // ((p%2)==0 arbeitet fehlerhaft!) {k=10; } // Kosten gerader Schritt: 10 else { k=14; // Kosten diagonaler Schritt: 15 if(CloseToTheEdge(xa,ya,d)) continue; // diagonal ist bei Hinderniskante verboten }
if ((v<MadlyEnormous)&&(w<MadlyEnormous)) // gueltiger Nachbar ?! { z=GetMap(v,w); // Hindernis vorhanden? if ( (z==0)&&(!(GetList(v,w)==2))) // frei und nicht in geschlossener Liste { F=GetF(xa,ya)+k; // F: Kosten zu (v,w) ueber aktuelles Quadrat (xa,ya)
if (!(GetList(v,w)==1)) // wenn NICHT in offener Liste { SetList (v,w,1); // dann in die offene uebernehmen SetxPrev(v,w, xa); // Vorgaenger: ^x-aktuell SetyPrev(v,w, ya); // Vorgaenger: ^y-aktuell SetdPrev(v,w, d); // Richtung zum Vorgaenger (0-7) if(d==GetdPrev(xa,ya)) F-=2; // Kosten geringer, wenn keine Kursaenderung SetF(v,w,F); // Kosten gesamt } // if (Nicht offen) else if (GetList(v,w)==1) // wenn aber bereits in offener Liste { if (F<=GetF(v,w)) // Wegekosten G dorthin vergleichen: // neuer Weg dahin: geringere Kosten als alte Wegekosten ? { // dann ueberschreiben + neuen Weg im Quadrat einspeichern SetxPrev(v,w, xa); // Vorgaenger: ^x-aktuell SetyPrev(v,w, ya); // Vorgaenger: ^y-aktuell SetdPrev(v,w, d); // Richtung zum Vorgaenger (0-7) if(d==GetdPrev(xa,ya)) F-=2;// Kosten geringer, wenn keine Kursaenderung SetF(v,w,F); // Kosten gesamt }// if (neuer Weg besser) } // else if (schon offen)
}// if (nicht besetzt, nicht geschlossen) } // if (gueltiger Nachbar)
}// for
}//ExpandNode
//******************************************************************** //********************************************************************
void A_Stern (short xStart, short yStart, short xZiel, short yZiel) { short i, j, c=0, xAkt, yAkt, xOld, yOld, xd, yd, xDa, yDa, FMin=MadlyEnormous;
xAkt=xZiel; // wir starten rueckwaerts und beginnen die Suche beim Ziel ! yAkt=yZiel; // Das Quadrat, von dem aus gesucht wird, heisst "aktuelles Quadrat"
SetList(xAkt, yAkt, 1); // akt. Quadrat in Offene Liste (=1) SetF (xAkt,yAkt,0); // Kosten F=G+H; G=0, H=0; SetList(xAkt, yAkt, 2); // aktuelles Quadrat in geschlossene Liste uebernehmen xd=abs(xZiel-xStart); yd=abs(yZiel-yStart); // solange das andere Ende (der Start) nicht gefunden wurde:
while (GetList(xStart,yStart)!=2) { FMin=MadlyEnormous; xOld=xAkt; yOld=yAkt; // suche in offener Liste (=1)das Feld mit kleinstem F
if ((xZiel>=xStart)&&(yZiel>=yStart)) { for (i=-limit;i<limit;i++) { for (j=-limit;j<limit;j++) { if ((GetList(i,j)==1)&&(GetF(i,j)<FMin) ) { FMin=GetF(i,j); xAkt=i; // wenn gefunden: nimm es als aktuelles Quadrat yAkt=j; xDa=abs(i-xStart); yDa=abs(j-yStart); if ((xDa<xd)||(yDa<yd)) { SetList(xAkt, yAkt, 2); // aktuelles Quadrat in geschlossene Liste SetPixel(xAkt,yAkt); ExpandNode(xAkt, yAkt); // Nachbarfelder untersuchen c+=1; } } // if } // for j SetList(xAkt, yAkt, 2); // aktuelles Quadrat in geschlossene Liste SetPixel(xAkt,yAkt); ExpandNode(xAkt, yAkt); // Nachbarfelder untersuchen c+=1; } // for i
} // if (Richtung) else if ((xZiel<xStart)&&(yZiel<yStart)){ for (i=limit-1;i>=(-limit);i-=1) { for (j=limit-1;j>=(-limit);j-=1) { if ((GetList(i,j)==1)&&(GetF(i,j)<FMin) ) { FMin=GetF(i,j); xAkt=i; // wenn gefunden: nimm es als aktuelles Quadrat yAkt=j; xDa=abs(i-xStart); yDa=abs(j-yStart); if ((xDa<xd)||(yDa<yd)) { SetList(xAkt, yAkt, 2); // aktuelles Quadrat in geschlossene Liste SetPixel(xAkt,yAkt); ExpandNode(xAkt, yAkt); // Nachbarfelder untersuchen c+=1; } } // if } // for j SetList(xAkt, yAkt, 2); // aktuelles Quadrat in geschlossene Liste SetPixel(xAkt,yAkt); ExpandNode(xAkt, yAkt); // Nachbarfelder untersuchen c+=1; } // for i
} // if (Richtung)
else if ((xZiel<xStart)&&(yZiel>=yStart)) { for (i=limit-1;i>=(-limit);i-=1) { for (j=-limit;j<limit;j+=1) { if ((GetList(i,j)==1)&&(GetF(i,j)<FMin) ) { FMin=GetF(i,j); xAkt=i; // wenn gefunden: nimm es als aktuelles Quadrat yAkt=j; xDa=abs(i-xStart); yDa=abs(j-yStart); if ((xDa<xd)||(yDa<yd)) { SetList(xAkt, yAkt, 2); // aktuelles Quadrat in geschlossene Liste SetPixel(xAkt,yAkt); ExpandNode(xAkt, yAkt); // Nachbarfelder untersuchen c+=1; } } // if } // for j SetList(xAkt, yAkt, 2); // aktuelles Quadrat in geschlossene Liste SetPixel(xAkt,yAkt); ExpandNode(xAkt, yAkt); // Nachbarfelder untersuchen c+=1; } // for i
} // if (Richtung)
else if ((xZiel>=xStart)&&(yZiel<yStart)) { for (i=-limit;i<limit;i+=1) { for (j=limit-1;j>=(-limit);j-=1) { if ((GetList(i,j)==1)&&(GetF(i,j)<FMin) ) { FMin=GetF(i,j); xAkt=i; // wenn gefunden: nimm es als aktuelles Quadrat yAkt=j; xDa=abs(i-xStart); yDa=abs(j-yStart); if ((xDa<xd)||(yDa<yd)) { SetList(xAkt, yAkt, 2); // aktuelles Quadrat in geschlossene Liste SetPixel(xAkt,yAkt); ExpandNode(xAkt, yAkt); // Nachbarfelder untersuchen c+=1; } } // if } // for j SetList(xAkt, yAkt, 2); // aktuelles Quadrat in geschlossene Liste SetPixel(xAkt,yAkt); ExpandNode(xAkt, yAkt); // Nachbarfelder untersuchen
c+=1; } // for i
} // if (Richtung) println(1, "Akt(X,Y)%4d%4d",xAkt,yAkt); // Anzeige von xAkt, yAkt printXY(46, 20, "Wege=%4d",c); // Anzeige der Zahl der Zyklen/Wege
if((xAkt==xOld)&&(yAkt==yOld)&&(FMin==MadlyEnormous)) { println(2,"%s","No Way!"); PlaySound(soundBeepBeep); Wait10Msec(200); break; }
} // while
printXY(46,10, "fertig %s",""); // Anzeige, wenn fertig ClearPixel(xStart,yStart); Wait10Msec(200);
}
//******************************************************************** //********************************************************************
sub DrawMap() // Hindernisse und Start/Ziel zeichnen { short x, y;
nxtDrawRect(0, MapSize+1, MapSize+1, 0); // Umrandung
//========================================================================== x=8; // Hindernis setzen: for (y=-3;y<=5;y++) // Hier kann man sich austoben und beliebige { // Hindernis-Punkte definieren: SetMap(x,y,occ); // a) in Karte eintragen (occ=1=besetzt) SetPixel(x,y); // b) auf Display malen } y=9; for (x=-8;x<=7;x++) // nochmal weitere { // Hindernis-Punkte definieren: SetMap(x,y,occ); // a) in Karte eintragen (occ=1=besetzt) SetPixel(x,y); // b) auf Display malen }
y=-5; for (x=0;x<=5;x++) // nochmal weitere { // Hindernis-Punkte definieren: SetMap(x,y,occ); // a) in Karte eintragen (occ=1=besetzt) SetPixel(x,y); // b) auf Display malen }
y=1; for (x=10;x<=12;x++) // nochmal weitere { // Hindernis-Punkte definieren: SetMap(x,y,occ); // a) in Karte eintragen (occ=1=besetzt) SetPixel(x,y); // b) auf Display malen }
y=6; for (x=8;x<=14;x++) // nochmal weitere { // Hindernis-Punkte definieren: SetMap(x,y,occ); // a) in Karte eintragen (occ=1=besetzt) SetPixel(x,y); // b) auf Display malen }
//==========================================================================
SetPixel(xStart,yStart); // Start als Punkt malen DrawRect(xZiel, yZiel,1); // Ziel als Viereck malen
}
//******************************************************************** //********************************************************************
void WalkThisWay() // virtuelle Fahrt der berechneten Route { short x, y, xn, yn, dir, olddir, turn;
x=xStart; y=yStart; dir=0; olddir=0; turn=0; //printXY(46,10,"Kost=%4d",GetF(xStart,yStart)); println(0, "Turn=%4d",turn); println(1, "Kurs=%4d",dir); println(2, "Pos XY%3d%3d",x,y); wait1Msec(1000);
while (! ((x==xZiel)&&(y==yZiel))) { println(2, "Pos XY%3d%3d",x,y); SetPixel(x, y); wait1Msec(500);
dir=180+(45*GetdPrev(x,y)); if (dir>=360) dir-=360;
turn=dir-olddir; if (turn<-180) turn=turn+360; if (turn>180) turn=360-turn;
println(0, "Turn=%4d",turn); wait1Msec(500);
println(1, "Kurs=%4d",dir); wait1Msec(500);
olddir=dir; xn=GetxPrev(x,y); yn=GetyPrev(x,y); x=xn; y=yn;
} SetPixel(x, y); println(2, "Pos XY%3d%3d",x,y); printXY(46,30, "%s","Ziel"); printXY(46,20, "%s","erreicht!");
}
//********************************************************************
sub initVar() { short i,j;
// verschiedene Start/Ziel-Kombinationen
xStart=0; yStart=0; // Startpunkt definieren xZiel=13; yZiel=4; // Zielpunkt definieren
//xStart=13; yStart=4; // Startpunkt definieren //xZiel=0; yZiel=0; // Zielpunkt definieren
//xStart=0; yStart=4; // Startpunkt definieren //xZiel=13; yZiel=0; // Zielpunkt definieren
//xStart=13; yStart=0; // Startpunkt definieren //xZiel=0; yZiel=4; // Zielpunkt definieren
for(i=0;i<MapSize;i++) { for (j=0;j<MapSize;j++) { Map[i][j]=0; // Felder frei xPrev[i][j]=0; // Vorgaenger Nullpunkt yPrev[i][j]=0; F[i][j]=MadlyEnormous; // Kosten gewaltig List[i][j]=0; // Liste undefiniert } } } //********************************************************************
task main() { eraseDisplay(); initVar(); DrawMap();
A_Stern(xStart,yStart,xZiel, yZiel);
while(true) { eraseDisplay(); DrawMap(); WalkThisWay(); wait1Msec(2000); } } //******************************************************************** //********************************************************************
|  |  |  |  |
|  |  |  |  |
English Version  |  |  |  | Code: / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / TamTam.c / / Version 1.0.7 "Dijkstra" / / Virtual path search on the display / / ************************************************ *************************** / / The A * (A-Star, A-star) - algorithm for the NXT / / For path finding and navigation past obstacles / / ************************************************ *************************** / / Here in the variant of Dijkstra / / (No estimated heuristic cost H, ie H = 0 => F = G! / / / / 1.0.4 at the end of route instructions are created / / 1.0.1 detects when no route to the destination exists / / 0.8.9 optimized search direction: direct paths are found very quickly / / 0.3.8 Courses optimized price changes lead to higher costs / / 0.2.9 cutting-edge obstacle is avoided / / (C) Helmut W. 2008 / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
# Include "RobotC +. H"
# Define println nxtDisplayTextLine # Define printXY nxtDisplayStringAt / / ************************************************ ******************** short Xstart, yStart, xZiel, yZiel;
const short MadlyEnormous = 10000; const short occ = 1;
const short MapSize = 30, / / 30x30 fields, each 30cm plus "edge" limit = const short MapSize / 2;
short Map [MapSize +1] [MapSize +1] / / 1 = busy / free = 0; short xPrev [MapSize +1] [MapSize +1] / / abscissa of predecessor (x) short yPrev [MapSize +1] [MapSize +1] / / ordinate of predecessors (y) short dPrev [MapSize +1] [MapSize +1] / / direction to predecessor (d = 0-7) short F [MapSize] [MapSize] / / total path cost F short list [MapSize] [MapSize] / / list belongs to: undef = 0 open = closed = 1 2
/ / ************************************************ ********************
short GetMap (short x, short y) / / state of the field: 0 = free, 1 = obstacle {Return map [x + limit] [y + limit];}
void SetMap (short x, short y, short val) {Map [+ limit x] [y + limit] = val;}
GetxPrev short (short x, short y) / / stored predecessor: x-coord. XPrev {return [x + limit] [y + limit];}
void SetxPrev (short x, short y, short XPTR) XPrev {[x + limit] [y + limit] = XPTR;}
GetyPrev short (short x, short y) / / stored predecessor: y-coord. YPrev {return [x + limit] [y + limit];}
void SetyPrev (short x, short y, short yPtr) YPrev {[x + limit] [y + limit] = yPtr;}
GetdPrev short (short x, short y) / / stored predecessor: Direction DPrev {return [x + limit] [y + limit];}
void SetdPrev (short x, short y, short d) DPrev {[x + limit] [y + limit] = d;}
GETF short (short x, short y) / / path cost F {Return F [x + limit] [y + limit];}
void SetF (short x, short y, short val) {F [x + limit] [y + limit] = val;}
short GetList (int x, int y) / / List component parts (0, 1, 2) {Return List [+ limit x] [y + limit];}
SetList void (int x, int y, int val) {List [+ limit x] [y + limit] = val;}
/ / ************************************************ ******************** / / ************************************************ ********************
void SetPixel (short x, short y) / / transformation of the coordinate systems { int xPix, yPix;
xPix = limit +1 + x; yPix = limit +1 + y; nxtSetPixel (xPix, yPix); }
/ / ************************************************ ********************
void drawRect (short x, short y, short rad) / / Draw a square around the center { int xPix, yPix;
xPix = limit + x +1 / / CenterX yPix = limit + y +1; / / CenterY nxtDrawRect (xPix wheel, yPix + rad + rad xPix, yPix-rad); }
/ / ************************************************ ********************
void Clear pixels (short x, short y) Delete / / map pixels { int xPix, yPix;
xPix = limit +1 + x; yPix = limit +1 + y; nxtClearPixel (xPix, yPix); }
/ / ************************************************ ******************** / / ************************************************ ********************
int GetNx (short x, short pos) / / 3 1 2 adjacent x-coordinate {/ / 4 0 int nx, / / 5 6 7
nx = MadlyEnormous / / if invalid neighbor if (((pos == 1) | | (pos == 0) | | (pos == 7)) && (x <limit-1)) {Nx = x +1;} else if ((pos == 2) | | (pos == 6)) {Nx = x;} else if (((pos == 3) | | (pos == 4) | | (pos == 5)) && (x> (-limit))) {Nx = x-1;}
return nx; }
/ / ************************************************ ********************
int GetNy (short y, short pos) / / 3 1 2 adjacent y coordinates {/ / 4 0 int ny, / / 5 6 7
ny = MadlyEnormous / / if invalid neighbor if (((pos == 1) | | (pos == 2) | | (pos == 3)) && (y <limit-1)) {Ny = y +1;} else if ((pos == 4) | | (pos == 0)) Ny = {y;} else if (((pos == 5) | | (pos == 6) | | (pos == 7)) && (y> (-limit))) {Ny = y-1;}
return ny; }
/ / ************************************************ ********************
CloseToTheEdge bool (short x, short y, short you) / / 3 1 2 cutting-edge no obstacle {/ / 4 0 bool edge; / / 5 6 7 short xm, xp, ym, yp;
edge = false, / / only take into account diagonal step)!
xm = (xm>-limit x-1: x?); xp = (xp <limit-1 x +1: x?); ym = (ym>-limit y-1: y?); yp = (yp <1 limit-y +1, y?);
if (dir == 1) {edge = ((GetMap (xp, y) == occ) | | (GetMap (x, y p) == occ));} / / 0 2 else if (dir == 3) {edge = ((GetMap (x, y p) == occ) | | (GetMap (xm, y) == occ));} / / 2 4 else if (dir == 5) {edge = ((GetMap (xm, y) == occ) | | (GetMap (x, ym) == occ));} / / 4 6 else if (dir == 7) {edge = ((GetMap (x, ym) == occ) | | (GetMap (xp, y) == occ));} / / 6 0
return edge; }
/ / ************************************************ ********************
void expandNode (short xa, ya short) / / check the neighboring fields of (xa, ya) { short F, d, k, v, w, z;
for (d = 0, d <= 7, d + +) { GetNx v = (xa, d); go through / / neighbors: 3 2 1 w = GetNy (ya, d) / / 4 0 / / 5 6 7 if ((d == 0) | | (d == 2) | | (d == 4) | | (d == 6)) / / ((p% 2) == 0 is faulty!) {K = 10} / / costs straight step: 10 else { k = 14, / / costs diagonal step: 15 if (CloseToTheEdge (xa, ya, d)) continue; / / diagonal is prohibited on obstacle edge }
if ((v <MadlyEnormous) && (w <MadlyEnormous)) / / Valid national neighbor? { z = GetMap (v, w) / / obstacle exists? if ((z == 0) && (! (GetList (v, w) == 2))) / / free and not in a closed list { GETF F = (xa, ya) + k / / F: cost (v, w) over current square (xa, ya)
if ((GetList (v, w) == 1)) / / if NOT open list { SetList (v, w, 1) / / then take over the open SetxPrev (v, w, xa), / / Predecessor: ^ x-date SetyPrev (v, w, ya), / / Predecessor: ^ y-current SetdPrev (v, w, d) / / direction to the predecessor (0-7) if (d == GetdPrev (xa, ya)) F = 2, / / cost less if no Last change Set Button F (v, w, F) / / Total cost of } / / If (not open) else if (GetList (v, w) == 1) / / but if already in the open list { if (F <= GETF (v, w)) / / infrastructure costs compare G there: / / New way there: lower costs than old road costs? {/ / Then storing overwrite + new path in the square SetxPrev (v, w, xa), / / Predecessor: ^ x-date SetyPrev (v, w, ya), / / Predecessor: ^ y-current SetdPrev (v, w, d) / / direction to the predecessor (0-7) if (d == GetdPrev (xa, ya)) F = 2 ;/ / cost less if no Last change Set Button F (v, w, F) / / Total cost of } / / If (new way better) } / / Else if (already open)
} / / If (not open, not closed) } / / If (Valid national neighbor)
} / / For
} / / ExpandNode
/ / ************************************************ ******************** / / ************************************************ ********************
void A_Stern (Xstart short, short yStart, xZiel short, short yZiel) { short i, j, c = 0, xAkt, YAKT, xOld, yold, xd, yd, XDA, YDA, FMin = MadlyEnormous;
xAkt xZiel = / / we start backwards and start the search at the goal! YAKT yZiel = / / The square, from which is sought is called "current square"
SetList (xAkt, YAKT, 1) / / act. Square in Open list (= 1) SetF (xAkt, YAKT, 0) / / costs F = G+ H, G = 0, H = 0; Take over / / current square in closed list; SetList (xAkt, YAKT, 2) xd = abs (xZiel-Xstart); yd = abs (yZiel-yStart); / / While the other end (the start) was not found:
while (GetList (Xstart, yStart)! = 2) { FMin = MadlyEnormous; xOld = xAkt; yold = YAKT; / / Search in the open list (= 1) is the smallest field with F
if ((xZiel> = Xstart) && (yZiel> = yStart)) { for (i =-limit, i <limit; i + +) { for (j =-limit j <limit j + +) { if ((GetList (i, j) == 1) && (GETF (i, j) <FMin)) { GETF FMin = (i, j); xAkt = i / / if found: take it as the current square YAKT = j; XDA = abs (i-Xstart); YDA = abs (j-yStart); if ((XDA <xd) | | (YDA <yd)) { SetList (xAkt, YAKT, 2) / / current list enclosed in square SetPixel (xAkt, YAKT); Examine / / neighboring fields; expandNode (xAkt, YAKT) + c = 1; } } / / If } / / For j SetList (xAkt, YAKT, 2) / / current list enclosed in square SetPixel (xAkt, YAKT); Examine / / neighboring fields; expandNode (xAkt, YAKT) + c = 1; } / / For i
} / / If (direction) else if ((xZiel <Xstart) && (yZiel <yStart)) { for (i = limit-1; i> = (-limit), i = 1) { for (j = limit-1; j> = (-limit), j = 1) { if ((GetList (i, j) == 1) && (GETF (i, j) <FMin)) { GETF FMin = (i, j); xAkt = i / / if found: take it as the current square YAKT = j; XDA = abs (i-Xstart); YDA = abs (j-yStart); if ((XDA <xd) | | (YDA <yd)) { SetList (xAkt, YAKT, 2) / / current list enclosed in square SetPixel (xAkt, YAKT); Examine / / neighboring fields; expandNode (xAkt, YAKT) + c = 1; } } / / If } / / For j SetList (xAkt, YAKT, 2) / / current list enclosed in square SetPixel (xAkt, YAKT); Examine / / neighboring fields; expandNode (xAkt, YAKT) + c = 1; } / / For i
} / / If (direction)
else if ((xZiel <xStart)&&(yZiel> yStart =)) { for (i = limit-1; i> = (-limit), i = 1) { for (j =-limit j <limit j + = 1) { if ((GetList (i, j) == 1) && (GETF (i, j) <FMin)) { GETF FMin = (i, j); xAkt = i / / if found: take it as the current square YAKT = j; XDA = abs (i-Xstart); YDA = abs (j-yStart); if ((XDA <xd) | | (YDA <yd)) { SetList (xAkt, YAKT, 2) / / current list enclosed in square SetPixel (xAkt, YAKT); Examine / / neighboring fields; expandNode (xAkt, YAKT) + c = 1; } } / / If } / / For j SetList (xAkt, YAKT, 2) / / current list enclosed in square SetPixel (xAkt, YAKT); Examine / / neighboring fields; expandNode (xAkt, YAKT) + c = 1; } / / For i
} / / If (direction)
else if ((xZiel> = Xstart) && (yZiel <yStart)) { for (i =-limit, i <limit; i + = 1) { for (j = limit-1; j> = (-limit), j = 1) { if ((GetList (i, j) == 1) && (GETF (i, j) <FMin)) { GETF FMin = (i, j); xAkt = i / / if found: take it as the current square YAKT = j; XDA = abs (i-Xstart); YDA = abs (j-yStart); if ((XDA <xd) | | (YDA <yd)) { SetList (xAkt, YAKT, 2) / / current list enclosed in square SetPixel (xAkt, YAKT); Examine / / neighboring fields; expandNode (xAkt, YAKT) + c = 1; } } / / If } / / For j SetList (xAkt, YAKT, 2) / / current list enclosed in square SetPixel (xAkt, YAKT); Examine / / neighboring fields; expandNode (xAkt, YAKT)
+ c = 1; } / / For i
} / / If (direction) println (1, "act (X, Y)% 4d% 4d", xAkt, YAKT) / / display of xAkt, YAKT printXY (46, 20, "paths =% 4d", c); / / display the number of cycles / paths
if ((== xAkt xOld) && (YAKT == yold) && (FMin == MadlyEnormous)) { println (2, "% s", "No Way!"); PlaySound (soundBeepBeep); Wait10Msec (200); break; }
} / / While
printXY (46,10, "% s finished", "") / / display when finished Clear pixels (Xstart, yStart); Wait10Msec (200);
}
/ / ************************************************ ******************** / / ************************************************ ********************
draw drawmap sub () / / obstacles and start / finish { short x, y;
nxtDrawRect (0, +1 MapSize, MapSize +1, 0), / / Border
/ / ================================================ ========================== x = 8, / / Set obstacle: for (y = -3, y <= 5, y + +) / / Here you can have fun and any Set {/ / obstacle points: SetMap (x, y, occ); / / a) enter in card (occ = 1 = occupied) Paint / / b) on display; SetPixel (x, y) } y = 9; for (x = -8, x <= 7, x + +) / / again more Set {/ / obstacle points: SetMap (x, y, occ); / / a) enter in card (occ = 1 = occupied) Paint / / b) on display; SetPixel (x, y) }
y = -5; for (x = 0, x <= 5, x + +) / / again more Set {/ / obstacle points: SetMap (x, y, occ); / / a) enter in card (occ = 1 = occupied) Paint / / b) on display; SetPixel (x, y) }
y = 1; for (x = 10, x <= 12, x + +) / / again more Set {/ / obstacle points: SetMap (x, y, occ); / / a) enter in card (occ = 1 = occupied) Paint / / b) on display; SetPixel (x, y) }
y = 6; for (x = 8, x <= 14, x + +) / / again more Set {/ / obstacle points: SetMap (x, y, occ); / / a) enter in card (occ = 1 = occupied) Paint / / b) on display; SetPixel (x, y) }
/ / ================================================ ==========================
Paint / / start as a point; SetPixel (Xstart, yStart) Paint / / target as a square; drawRect (xZiel, yZiel, 1)
}
/ / ************************************************ ******************** / / ************************************************ ********************
void WalkThisWay () / / virtual ride the calculated route { short x, y, xn, yn, you olddir, turn;
x = Xstart; y = yStart; dir = 0; olddir = 0; turn = 0; / / PrintXY (46.10, "food =% 4d", GETF (Xstart, yStart)); println (0, "Turn =% 4d", turn); println (1, "price =% 4d", dir); println (2, "Pos XY% 3d% 3d", x, y); wait1Msec (1000);
while (! ((x == xZiel) && (y == yZiel))) { println (2, "Pos XY% 3d% 3d", x, y); SetPixel (x, y); wait1Msec (500);
dir = 180 + (45 * GetdPrev (x, y)); if (dir> = 360) dir = 360;
turn = dir olddir; if (turn <-180) turn = turn +360; if (turn> 180) turn = 360 turn;
println (0, "Turn =% 4d", turn); wait1Msec (500);
println (1, "price =% 4d", dir); wait1Msec (500);
olddir = dir; GetxPrev = xn (x, y); GetyPrev yn = (x, y); x = x; y = yn;
} SetPixel (x, y); println (2, "Pos XY% 3d% 3d", x, y); printXY (46,30, "% s", "target"); printXY (46,20, "% s", "reach");
}
/ / ************************************************ ********************
initVar sub () { short i, j;
/ / Different start / finish combinations
Xstart = 0; yStart = 0; / / Define start point xZiel = 13; yZiel = 4, / / define the target point
/ / Xstart = 13; yStart = 4 / / define start point / / XZiel = 0; yZiel = 0 / / Define target point
/ / Xstart = 0; yStart = 4 / / define start point / / XZiel = 13; yZiel = 0 / / Define target point
/ / Xstart = 13; yStart = 0 / / Define start point / / XZiel = 0; yZiel = 4, / / define the target point
for (i = 0; i <MapSize, i + +) { for (j = 0; j <MapSize j + +) { Map [i] [j] = 0 / / clear fields xPrev [i] [j] = 0, / / Predecessor zero yPrev [i] [j] = 0; F [i] [j] = MadlyEnormous / / huge costs List [i] [j] = 0, / / undefined list } } } / / ************************************************ ********************
task main () { eraseDisplay (); initVar (); Drawmap ();
A_Stern (Xstart, yStart, xZiel, yZiel);
while (true) { eraseDisplay (); Drawmap (); WalkThisWay (); wait1Msec (2000); } } / / ************************************************ ******************** / / ************************************************ ********************
|  |  |  |  |
|
Wed Jul 24, 2013 2:23 am |
|
 |
Azhari
Rookie
Joined: Thu Mar 14, 2013 10:32 pm Posts: 25 Location: Malaysia
|
 Re:
 |  |  |  | Ford Prefect wrote: this is an additional file which is included and needed for some variable definitions a.s.o.:  |  |  |  | Code: /////////////////////////////////////////////////////////////////////// // RobotC+.h - damit's ein bisserl einfacher wird... ;-) // // Version 0.5.8 ///////////////////////////////////////////////////////////////////////
//=================================================================== // mathematische Konstanten und Funktionen //===================================================================
// Eulersche Zahl E: const float E=2.718281828450945235360287471352662497757247093699959574966967627724076630353547594571;
//********************************************
int min(int a, int b) { int m; m=(a<b ? a:b); return m; }
//********************************************
float min(float a, float b) { float m; m=(a<b ? a:b); return m; }
//********************************************
int max(int a, int b) { int m; m=(a>b ? a:b); return m; }
//********************************************
float max(float a, float b) { float m; m=(a>b ? a:b); return m; }
//********************************************
int round(float f) { if(f>0) return (int)(f + 0.5); else return (int)(f - 0.5); }
//********************************************
// ArcusTangens mit Sonderfaellen; Angabe in Grad! // x=Ankathete y=Gegenkathete Tangens=y/x
float atan2Degrees(float x, float y) { float phi, alpha; //phi=Bogenmass, alpha=Grad;
if (x>0) {phi=atan(y/x);} else if ((x<0)&&(y>=0)) {phi=PI+atan(y/x);} else if ((x<0)&&(y<0)) {phi=-PI+atan(y/x);} else if ((x==0)&&(y>0)) {phi=PI/2;} else if ((x==0)&&(y<0)) {phi=-PI/2;} else if ((x==0)&&(y==0)) {phi=0;}
alpha=radiansToDegrees(phi); return alpha; }
//********************************************
bool IsInRange(int Wert, int Basis, int Toleranz) { return((Wert>=Basis-Toleranz) && (Wert<=Basis+Toleranz)); }
//=================================================================== // Prozeduren fuer Sensoren //===================================================================
bool SMuxPressed(int port, byte ch) // MUX-Port an NXT (S1, S2,... ) // ch: 0=direkt, 1=ch1, 2=ch2, 3=ch3
{ int v; bool pressed;
v=SensorValue(port); if (ch==0) { pressed=(v<100 ); } else if (ch==1) { pressed=( IsInRange(v,775,30) || IsInRange(v,523,25) || IsInRange(v,343,8) || IsInRange(v,275,8)) ; } else if (ch==2) { pressed=( IsInRange(v,620,40) || IsInRange(v,523,25) || IsInRange(v,295,8) || IsInRange(v,275,8)) ; } else if (ch==3) { pressed=( IsInRange(v,363,15) || IsInRange(v,343,8) || IsInRange(v,295,8) || IsInRange(v,275,8)) ; } return pressed;
// Wertetabelle fuer Sensor-Muxer (natuerlich Bauform-abhaengig) //************************************************************* // // rcx ch3 ch2 ch1 raw // RCX-WiderstMux als sensorRawValue definiert // 0 0 0 0 1023 // an 3 Mux-Einaenge UND direktan rcx-Eingang // 0 0 0 1 775 // je 1 Taster angeschlossen // 0 0 1 0 620 // 0 0 1 1 523 // 0 1 0 0 363 // 0 1 0 1 343 // 0 1 1 0 295 // 0 1 1 1 275 // 1 0 0 0 79 // ab hier die Werte, wenn der // 1 0 0 1 64 // 4. Taster direkt am rcx-Sensor-Eingang // 1 0 1 0 74 // gedrueckt wird. // 1 0 1 1 70 // Eine Unterscheidung, ob dann auch // 1 1 0 0 69 // gleichzeitig noch andere Taster // 1 1 0 1 55 // zusaetzlich gedrueckt sind, // 1 1 1 0 55 // ist dann kaum mehr moeglich. // 1 1 1 1 55 //
}
//=================================================================== // globale Prozeduren fue Motor-Steuerung //===================================================================
void motSync (int m, int s) // setzt Sync-Master & Slave { if ((m==0) && (s==0)) { nSyncedMotors = synchNone;} else if ((m==0) && (s==1)) { nSyncedMotors = synchAB;} else if ((m==0) && (s==2)) { nSyncedMotors = synchAC;} else if ((m==1) && (s==0)) { nSyncedMotors = synchBA;} else if ((m==1) && (s==2)) { nSyncedMotors = synchBC;} else if ((m==2) && (s==0)) { nSyncedMotors = synchCA;} else if ((m==2) && (s==1)) { nSyncedMotors = synchCB;} else nSyncedMotors = synchNone; }
//=================================================================== //===================================================================
|  |  |  |  |
|  |  |  |  |
English Version  |  |  |  | Code: / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / RobotC + h -.'s A little something that will be easier ... ;-) / / / / Version 0.5.8 / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
/ / ================================================ =================== / / Mathematical constants and functions / / ================================================ ===================
/ / Euler number e: const float e = 2.718281828450945235360287471352662497757247093699959574966967627724076630353547594571;
/ / ********************************************
int min (int a, int b) { int m; m = (a <b, a: b?); return m; }
/ / ********************************************
float min (float a, float b) { float m; m = (a <b, a: b?); return m; }
/ / ********************************************
int max (int a, int b) { int m; m = (a> b a: b?) return m; }
/ / ********************************************
float max (float a, float b) { float m; m = (a> b a: b?) return m; }
/ / ********************************************
int round (float f) { if (f> 0) return (int) (f + 0.5); else return (int) (f - 0.5); }
/ / ********************************************
/ / Arc tangent with Sonderfaellen, specified in degrees! / / X = y = opposite side to the adjacent side tan = y / x
atan2Degrees float (float x, float y) { float phi, alpha / / phi = radians, alpha = degree;
if (x> 0) {phi = atan (y / x);} else if ((x <0) && (y> = 0)) {phi = PI + atan (y / x);} else if ((x <0) && (y <0)) {phi = PI + atan (y / x);} else if ((x == 0) && (y> 0)) {phi = PI / 2;} else if ((x == 0) && (y <0)) {= phi-PI / 2;} else if ((x == 0) && (y == 0)) {phi = 0;}
alpha = radiansToDegrees (phi); return alpha; }
/ / ********************************************
IsInRange bool (int value, int base, int tolerance) { return ((value> = base-tolerance) && (value <= base + tolerance)); }
/ / ================================================ =================== / / Procedures for sensors / / ================================================ ===================
SMuxPressed bool (int port, byte ch) / / MUX port on NXT (S1, S2, ...) / / ch: 0 = right, 1 = ch1, ch2 = 2, 3 = ch3
{ int v; bool pressed;
v = sensor value (port); if (ch == 0) Pressed = {(v <100);} else if (ch == 1) {Pressed = (IsInRange (v 775.30) | | IsInRange (v 523.25) | | IsInRange (v, 343.8) | | IsInRange (v, 275.8));} else if (ch == 2) {Pressed = (IsInRange (v, 620,40) | | IsInRange (v 523.25) | | IsInRange (v, 295.8) | | IsInRange (v, 275.8));} else if (ch == 3) {Pressed = (IsInRange (v, 363,15) | | IsInRange (v, 343.8) | | IsInRange (v, 295.8) | | IsInRange (v, 275.8));} return pressed;
/ / Table of values for sensor-muxer (of course design-dependent) / / ************************************************ ************* / / / / Rcx ch1 ch2 ch3 raw / / RCX WiderstMux defined as sensorRawValue / / 0 0 0 0 1023 / / 3 at Mux Einaenge AND rcx other via input / / 0 0 0 1 775 / / 1 each probe connected / / 0 0 1 0 620 / / 0 0 1 1 523 / / 0 1 0 0 363 / / 0 1 0 1 343 / / 0 1 1 0 295 / / 0 1 1 1 275 / / 1 0 0 0 79 / / from here the values when the / / 1 0 0 1 64 / / 4 Button located on the rcx sensor input / / 1 0 1 0 74 / / pressured. / / 1 0 1 1 70 / / A distinction, whether then / / 1 1 0 0 69 / / at the same time, other key / / 1 1 0 1 55 / / are additionally pressured, / / 1 1 1 0 55 / / is then hardly possible. / / 1 1 1 1 55 / /
}
/ / ================================================ =================== / / Global procedures fue motor control / / ================================================ ===================
motSync void (int m, int s) / / sets sync master & slave { if ((m == 0) && (s == 0)) {NSyncedMotors = synchNone;} else if ((m == 0) && (s == 1)) {NSyncedMotors = synchAB;} else if ((m == 0) && (s == 2)) {NSyncedMotors = synchAC;} else if ((m == 1) && (s == 0)) {NSyncedMotors = synchBA;} else if ((m == 1) && (s == 2)) {NSyncedMotors = synchBC;} else if ((m == 2) && (s == 0)) {NSyncedMotors = synchCA;} else if ((m == 2) && (s == 1)) {NSyncedMotors = synchCB;} else nSyncedMotors = synchNone; }
/ / ================================================ =================== / / ================================================ ===================
|  |  |  |  |
|
Wed Jul 24, 2013 2:24 am |
|
|
|
Page 1 of 1
|
[ 11 posts ] |
|
Who is online |
Users browsing this forum: No registered users and 2 guests |
|
You cannot post new topics in this forum You cannot reply to topics in this forum You cannot edit your posts in this forum You cannot delete your posts in this forum You cannot post attachments in this forum
|
|