ROBOTC.net forumshttp://robotc.net/forums/ aStar (a-star, A-Stern) Algorithm for NXT: TamTam.chttp://robotc.net/forums/viewtopic.php?f=15&t=456 Page 1 of 1

 Author: Ford Prefect [ Sun Mar 16, 2008 12:00 pm ] Post subject: 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).

Author:  Ford Prefect [ Sat Mar 22, 2008 2:08 pm ]
Post subject:

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 Fshort 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)))     {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)))     { 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=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)=yStart))    {    for (i=limit-1;i>=(-limit);i-=1)    {        for (j=-limit;j=xStart)&&(yZiel=(-limit);j-=1)       {          if ((GetList(i,j)==1)&&(GetF(i,j)=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

Author:  Ford Prefect [ Sat Mar 22, 2008 2:38 pm ]
Post subject:

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=(ab ? 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/xfloat 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;}//===================================================================//===================================================================

 Author: lololol [ Mon Nov 09, 2009 7:22 pm ] Post subject: 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.

 Author: Azhari [ Mon Jul 22, 2013 10:42 pm ] Post subject: 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

 Author: mightor [ Tue Jul 23, 2013 1:31 am ] Post subject: 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

Author:  Azhari [ Tue Jul 23, 2013 5:33 am ]
Post subject:  Re: aStar (a-star, A-Stern) Algorithm for NXT: TamTam.c

 mightor wrote: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

which language i should use?

 Author: mattallen37 [ Tue Jul 23, 2013 9:38 am ] Post subject: Re: aStar (a-star, A-Stern) Algorithm for NXT: TamTam.c German

Author:  Azhari [ Tue Jul 23, 2013 11:28 am ]
Post subject:  Re: aStar (a-star, A-Stern) Algorithm for NXT: TamTam.c

 mattallen37 wrote:German

thanks

Author:  Azhari [ Wed Jul 24, 2013 2:23 am ]
Post subject:  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 Fshort 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)))     {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)))     { 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=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)=yStart))    {    for (i=limit-1;i>=(-limit);i-=1)    {        for (j=-limit;j=xStart)&&(yZiel=(-limit);j-=1)       {          if ((GetList(i,j)==1)&&(GetF(i,j)=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

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 Fshort 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: DirectionDPrev {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)))     {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)))     {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 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 = 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) yStart =))    {    for (i = limit-1; i> = (-limit), i = 1)    {        for (j =-limit j = Xstart) && (yZiel = (-limit), j = 1)       {          if ((GetList (i, j) == 1) && (GETF (i, j) = 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

Author:  Azhari [ Wed Jul 24, 2013 2:24 am ]
Post subject:  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=(ab ? 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/xfloat 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 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 / xatan2Degrees 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;}/ / ================================================ ===================/ / ================================================ ===================

 Page 1 of 1 All times are UTC - 5 hours [ DST ] Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Grouphttp://www.phpbb.com/