View unanswered posts | View active topics It is currently Sun Aug 31, 2014 2:39 am






Reply to topic  [ 6 posts ] 
Navigation Robot Project 
Author Message
Guru
User avatar

Joined: Sat Mar 01, 2008 12:52 pm
Posts: 1030
Post Navigation Robot Project
Hi everybody,
here's my navigation robot project. It is based on a header file ("navigator.h"), and all functions may be used by any personalized main program; a simple example see below.
RobotC version= 1.10
An extern help file "RobotC+.h" is needed additionally.

Due to better actualization all files are posted isolated.

All questions and proposings are welcome!

Remark: the integration of the HT Compass Sensor is not finished yet.
In the current version, the calculated "odometric" heading is corrected by the compass heading at each stop; in future it may be possible to do this correction always at once even while moving

_________________
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 Fri Jul 25, 2008 3:10 pm, edited 7 times in total.



Sun Mar 16, 2008 11:47 am
Profile
Guru
User avatar

Joined: Sat Mar 01, 2008 12:52 pm
Posts: 1030
Post 
first,
the Help file "RobotC+.h"; some (not all) of the routines (e.g., min, max, round, atan2) are needed for the navigation procedures:
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)}


Last edited by Ford Prefect on Mon Jun 30, 2008 5:56 am, edited 3 times in total.



Sun Mar 16, 2008 11:49 am
Profile
Guru
User avatar

Joined: Sat Mar 01, 2008 12:52 pm
Posts: 1030
Post 
Now, the navigator main file "navigator.h":
Code:
//================================================================//
//                          navigator.h                           //
//                 Navigations-Routinen fuer RobotC               //
//================================================================//
//                         Version 0.7.4.
//
//  0.7.0.: Kompass-Korrektur bei Zwischenstopp
//  0.6.0.: User-Interface Routinen neu
// (c) Helmut W. 2008
//=================================================================//

#include "RobotC+.h";
#include "HTCompassDrv.h"

//=============================================================================================//
// Interface:
//=============================================================================================//

//***************************************************************//
// Initialisierung:
// Uebergabe der Groessenverhaeltnisse des Robots und Motor-Ports
// beim Start des eigenen Hauptprogramms an diesen Navigator:
//***************************************************************//

void InitNavBot (float Raddurchmesser,
                 float _RadAbstand,
                 float LaengeVoraus,
                 short MotorPortRechts,
                 short MotorPortLinks);


//***********************//
// Globale Variablen     //
// (Benutzer-definiert)  //
//***********************//

bool CompassExists;      // soll im Hauptprogramm gesetzt werden bei Kompass-Initialisierung

//**************************//
// globale Motor-Parameter  //
// (Benutzer-definiert)     //
//**************************//

const short mVSlow=15;         // MotorSpeed, haengt vom Getriebe ab
const short mSlow=25;
const short mNorm=40;
const short mFast=60;
const short mMax=100;



//*****************************//
// Antriebs-Steuerung          //
//*****************************//


void StopMotors ();               // muss nach jedem Einzelmanoever aufgerufen werden!
                                  // stoppt + speichert augenblickliche Positionsdaten

void move_cm(float s);            // geradeaus Anzahl cm vor o. zurueck
void forward();                   // vorwaerts bis StopMotors() aufgerufen wird
void reverse();                   // rueckwaerts bis StopMotors() aufgerufen wird

void turn_degr(float Winkel);     // drehen um einen best. Winkel (+) links, (-) rechts herum
void turn_left();                 // links herum bis StopMotors() aufgerufen wird
void turn_right();                // rechts herum bis StopMotors() aufgerufen wird

void GotoXY(float x, float y);       // geht von der aktuellen zur angegeb. Zielposition
void GotoField(short xF, short yF);  // geht ins Zentrum des betr. Feldes
void turn_toXY(float x, float y);    // dreht aus der aktuellen zur angegeb. Zielposition
float GetDistToXY(float x, float y); // berechnet die Weglaenge zur angegeb. Zielposition



//***********************//
// Landkarte             //
//***********************//


short GetMap(int x, int y);             // liest den Zustand des Feldes: 0=frei, 1= Hindernis
void  SetMap(int x, int y, int wert);   // schreibt den Zustand des Feldes

