View unanswered posts | View active topics It is currently Wed Apr 23, 2014 11:51 am






Reply to topic  [ 11 posts ] 
aStar (a-star, A-Stern) Algorithm for NXT: TamTam.c 
Author Message
Guru
User avatar

Joined: Sat Mar 01, 2008 12:52 pm
Posts: 1030
Post 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
Profile
Guru
User avatar

Joined: Sat Mar 01, 2008 12:52 pm
Posts: 1030
Post 
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
Profile
Guru
User avatar

Joined: Sat Mar 01, 2008 12:52 pm
Posts: 1030
Post 
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
Profile
Rookie

Joined: Sat Jan 10, 2009 6:26 pm
Posts: 14
Post 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
Profile
Rookie
User avatar

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


Mon Jul 22, 2013 10:42 pm
Profile
Moderator
Moderator
User avatar

Joined: Wed Mar 05, 2008 8:14 am
Posts: 3114
Location: Rotterdam, The Netherlands
Post 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
Profile WWW
Rookie
User avatar

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

Joined: Thu Sep 29, 2011 11:09 pm
Posts: 184
Location: Michigan USA
Post Re: aStar (a-star, A-Stern) Algorithm for NXT: TamTam.c
German

_________________
Matt


Tue Jul 23, 2013 9:38 am
Profile WWW
Rookie
User avatar

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


thanks


Tue Jul 23, 2013 11:28 am
Profile
Rookie
User avatar

Joined: Thu Mar 14, 2013 10:32 pm
Posts: 25
Location: Malaysia
Post 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
Profile
Rookie
User avatar

Joined: Thu Mar 14, 2013 10:32 pm
Posts: 25
Location: Malaysia
Post 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
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 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

Search for:
Jump to:  



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