Mindstormsforum Dateiupload | Links | Lexikon | NXT Shop | Wettbewerbe |    Seitenreport - Die SEO und Website Analyse

bild
bild
Unbeantwortete Themen | Aktive Themen Aktuelle Zeit: 31. Jul 2014 04:20


Auf das Thema antworten  [ 1 Beitrag ] 
NXC: Navigation mit Odometrie, Gyro, Kompass 
Autor Nachricht
Administrator
Administrator
Benutzeravatar

Registriert: 11. Jan 2006 21:01
Beiträge: 4189
Wohnort: ein kleiner Planet in der Nähe von Beteigeuze
Beitrag NXC: Navigation mit Odometrie, Gyro, Kompass
hi,

Hier der Navigations-Roboter mit Odometrie, Gyro und Kompass, alle Sensoren werden dabei nach Plausibilität ihrer Werte verknüpft.
Ich habe das Programm mal aus dem Gyro-Thread ausgekoppelt, weil diese Version umfangreicher ist.
Danke an ketaljen und Alphasucht für eure Ideen und Diskussionsbeiträge!

Zwischenstand: Tetrix-Tribot-Chassis, Räder mit ca. 8cm Durchmesser, Antrieb 1:2 untersetzt (Motorachse gegenüber Radachsen), Achsenlänge (Radabstand) ca. 30cm;
max. Geschwindigkeit: 360°/1s = ca. 25cm/s.
bei Dreiecksfahrt (3 virtuelle Raumpunkte) über ca. 5m: Fehler am Start/Zielpunkt mit Doppel-Antriebsachse (grob vergleichbar mit Kettenantrieb) 20cm, mit einfacher Antriebsachse (wie bei Tribot) <10cm, meist noch weniger.
Stärkste Fehlerquellen: Gyro-Drift und Kompass-Störfelder in Räumen.
Bei wiederholtem Runden-Fahren (5x, also 25 m ) schwankt die Genauigkeit (Distanzbetrag) um den Start-Zielpunkt im Bereich um +- 2%.
Hier ist der letzte Entwicklungsstand OHNE stochastischen Filter (nur Sensor-Mittelwerte etc.), evtl. kommt bald noch ein Accelerometer (Beschleunigungs-Sensor) dazu, vllt sogar einmal ein Kalmanfilter:
Code:
// Navigator Robot, TriBot architecture
// for NXC, tested with BricxCC 3.3.8.10 (2011-July-03)
// version 0.43
// (c) HaWe 2011
// based on:
// Odometry
// Compass
// Gyro sensor

//--------------------------------------------------
// mechanics, motors + sensors
//--------------------------------------------------

#define WheelDiameter   7.7    // Raddurchmesser
#define WheelGauge     28.8    // Achsbreite (Mittellinie Antriebsräder)
#define WheelGearRatio  0.5    // Untersetzungsgetriebe 1:2


#define Mleft   OUT_B
#define Mright  OUT_C
#define Mboth   OUT_BC
#define Menc(a) MotorRotationCount(a)

#define SMenc            1
#define SEncoderLeft     2
#define SEncoderRight    3
#define SCompass        10
#define SGyro           11
#define SAccel          12
#define SUSFrLeft       20
#define SUSFrRight      21
#define SUSFrMiddle     22
#define SUSLeftSide     23
#define SUSRightSide    24
#define SUSBackw        25


                               // Radumfang
float WheelCircumference   = PI*WheelDiameter;   // 2*PI*r = PI*d

                               // Abrollstrecke pro 1° MotorEncoderTick
float WheelLegPerEncDegree = (PI*WheelDiameter*WheelGearRatio)/360.0;

float MencHdg;                // Heading to Odometry, geographical sense

float fxpos=0, fypos=0;


//--------------------------------------------------

#define GYRO    S3
#define COMPASS S4

float CompHdg;
float GyroHdg, GyroValue, GyroOffset=0.0;


//--------------------------------------------------
// IO functions
//--------------------------------------------------

#define Beep(f,d) PlayTone(f,d)

#define printf1( _x, _y, _format1, _value1) { \
  string sval1 = FormatVal(_format1, _value1); \
  TextOut(_x, _y, sval1); \
}