const short MapSize=30;             // Karte hat 30*30 Felder (soll durch 2 teilbar sein)...
const short Kante=30;               // Kantenlaenge von je 30cm (angepasst an Roboter-Groesse!)
                                    // ergibt eine Landkate von ca. 100 Quadratmeter Groesse

// spezielle globale Variablen fuer die Landkarte

short maxXYMap;         // bewegt er sich darueber hinaus, ist das erlaubt,
float maxXYpos;         // die echte Postion wird weiter berechnet
                        // aber nichts mehr in die Landkarte eingetragen
                        // Startpunkt ist (0,0) mit je 18 Feldern
                        // nach links, rechts, oben, unten


//=============================================================================================//
// Private Deklarationen, NICHT AENDERN!
//=============================================================================================//
//***********************//
// PRIVAT:
// Robot-Bewegungsformen //
//***********************//


#define _navMoveStop         0      // beide Mot. synchron stopp
#define _navMoveStraight   100      // beide Mot. synchron in dieselbe Richtung
#define _navMoveTurn      -100      // Mot. synchron gegenläufig links/rechts

int navMoveType=-1;


//************************************//
// Roboter-Messwerte und Anschluesse,
// ueber InitRobot initialisiert
//************************************//

short Mre=1, Mli=2;   // Motor-Ports fuer synchronisierte Motoren rechts und links;

float _DRad=0;        // Durchmesser Antriebsrad in cm, wird im task main initialisiert
float _RadAbst=0;     // Abstand der Antriebraeder in cm (_RadAbstand), muss im Hauptprogramm gesetzt werden
float _LVorn=0;       // Roboter-Laenge von Achse bis zur Spitze (in cm),
                      // wo Hindernisse per Stoss-Sensor oder Ultraschall erkannt werden)

//*******************************//
// Roboter-Messwerte,
// von InitRobot errechnet
//*******************************//


float _cmPerTick=0;     // Weg pro Tick (360 Ticks = 360 Grad = 1 Umdrehung)
float _AnglePerTick=0;  // gedrehter Bogen in Grad pro Tick
int   _TVorn=0;         // Roboter-Laenge (in Motor-Ticks) von Achsmitte bis zur Spitze,
                       // wo Hindernisse per Stoss-Sensor oder Ultraschall erkannt werden)



//*******************************//
// waehrend der Laufzeit
// immer neu errechnete Werte
//*******************************//


float Stretch=0;       // gefahrenes Wegstueck in cm (seit letztem Zwischenstopp)
float DrehWinkel=0;    // gedrehter Winkel in Grad  (seit letztem Zwischenstopp)


float xpos=0, ypos=0;           // errechnete Position in rechtw. Koordinaten
float xAlt=0, yAlt=0;           // Alte Position seit dem letzten Manoever in rechtw. Koordinaten

float xVorn=0, yVorn=0;         // Position der Roboter-Spitze, die ein Hindernis erkennen kann

short Map [MapSize][MapSize];   // Planquadrat-Feld, enthaelt Angaben ueber frei oder besetzt
short xMap=0, yMap=0;           // errechnete aktuelle Planquadrat-Position der Achsmitte
short xFNext=0, yFNext=0;       // errechnete aktuelle Planquadrat-Position der Roboterspitze

float Kurs;                     // errechneter Kurs (bezogen auf x-Achse)
float KKurs;                    // gemessener Kompass-Kurs (Kompass-Sensor, bezogen auf magnet. Nord)
float MKurs;                    // magnetisch (durch Kompass) korrigierter Kurs (bezogen auf x-Achse)
float oKurs;                    // offset Winkel zw. Kurssystem und magnetischem Koordinatensystem

bool TiltFront=false;           // vorne Hindernis erreicht bzw. angestossen



//===============================================================================================//
// Prozeduren
//===============================================================================================//

short GetMap(short x, short y)            // gibt den Zustand des Feldes zurueck: 0=frei, 1= Hindernis
{   return Map[x+(MapSize/2)][y+(MapSize/2)];}

void SetMap(short x, short y, short wert) // schreibt den Zustand des Feldes: 0=frei, 1= Hindernis
{   Map[x+(MapSize/2)][y+(MapSize/2)]=wert;}

//=============================================================================================//
// Kompass-Navigation
//===============================================================================================//

