View unanswered posts | View active topics It is currently Mon Aug 20, 2018 11:21 am

 Page 1 of 1 [ 11 posts ]
 Print view Previous topic | Next topic
aStar (a-star, A-Stern) Algorithm for NXT: TamTam.c
Author Message
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
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 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

_________________
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
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=(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;}//===================================================================//===================================================================

_________________
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
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
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
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
Rookie

Joined: Thu Mar 14, 2013 10:32 pm
Posts: 25
Location: Malaysia
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?

Tue Jul 23, 2013 5:33 am
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
Rookie

Joined: Thu Mar 14, 2013 10:32 pm
Posts: 25
Location: Malaysia
Re: aStar (a-star, A-Stern) Algorithm for NXT: TamTam.c
 mattallen37 wrote:German

thanks

Tue Jul 23, 2013 11:28 am
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 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

Wed Jul 24, 2013 2:23 am
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=(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;}/ / ================================================ ===================/ / ================================================ ===================

Wed Jul 24, 2013 2:24 am
Display posts from previous:  Sort by
 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 forumYou cannot reply to topics in this forumYou cannot edit your posts in this forumYou cannot delete your posts in this forumYou cannot post attachments in this forum

Search for:
 Jump to:  Select a forum ------------------ ROBOTC Applications    ROBOTC for LEGO MINDSTORMS       Third-party sensors    ROBOTC for CORTEX & PIC    ROBOTC for VEX IQ    ROBOTC for Arduino    Robot Virtual Worlds    Multi-Robot Communications    Issues and Bugs Competitions & Partners    Mini Urban Challenge    CS2N Robot Virtual Worlds Competitions       VEX Skyrise Competition 2014-2015       VEX Toss Up 2013-2014       FTC Block Party! 2013-2014    Competitions using VEX - BEST, TSA, VEX, and RoboFest!    FTC Programming    RoboCup Junior and Other ROBOT Competitions Virtual Brick Robotics Discussions    General Discussions    Project Discussions Off-Topic ROBOTC Forum & ROBOTC.net Suggestions/Feedback    ROBOTC Forums Suggestions/Comments    ROBOTC.net Suggestions/Comments       NXT Programming: Tips for Beginning with ROBOTC       VEX Programming: Tips for Beginning with ROBOTC    2013 Robotics Summer Of Learning       VEX Toss Up Programming Challenge       FTC Ring It Up! Programming Challenge    International Forums       Spanish Forums          ROBOTC for MINDSTORMS          ROBOTC for VEX       French Forums          ROBOTC pour Mindstorms          ROBOTC pour IFI VEX       Japanese Forums （日本語のフォーラム）       German Forums    2015 Spring Carnival Event    PLTW (Project Lead The Way)    Robotics Merit Badge    2014 Robotics Academy Summer of Learning

Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group.
Designed by ST Software for PTF.