const char LCDline[]={56,48,40,32,24,16,8,0};

const string clrln="                 ";

inline bool btnhit(){
   return ( ButtonPressed(BTN1, false) || ButtonPressed(BTN2, false)
         || ButtonPressed(BTN3, false) || ButtonPressed(BTN4, false));
}



//--------------------------------------------------
// math
//--------------------------------------------------

#define min(a,b) (a<b?a:b)

#define max(a,b) (a>b?a:b)

inline long round(float f) { return ((f>=0) ? (f + 0.5) : (f - 0.5)); }


float ArrayDegreesMean (float src[], int len) {
   int i;
   for (i=0; i<len; ++i) {
     if (src[i]>180) src[i]=(src[i]-360);
   }
   return (ArrayMean(src, NA, NA));
}


inline float ArrayMedianF(float src[], int len)
{
  float ftemp[];
  ArraySort(ftemp, src, NA, NA)
  return ftemp[(len-1)/2];
}


//--------------------------------------------------
// sensor calibration
//--------------------------------------------------


safecall void SetGyroOffset()
{
  float old, delta;
  old=GyroOffset;
  do {
    float tot = 0;
    for (int i=0; i<200; ++i)
    {
       tot += SensorHTGyro(GYRO, 0); // tot var is 200 gyro readings
       Wait(4);                     // 5ms between analog readings
    }
    Beep(880,100);
    GyroOffset=tot/200.0;
    delta=old-GyroOffset;
    old=GyroOffset;
  } while (abs(delta)>0.005)
  Wait(20);
  CompHdg= SensorHTCompass(COMPASS);
  GyroHdg=MencHdg=CompHdg;
}

//--------------------------------------------------

safecall void TestGyroOffset() {
  float old, delta;
  old=GyroOffset;

    float tot = 0;
    for (int i=0; i<200; ++i)
    {
       tot += SensorHTGyro(GYRO, 0); // tot var is 200 gyro readings
       Wait(4);                     // 5ms between analog readings
    }
    Beep(880,100);
    GyroOffset=tot/200.0;
    delta=old-GyroOffset;
    old=GyroOffset;

  if (abs(delta)>0.01)  SetGyroOffset();
}

//--------------------------------------------------
// move commands
//--------------------------------------------------

void SpinUntilHdg(float TargHdg, char tSensor) {

  float currHdg, dHdg;
  int dirle, dirri, speedle, speedri, dle, dri, nspeed=80;

  if (tSensor==SCompass)  {currHdg=SensorHTCompass(COMPASS);}
  else
  if (tSensor==SGyro)     {currHdg=GyroHdg;}
  else
  if (tSensor==SMenc)     {currHdg=MencHdg;}
 
  dHdg=TargHdg-currHdg;
  speedle=speedri=20;

  while(TargHdg < 0)  TargHdg+=360;
  while(TargHdg>=360) TargHdg-=360;

  while(abs(dHdg)>0.5) {

    if ( (abs(dHdg)>10) || ( (speedle<30) && (speedri<30)))  {  // ramp up
      if (speedle<nspeed) speedle+=5;
      if (speedri<nspeed) speedri+=5;
    }

    if (tSensor==SCompass)  {currHdg=SensorHTCompass(COMPASS);}
    else
    if (tSensor==SGyro)     {currHdg=GyroHdg;}
    else
    if (tSensor==SMenc)     {currHdg=MencHdg;}

    dHdg=TargHdg-currHdg;

    if (dHdg<-180) dHdg= 360-dHdg;
    if (dHdg>=180) dHdg-=360;

    dirle=(dHdg>=0?1:-1);
    dirri=-dirle;
 
    OnFwd(Mright, dirri*(speedri+dri));
    OnFwd(Mleft,  dirle*(speedle+dle));

    if (abs(dHdg)<50) {
      if (dHdg<0) {
        speedle = abs(dHdg)*5;             // left turn:    // ramp down
        speedri = abs(dHdg)*5;
      }
      else if (dHdg>0) {
        speedle = abs(dHdg)*10;            // right turn:   // later ramp down
        speedri = abs(dHdg)*10;
      }

      if (speedle>nspeed) speedle=nspeed;
      if (speedle<30) speedle=30;

      if (speedri>nspeed) speedri=nspeed;
      if (speedri<30) speedri=30;
    }
   
    Wait(10);
  }
  Off (Mboth);
  Beep(100,100);  Wait(100);
  Coast(Mboth);
 
}