float Compass2MathKurs(float KWert)
{
   float k;

   k= oKurs-KWert;
   if (k<0) k+=360;
   if (k>=360) k-=360;
   return k;
}


//=============================================================================================//

void turn_degr(float Winkel)
{
   int SollTicks, speed;

   navMoveType=_navMoveTurn;

   nSyncedTurnRatio=-100;
   nMotorEncoder[Mre]=0;
   wait1Msec(10);

   SollTicks=round(Winkel/_AnglePerTick)+1;

   while(nMotorEncoder[Mre]!=SollTicks)
   {
     if (abs(nMotorEncoder[Mre]-SollTicks)>20){speed=mSlow;} // anpassen fuer optimales Dreh-
     else speed=mVSlow;                                      // und Bremsverhalten

      if (nMotorEncoder[Mre]<SollTicks)
     {   motor[Mre]=speed;  }
     else
     if (nMotorEncoder[Mre]>SollTicks)
     {   motor[Mre]=-speed; }
   }
   motor[Mre]=0;

}

//=============================================================================================//

void turn_left()   // links herum bis StopMotors() aufgerufen wird
{
   navMoveType=_navMoveTurn;

   nSyncedTurnRatio=-100;
   nMotorEncoder[Mre]=0;
   wait1Msec(10);

  motor[Mre]=mSlow;
}

//=============================================================================================//

void turn_right()   // rechts herum bis StopMotors() aufgerufen wird
{
   navMoveType=_navMoveTurn;

   nSyncedTurnRatio=-100;
   nMotorEncoder[Mre]=0;
   wait1Msec(10);

  motor[Mre]=-mSlow;
}

//=============================================================================================//

void move_cm(float s)
{
  int target, speed;

  nSyncedTurnRatio=100;
  nMotorEncoder[Mre]=0;
  wait1Msec(10);

  navMoveType=_navMoveStraight;

  target=round(s/_cmPerTick);

  while (target!=nMotorEncoder[Mre])
  {
    if (abs(nMotorEncoder[Mre]-target)>20) {speed=mNorm;}  // anpassen fuer optimales Fahr-
    else speed=mVSlow;                                         // und Bremsverhalten
    if (target > nMotorEncoder[Mre]) {motor[Mre]=speed; }
    else
    if (target < nMotorEncoder[Mre]) {motor[Mre]=-speed; }
  }
  motor[Mre]=0;
}


//=============================================================================================//

void forward()   // vorwaerts bis StopMotors() aufgerufen wird
{
  nSyncedTurnRatio=100;
  nMotorEncoder[Mre]=0;
  wait1Msec(10);

  navMoveType=_navMoveStraight;
  motor[Mre]=mNorm;
}

//=============================================================================================//

void reverse()   // rueckwaerts bis StopMotors() aufgerufen wird
{
  nSyncedTurnRatio=100;
  wait1Msec(10);

  nMotorEncoder[Mre]=0;
  navMoveType=_navMoveStraight;
  motor[Mre]=-mNorm;
}

//=============================================================================================//


void StopMotors ()
{
    navMoveType=_navMoveStop;
    motor[Mre]=0;
    wait1Msec(100);  // Zeit zum Bremsen

    nSyncedTurnRatio=100;
    nMotorEncoder[Mre]=0;
    wait1Msec(10);

    xAlt=xpos;
    yAlt=ypos;

    Stretch=0;

    Kurs=Kurs+DrehWinkel;
    DrehWinkel=0;



    if ((CompassExists))
    {
       KKurs=(float)HTCompassValue(CompassPort);
       MKurs=Compass2MathKurs(KKurs);
       Kurs=MKurs;
    }

    while (Kurs>360) {Kurs=Kurs-360;}
    while (Kurs<0) {Kurs=Kurs+360;}
}

//=============================================================================================//

void GotoXY(float x, float y)
{
   float xDelta, yDelta, savKurs, ZielWinkel, ZielStrecke;

   savKurs=Kurs;       // augenblicklichen Kurs speichern

   xDelta=x-xpos;      // relative Position zum Ziel in rechtw. Koordinaten
   yDelta=y-ypos;

   ZielWinkel=atan2Degrees(xDelta, yDelta);  // relative Zielrichtung zwischen beiden Punkten
   turn_degr(ZielWinkel-savKurs);            // drehen auf neuen Kurs!
   StopMotors();                             // Positionsdaten zwischenspeichern

   ZielStrecke =sqrt((xDelta*xDelta)+(yDelta*yDelta));  // Laenge des rel. Vektors zum Ziel
   move_cm(ZielStrecke);                     // ...und losfahren!

}

//=============================================================================================//

void GotoField(short xF, short yF)    // geht ins Zentrum des betr. Feldes
{
   float xc, yc;
   xc=(float)(xF*Kante);
   yc=(float)(yF*Kante);
   GotoXY(xc, yc);
}

//=============================================================================================//


float GetDistToXY(float x, float y) // berechnet Weglaenge (ohne loszufahren!)
{
   float xDelta, yDelta, ZVLaenge;

   xDelta=x-xpos;
   yDelta=y-ypos;

   ZVLaenge =sqrt((xDelta*xDelta)+(yDelta*yDelta));  // Laenge des rel. Vektors zum Ziel

   return(ZVLaenge);    //

}

//=============================================================================================//

void turn_toXY(float x, float y)  // dreht auf die angegebene Zielposition
{
   float xDelta, yDelta, savKurs, ZVWinkel;

   savKurs=Kurs;         // augenblicklichen Kurs speichern

   xDelta=x-xpos;        // relative Position zum Ziel in rechtw. Koordinaten
   yDelta=y-ypos;

   ZVWinkel=atan2Degrees(xDelta, yDelta);  // relative Zielrichtung zwischen beiden Punkten

   turn_degr(ZVWinkel-savKurs);   // ...und losdrehen auf neuen Kurs!

}



//=============================================================================================//


task odometrics ()
{
  int ticks;

  while(true)
  {
      if (navMoveType==_navMoveStraight) // geradeaus
      {
          // zeichnet durchgefuehrte Geradeaus-Fahrten auf
          // Umrechnung der Polar-Koordinaten in rechtwinklige (kartesische)
          // x=r*cos(Kurs); y=r*sin(Kurs); r=Fahrstrecke=Ticks*_cmPerTick

          if ((CompassExists))
          {
            KKurs=(float)HTCompassValue(CompassPort);
            MKurs=Compass2MathKurs(KKurs);
            //Kurs=MKurs;
          }

           ticks=nMotorEncoder[Mre];
          xpos=xAlt + ticks*_cmPerTick*cosDegrees(Kurs);  // ermittelt die Postion der Roboter-Mitte (Achse)
          ypos=yAlt + ticks*_cmPerTick*sinDegrees(Kurs);  //

          if ( (abs(xpos)<maxXYpos) && (abs(ypos)<maxXYpos))
          {
            xMap=round((xpos/Kante));  // (xpos,ypos) umgerechnet in (xMap,yMap)
            yMap=round((ypos/Kante));  // Feld, in dem sich die Roboter-Achsenmitte befindet
            SetMap(xMap, yMap, 0);     // "0" schreiben kann entfallen, wenn bereits mit "0" initialisiert
          }

          xVorn=xAlt + (ticks+_TVorn)*_cmPerTick*cosDegrees(Kurs);  // ermittelt die Postion der Roboter-Spitze
          yVorn=yAlt + (ticks+_TVorn)*_cmPerTick*sinDegrees(Kurs);  // (wo Hindernisse erkannt werden koennen)

          if ( (abs(xVorn)<maxXYpos) && (abs(yVorn)<maxXYpos))
          {
            xFNext=round((xpos/Kante)); // (xVorn,yVorn) umgerechnet in (xFNext,yFNext)
            yFNext=round((ypos/Kante));
            if (TiltFront) SetMap(xFNext, yFNext, 0);   // Feld-Wert vorn an der Spitze =0 fuer "frei";
            else SetMap(xFNext, yFNext, 1);             // falls man vorn anstoesst, wird vorne =1 gesetzt
          }
          Stretch=ticks*_cmPerTick;          // gefahrenes Stretch seit letztem Stopp / Manoever

      }
      else
        if (navMoveType==_navMoveTurn)     //  drehen
        {  DrehWinkel=_AnglePerTick*nMotorEncoder[Mre];    }
        else
        if (navMoveType==_navMoveStop)
        {  wait1Msec(20) ;            }    // warten
  }
  return;
}