//--------------------------------------------------


void MoveToPoint(float xTarg, float yTarg) {

  float currHdg, dHdg, tx, ty, TargHdg, total, rest;
  int dirle, dirri,  speed, dle, dri, nspeed=80;

  tx=xTarg-fxpos;
  ty=yTarg-fypos;
  TargHdg=atan2(tx,ty)*(180/PI) ;
  total=sqrt(tx*tx+ty*ty);
 
  SpinUntilHdg(TargHdg, SGyro);
  if (abs(MencHdg-GyroHdg)>4) {
    MencHdg=GyroHdg;
  }

  speed=20;

  while(TargHdg < 0)  TargHdg+=360;
  while(TargHdg>=360) TargHdg-=360;

  while( (abs(fxpos-xTarg)>1) || (abs(fypos-yTarg)>1)) {
    rest= sqrt(tx*tx+ty*ty);

    if ( (rest>3) || (speed<30) )  {  // ramp up
      if (speed<nspeed) speed+=5;
     }
    if (abs(MencHdg-GyroHdg)>4) MencHdg=GyroHdg;
    currHdg=MencHdg;

    tx=xTarg-fxpos;
    ty=yTarg-fypos;
    TargHdg=atan2(tx,ty)*(180/PI) ;
    while (TargHdg >=360) TargHdg-=360;
    while (TargHdg < 0)   TargHdg+=360;
   
    dHdg=TargHdg-currHdg;
    if (dHdg<-180) dHdg= 360-dHdg;
    if (dHdg>=180) dHdg-=360;
   

    if  (abs(dHdg>10)) {
      Off(Mboth); Wait(100);
      tx=xTarg-fxpos;
      ty=yTarg-fypos;
      TargHdg=atan2(tx,ty)*(180/PI) ;
      total=sqrt(tx*tx+ty*ty);
      MencHdg=GyroHdg;
      SpinUntilHdg(TargHdg, SGyro);
      MencHdg=GyroHdg;
    }
    else
    if (abs(dHdg>1) || (rest<30) || (rest>total-10))  {
      if ((dHdg)>0) {
        Coast(Mright);
        OnFwd(Mleft, speed);
      }
      if ((dHdg)<0) {
        Coast(Mleft);
        OnFwd(Mright, speed);
      }
    }
    else OnFwd(Mboth, speed);
   
    if (rest<5) {                                         // ramp down
      speed = rest*200/nspeed;

      if (speed>nspeed) speed=nspeed;
      if (speed<30) speed=30;
    }
    Wait(10);

  }

  Off (Mboth);
  Beep(100,100);  Wait(100);
  Coast(Mboth);
 
}


//--------------------------------------------------
// motor control
//--------------------------------------------------

float VwMleft, VwMright;  // deg per sec

task MotorSpeed() {
 float Mle, OMle, dVMle;
 float Mri, OMri, dVMri;

 while (1) {
     Mle=Menc(Mleft);
     Mri=Menc(Mright);
     dVMle=Mle-OMle;
     dVMri=Mri-OMri;
     OMle=Mle;
     OMri=Mri;
     VwMleft=dVMle*(100);
     VwMright=dVMri*(100);
     Wait(10);
  }
}

//------------------------------------------------------------------------------