//=============================================================================================//

void InitVariables()
{
  int x,y;
  int kh;

  _cmPerTick=(PI*_DRad)/360;      // Wegstrecke pro Tick
  _AnglePerTick=_DRad/_RadAbst;   // Drehwinkel pro Tick
  _TVorn=_LVorn/_cmPerTick;        // Roboter-Laenge (in Motor-Ticks) von Achsmitte bis zur Spitze

  for(x=0;x<MapSize;x++)
  {
      for (y=0;y<MapSize;y++)      // initialisiert alle Kartenfelder
      {Map[x][y]=0;}             // mit Wert =0 fuer "frei"
  }

  maxXYMap=(int)((MapSize/2)-1);
  maxXYpos=(float)maxXYMap*Kante;

  Kurs=0;
  KKurs=0;
  oKurs=0;
  MKurs=0;
  DrehWinkel=0;
  Stretch=0;
  xAlt=xpos=0;
  yAlt=ypos=0;
  nMotorEncoder[Mre]=0;

  if (CompassExists)
  {
     KKurs=(float)HTCompassValue(CompassPort);
     oKurs=KKurs;
     MKurs=Compass2MathKurs(KKurs);
  }

}


//=============================================================================================//
//       InitRobot muss im Hauptprogramm vom Benutzer mit 5 Parametern aufgerufen werden:       //
//=============================================================================================//

void InitNavBot (float Raddurchmesser,
                 float _RadAbstand,
                 float LaengeVoraus,
                 short MotorPortRechts,
                 short MotorPortLinks)
{

   _DRad=Raddurchmesser;       // Zuweisung der uebergebenen Parameter auf die globalen Variablen
   _RadAbst= _RadAbstand;
   _LVorn=LaengeVoraus;

   Mre=MotorPortRechts;
   Mli=MotorPortLinks;
   motSync(Mre,Mli);

   InitVariables();

   StartTask (odometrics);

}
//=============================================================================================//
//=============================================================================================//


_________________
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 Fri Jul 25, 2008 3:08 pm, edited 11 times in total.



Sun Mar 16, 2008 11:50 am
Profile
Guru
User avatar

Joined: Sat Mar 01, 2008 12:52 pm
Posts: 1030
Post 
last but not least, a sample programm which accesses the navigator routines:

Code:

// =================================================================================
// Demo-Programm fuer  Navigations-Routinen
// Version 0.6.0
// notwendige Roboter-Bauform: beliebiger "Tribot" oder "RoverBot"
// (Antriebsachse mit 2 unabhaengigen Antriebsmotoren fuer Raeder oder Raupen)
// =================================================================================

#include "navigator.h"
#define println nxtDisplayString

//===================================================================

task displayValues()
{
   eraseDisplay();
   while (true)
  {

    println(0, "Wegst %5.1f", Stretch);               // Weg seit letzter Kursaenderung (xAlt,yAlt)

    println(2, "Pos %5.1f %5.1f", xpos,  ypos);       // aktuelle Position (xpos,ypos)
    println(3, "Feld%5d%5d", (int)xMap, (int)yMap );  // aktuelles Planfeld (xFeld,yFeld)

    println(5, "Drehw= %5.1f",  DrehWinkel );         // Drehwinkel
    println(6, "Kurs = %5.1f",  Kurs );               // Kurs in Bezug auf x-Achse

  }
  return;
}


//===================================================================