task Navigator(){
  long itemp;
  float ftemp, dfG, dfC, dfE;
  float fbuf[3];
  long  encLeft, encRight, encLeftOld, encRightOld, dEncLeft, dEncRight;
  float fxold=0, fyold=0, Urw, Ulw, Um, dMencHdgRad, MencHdgRad;
 
  float GyroHdgRad, GyroHdgOld, dGyroHdg, dGyroHdgRad;
  float CompHdgRad, CompHdgOld, dCompHdg, dCompHdgRad;

  long startTComp, startTGyro, startTMenc, dTComp, dTGyro, dTMenc;

  encLeft =Menc(Mleft);
  encRight=Menc(Mright);

  //-------------------------------------------//

  CompHdg=SensorHTCompass(COMPASS);

  //-------------------------------------------//

  GyroHdg=MencHdg=CompHdg;

  startTComp=startTGyro=startTMenc=CurrentTick();

     //-------------------------------------------//

  while(1) {

     if (ButtonPressed(BTNCENTER, false)) {  // press BTNCENTER:
        SetGyroOffset();                    // recalibrate gyro offset
     }

     if (ButtonPressed(BTNLEFT, false)) {          // press BTNLEFT:
        MencHdg=GyroHdg=SensorHTCompass(COMPASS);  // reset Hdgs to compass
        while (btnhit()); Beep(1000,10);
     }

     if (ButtonPressed(BTNRIGHT, false)) {   // press  BTNRIGHT:
        fxpos=fypos=0;                       // reset x,y to 0
        while (btnhit()); Beep(1000,10);
     }

     CompHdgOld=CompHdg;
     startTComp = CurrentTick();

     CompHdg=SensorHTCompass(COMPASS);       //----------------------------//
     dTComp = CurrentTick() - startTComp;    // Compass heading            //
     CompHdgRad=CompHdg*PI/180;              //----------------------------//

     dCompHdg=CompHdg-CompHdgOld;
     if (dCompHdg<-180) dCompHdg= 360-dCompHdg;
     if (dCompHdg >180) dCompHdg-=360;
     dCompHdgRad=dCompHdg*PI/180;
     
     startTComp = CurrentTick();

     //-------------------------------------------//

     GyroHdgOld=GyroHdg;
     GyroValue=(SensorHTGyro(GYRO,0) - GyroOffset) ;    //
     dTGyro = CurrentTick() - startTGyro;
     GyroHdg+=GyroValue*dTGyro /1000.0;

     startTGyro = CurrentTick();

     while (GyroHdg>=360) GyroHdg-=360;      //----------------------------//
     while (GyroHdg<  0)  GyroHdg+=360;      // Gyro heading               //
     GyroHdgRad=GyroHdg*PI/180;              //----------------------------//
     
     dGyroHdg=GyroHdg-GyroHdgOld;
     if (dGyroHdg<-180) dGyroHdg= 360-dGyroHdg;
     if (dGyroHdg >180) dGyroHdg-=360;
     dGyroHdgRad=dGyroHdg*PI/180;

     //-------------------------------------------//
     
     encLeftOld =encLeft;                       // save old encoder values
     encRightOld=encRight;
     encLeft =Menc(Mleft);                      // get new encoder values
     encRight=Menc(Mright);

     dEncLeft=encLeft-encLeftOld;
     Ulw= WheelLegPerEncDegree*dEncLeft;       // left wheel leg way
     dEncRight=encRight-encRightOld;
     Urw= WheelLegPerEncDegree*dEncRight;      // right wheel leg way

     Um=(Urw+Ulw)/2;                                // intermedium leg way

     dMencHdgRad=atan2((Ulw - Urw),WheelGauge);     // delta heading !! GEO !!

     //-----------------------------------------------------------------------//

     if (abs(dCompHdgRad-dGyroHdgRad)<(PI/90))
        dMencHdgRad=(dGyroHdgRad+dCompHdgRad)/2;
     else {
       fbuf[0]=dCompHdgRad;
       fbuf[1]=dGyroHdgRad;
       fbuf[2]=dMencHdgRad;
       ftemp=ArrayMean(fbuf,NA, NA);
       dfC=abs(dCompHdgRad-ftemp);
       dfG=abs(dGyroHdgRad-ftemp);
       dfE=abs(dMencHdgRad-ftemp);
       if ((dfC<dfE) && (dfG<dfE)) {MencHdgRad=ArrayMedianF(fbuf, 3); }
     }
     //-----------------------------------------------------------------------//

     fxold=fxpos; fyold=fypos;                         // save old pos (x,y)

     MencHdgRad=MencHdg*PI/180;                        // Hdg in deg => rad

     fxpos=fxold + Um*(sin((MencHdgRad+(dMencHdgRad /2))));  // new xpos !! GEO !!
     fypos=fyold + Um* cos(-(MencHdgRad+(dMencHdgRad /2)));   // new ypos !! GEO !!

     MencHdgRad = MencHdgRad + dMencHdgRad;        // new motor enc Heading
     MencHdg=MencHdgRad*180/PI;
                                              //----------------------------//
     while (MencHdg >=360) MencHdg-=360;      // odometry heading           //
     while (MencHdg < 0)  MencHdg+=360;       //----------------------------//
     
     //-----------------------------------------------------------------------//
     fbuf[0]=CompHdg;
     fbuf[1]=GyroHdg;
     fbuf[2]=MencHdg;
     MencHdg=ArrayMedianF(fbuf,3);
     MencHdgRad=MencHdg*PI/180;
     //-----------------------------------------------------------------------//

     Wait(19);
  }
}