task main ()
{
   InitNavBot (8.2, 13.5, 22.0, 1, 2);  // einmalig aufrufen zum Start des Navigators!
               // Raddurchmesser=8.2cm,
                    // Abstand der Antriebsraeder=13.5cm,
                         // LaengeVoraus (Achse bis Spitze)=22cm
                                // reMotor=SyncMaster=port1=motorB,
                                  // liMotor=SyncSlave=port2=motorC

  StartTask (displayValues);          // optional, kann man natuerlich weglassen

  move_cm(50);           // gehe 50 cm vorwaerts => Position (50, 0)
  StopMotors();          // schreibt die momentane Position und den Kurs
  wait1Msec(4000);       // nur zur besseren Beobachtung der Messwerte (kann wegfallen)

  turn_degr(90);         // drehe linksrum um 90 Grad
  StopMotors();          // schreibt  momentane Position und Kurs
  wait1Msec(4000);       // (s.o., kann wegfallen)

  move_cm(50);           // gehe 50 cm vorwaerts => Position (50, 50) (angenaehert)
  StopMotors();          // schreibt  momentane Position und  Kurs
  wait1Msec(4000);       // (s.o., kann wegfallen)

  GotoXY(10,10);         // Nachhauseweg grob anpeilen
  StopMotors();          // schreibt  momentane Position und Kurs
  wait1Msec(4000);       // (s.o., kann wegfallen)

  GotoXY(0,0);           // ...und jetzt noch das letzte Stueck nach Hause
  StopMotors();          // schreibt  momentane Position und Kurs
  wait1Msec(4000);       // (s.o., kann wegfallen)

  turn_toXY(100,0);      // zurueck in die Ausgangs-Richtung 0 Grad
  StopMotors();          // steht wieder fast wie am Anfang

  while (true)
  { wait1Msec(50);}      // zum Ablesen der Messwerte
}
//===================================================================
//===================================================================

_________________
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 Fri Jul 25, 2008 2:39 pm, edited 1 time in total.



Sun Mar 16, 2008 11:53 am
Profile
Guru
User avatar

Joined: Sat Mar 01, 2008 12:52 pm
Posts: 1030
Post 
updated version of the compass driver used by navigator.h:
Code:
/**************************************************************************
*                       HiTechnic Compass Sensor                          *
*                            HTCompassDrv.h                               *
*                             version 0.72                                *
***************************************************************************/


tSensors CompassPort;

void SetHTCompassSensor(tSensors port)
{
  SensorType[port] = sensorI2CCustomStd;
  wait10Msec(5);
  nI2CBytesReady[port]=0;
  while(nI2CStatus[port]== STAT_COMM_PENDING)
  wait1Msec(2);
  CompassPort=port;
}

//**************************************************************************

int HTCompassValue(tSensors port)
{
   typedef struct{
  byte nMsgSize;
  byte nDeviceAddress;
  byte nLocationPtr;
  byte nSensorMode;
  } TI2C_Output;


  TI2C_Output sOutput;
  int hUpper, hLower, heading;
  byte rMessage[6];

  nI2CBytesReady[port] = 0;
   sOutput.nMsgSize = 2;                   //read the value from the sensor
  sOutput.nDeviceAddress = 0x02;
  sOutput.nLocationPtr = 0x42;
  sendI2CMsg(port, sOutput.nMsgSize, 6);

  while (nI2CStatus[port] == STAT_COMM_PENDING)
  {  wait1Msec(2); }

  if (nI2CStatus[port] == NO_ERR)
  {
    readI2CReply(port, rMessage[0], 6);      //get the compass heading

    hUpper = (rMessage[0] & 0x00FF);
    hLower = (rMessage[1] & 0x00FF);
    heading = (hUpper << 1) | hLower ;    //convert contents of 0x44 0x45
  }
  else
  { heading=nI2CStatus[port]; }

  return heading;
}
//**************************************************************************
//**************************************************************************

_________________
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 29, 2008 7:23 pm
Profile
Guru
User avatar

Joined: Sat Mar 01, 2008 12:52 pm
Posts: 1030
Post 
if the motors run jerky and the bot shakes about,
see this:
mightor wrote:
Did some more testing and you can reduce the jerkiness by changing the nPidUpdateInterval parameter.
Code:
  nSyncedMotors = synchBC;  // Motor ‘C’ is slaved to motor ‘B’
  nPidUpdateInterval = 15;

The default value is apparently 25ms and that doesn't seem to be small enough. However, when you make it <= 15 it is a LOT smoother.

Regards,
Xander

viewtopic.php?p=2176&highlight=#2176

if the Compass Sensor Drive doesn't work with some I²C sensor definitions, see this:
starwarslegokid wrote:
In 1.38, sensorI2CCustomStd is not a valid sensor type.

Try sensorI2CCustom and see if that works. There is also a sensorI2CHiTechnicCompass, that may be right up your ally as well.

Hope This Helps
Scott B-)


viewtopic.php?p=2568&highlight=#2568

_________________
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 Jul 26, 2008 3:30 am
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 6 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.