//------------------------------------------------------------------------------

task DisplayValues() {
  float fCompass;
  while (1) {
     fCompass=SensorHTCompass(COMPASS);
     
     printf1(0, 56, "mot_B%6d", Menc(Mleft));
     printf1(0, 48, "mot_C%6d", Menc(Mright));

     printf1(0, 40, "GValu %9.5f", GyroValue);
     printf1(0, 32, "GOffs %9.5f", GyroOffset);
     printf1(0, 24, "GyroHdg %8.3f", GyroHdg);
     printf1(0, 16, "MencHdg %8.3f", MencHdg);
     printf1(0,  8, "CompHdg %8.3f", fCompass);
     
     printf1(0,  0, "x %5.1f", fxpos); printf1(50, 0, "y %5.1f", fypos);

     Wait(20);
  }
}


//------------------------------------------------------------------------------
void InitGyro() {
    int OldHdg;
   
    OldHdg=SensorHTCompass(COMPASS);
    Wait(20);

    SpinUntilHdg(OldHdg-90, SCompass);
    CompHdg=SensorHTCompass(COMPASS);
    Wait(2000);

    SpinUntilHdg(OldHdg, SCompass);
    CompHdg=SensorHTCompass(COMPASS);
    SetGyroOffset();
}
//------------------------------------------------------------------------------

task main(){
  float fold;
  SetSensorHTGyro(GYRO);
  SetSensorLowspeed(COMPASS);

  start DisplayValues;
  start MotorSpeed;

  while(!btnhit());

  if (ButtonPressed(BTNCENTER, false))  {
    while (!ButtonPressed(BTNCENTER, false));
    InitGyro();
  }
 
  start Navigator;

rep:
  Wait(1000);
  fold=CompHdg;

  MoveToPoint(150,100);
  Wait(2000);  MencHdg=GyroHdg;

  MoveToPoint(150,-100);
  Wait(2000);  MencHdg=GyroHdg;

  MoveToPoint(0,0);
  Wait(2000);  MencHdg=GyroHdg;

  SpinUntilHdg(fold, SCompass);
  Wait(2000);  MencHdg=GyroHdg=CompHdg;


  while(1) {
    if (ButtonPressed(BTNCENTER, false))  {
    while (!ButtonPressed(BTNCENTER, false));
    InitGyro();
    goto rep;
  }
  }
}


_________________
Gruß,
HaWe
±·≠≈²³αβγδε∂ζλμνπξφωΔΦ≡ΠΣΨΩ∫√∀∃∈∉∧∨¬⊂⊄∩∪∅∞
NXC NXT CHESS SCHACH: www.mindstormsforum.de/viewtopic.php?f=70&t=6790


20. Aug 2011 12:29
Profil
Beiträge der letzten Zeit anzeigen:  Sortiere nach  
Auf das Thema antworten   [ 1 Beitrag ] 

Wer ist online?

Mitglieder in diesem Forum: 0 Mitglieder und 12 Gäste


Du darfst keine neuen Themen in diesem Forum erstellen.
Du darfst keine Antworten zu Themen in diesem Forum erstellen.
Du darfst deine Beiträge in diesem Forum nicht ändern.
Du darfst deine Beiträge in diesem Forum nicht löschen.
Du darfst keine Dateianhänge in diesem Forum erstellen.

Suche nach:
Gehe zu:  
cron
Impressum Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group.
Designed by ST Software for PTF.
Deutsche Übersetzung durch phpBB.de