Neuling: Odometrie

**radbruch** · 27.12.2007, 19:41

Hallo teHIngo

Ich wollte mit meinem letzten Post keineswegs vorlaut oder fordernd sein

Das habe ich auch nicht so aufgefasst. Ich war nur im ersten Moment überrascht von dem recht eigenständigen Lösungsansatz und von den vielen verschachtelten Schleifen verwirrt. Bei genauerer Betrachtung wurde mir die Funktion dann klarer, allerdings weiß ich auch noch nicht sicher, wo der Denkfehler liegt.

Richtung[] dient dazu, das Programm etwas mit den Codescheiben zu syncronisieren. In deiner Version hätte die selbe ansteigende oder abfallende Flanke mehrfach hintereinander erkannt werden können. Jetzt wird nach erkannter Flanke nur die nächste in der Gegenrichtung als gültig zugelassen. spd0/1 werden sehr wohl zu 0, aber eben nicht kleiner als 0. Allerdings bleiben die Motoren schon deutlich früher stehen.

Ich vermute, die f-Schleife ist ein Problem, denn wir prüfen j*i*warten_auf_Flanke_in_der_richtigen_Richtung lang nur eine Seite und verpassen dabei alle Segmentwechsel auf der anderen Seite. Vielleicht sollte man anstelle der f-Schleife innerhalb der j-schleife beide Seiten gleichzeitig auswerten. Die ODO-Daten werden ja sowieso gleichzeitig eingelesen, aber immer nur für f-Seite ausgewertet. Dazu müste man aber alle [f]-Indizies(?) nach [0] und [1] auflösen.

Ein anderes Problem könnte die Schrittweite von spd0/1 sein. Wenn der Drehzahlunterschied zu schnell größer wird, kann man ihn nicht einholen, wenn nur nach allen 10 e-Schleifendurchgängen die Werte für MotorSpeed() nur um einen Punkt geändert werden. Vielleicht sollten spd0/1 in 5er oder gar 10er-Schritten geändert werden. Man könnte die e-Schleife vielleicht auch ganz löschen, dann würde häufiger ein neuer MotorSpeed()-Wert ermittelt und gesetzt.

Wichtig bei der Odometrie sind auch noch die Optimierungen der Sensorik: Ausrichtung der Bauteile, Verringerung des axialen Spiels der Codescheiben und Schutz vor Fremdlichteinflüssen.

...aber erstmal habe ich den ehrgeiz das hier endlich zum laufen zu bringen

Die meisten hier verwenden Codes die sie irgendwo kopiert haben oder nutzen die aktuelle Library und wissen nicht, wie das im Hintergrund gemacht wird. Deinen Weg das selbst zu ergründen halte ich aber für sehr viel besser.

Gruß

mic

**radbruch** · 29.12.2007, 11:30

Hallo

Ich habe noch einen großen Mangel entdeckt der wohl den jetzigen Ansatz völlig unbrauchbar macht. Wir wollen ja prüfen, ob die Werte auf- oder absteigend sind. Mit dieser Schleifenlösung prüfen wir aber nicht kontinuierlich alle Werte, sondern werfen nach der Prüfung die alten Werte weg und beginnen komplett von vorne. Wir wissen deshalb nie genau, an welcher Stelle der Wertereihe wir uns wirklich befinden.

Die Lösung wäre wohl eine Art Ringspeicher bei dem immer der aktuellste Wert den ältesten überschreibt. Zudem sollte nach jedem neuen Wert die Folge geprüft werden um möglichst schnell zu erkennen, ob die nächste Flanke schon erreicht ist.

Ich habe das oben mit einem FiFo-Speicher schon angedeutet, der zwar mehr Aufwand beim Hinzufügen neuer Werte bedeutet, aber zum Auswerten immer den selben Code verwendet. Ringspeicher arbeiten mit Schreib- und Lesezeiger und sind deshalb eher für größere Datenmengen geeignet, weil dann das Umkopieren eines FiFos deulich mehr Aufwand ist als die Zeiger zu verwalten.

Gruß

mic

**teHIngo** · 29.12.2007, 12:47

Danke nochmal an dieser Stelle, doch ich muss mich hiermit kurz verabschieden, denn ich bin gerade im Reisestress. Heute Nachmittag gehts los zum Skifahrn

.

Ich melde mich sobald ich zurück bin wieder!

Guten Rutsch
tehingo

**teHIngo** · 15.03.2008, 13:59

*räusper* *hust* ... bin wieder aus dem Urlaub zurück

.
Nein, Spaß bei Seite, ich hatte Abiturtechnisch relativ wenig Zeit für den ASURO.

Also da mein voriges Programm kompletter murks war (müsst ihr euch nicht anschauen), habe ich einen komplett neuen Ansatz gewählt.
Im folgenden werde ich das Prinzip kurz umreißen:

ASURO nimmt drei Odometrie-Messwerte auf. Dabei überprüft er permanent ob bei den Werten ein neues Maximum oder Minimum vorliegt.
Anhand dieser Werte wird dann, um einen Referenzwert für Übergänge zu erhalten, die Hälfte der Amplitude bestimmt ((max+min)/2).
Die Bestimmung ob es einen Übergang von Schwarz nach Weiß oder umgekehrt gegeben hat funktioniert nun so: Wenn alle drei Messwerte auf/absteigend sind, und sich der mittlere Messwert in der Umgebung des Referenzwertes/der Hälfte der Amplitude befindet, wird ein Zähler erhöht.
Unterscheiden sich die beiden Zähler für links und rechts, wird dementsprechend die Geschwindigkeit der Motoren angepasst.
Um Messwerte für die Funktion zu erhalten wird das ganze initialisiert.

Doch, wer hätte es gedacht: Es klappt natürlich nicht

! Und ein weiteres mal ist es mir unerklärlich und ich frage mich warum!

Es wäre klasse wenn ihr mir helfen könntet. n>2 Augen sehen mehr als n=2 Augen!
Vielen Dank im Voraus!

Code:

#include "asuro.h"      // Konfigurationsdateien laden
#define MITTER 145      // Die ca. Hälfte der Amplitude
#define MITTEL 160      // für die ersten Anpassungen

/* Funktionen definieren */

/* Globale Variablen */
unsigned char i;              // Zähler
char alr=0;            // schon (already) gelaufen?
int cntsL=0;	      // Zähler (counts)
int cntsR=0;	      // Zähler
char cnts=0;	      // Zähler
unsigned int odoData[2];      // Array für Odo Werte
unsigned int lrHi[2]={MITTER, MITTER};          // left right highest
unsigned int lrLo[2]={MITTER, MITTER};          // left right lowest
unsigned int spd0=190; // Geschwindigkeit (speed)
unsigned int spd1=190; // Geschwindigkeit
unsigned int odoMemL[3];      // Ododaten zwischenspeichern (ododmetrie memory)
unsigned int odoMemR[3];      // Ododaten zwischenspeichern
unsigned int testErg[2];      // Ergebnisse des Durchlaufes (test ergebnis)
                              // 0=low, 1=hi

void schlafen(int wert){
   for(i=0; i<=wert; i++){
   Sleep(72);}
}

/* Geradeausfahren Initialisieren */

char InitFwd(void){

 schlafen(250);
 StatusLED(RED);
 schlafen(250);
 StatusLED(YELLOW);

   MotorDir(FWD,FWD);   // zunächst gasgeben damit er fährt und Werte sammelt, Vortest
   MotorSpeed(190,190);

   while(cnts<100){       // 20 mal zählen
      OdometrieData(odoData);

      for(i=0; i<3; i++){  // 3 durchläufe für OdoWerte
      OdometrieData(odoData);
      odoData[0]=odoMemL[i];
      odoData[1]=odoMemR[i];

         if(odoMemL[i]>lrHi[0]){       // die Höchstwerte ermitteln LINKS
            lrHi[0]=odoMemL[i];
         }
         else if(odoMemL[i]<lrLo[0]){  // die Tiefstwerte ermitteln
            lrLo[0]=odoMemL[i];
         }

         if(odoMemR[i]>lrHi[1]){       // die Höchstwerte ermitteln RECHTS
            lrHi[1]=odoMemL[i];
         }
         else if(odoMemR[i]<lrLo[1]){  // die Tiefstwerte ermitteln
            lrLo[1]=odoMemL[i];
         }
      }

      /* Hier wird geschaut, ob die Schwelle übertreten wird, falls ja hat Umdrehung stattgefunden */
      /* links */
      if((odoMemL[0]>odoMemL[1] && odoMemL[1]>odoMemL[2]) && ((odoMemL[1]/MITTEL)<1.02 && (odoMemL[1]/MITTEL)>0.98)){
      cntsL++;
      }
      else if((odoMemL[0]<odoMemL[1] && odoMemL[1]<odoMemL[2]) && ((odoMemL[1]/MITTEL<1.02) && (odoMemL[1]/MITTEL)>0.98)){
      cntsL++;
      }

      /* rechts */
      if((odoMemR[0]>odoMemR[1] && odoMemR[1]>odoMemR[2]) && ((odoMemR[1]/MITTER<1.02) && (odoMemR[1]/MITTER>0.98))){
      cntsR++;
      }
      else if((odoMemR[0]<odoMemR[1] && odoMemR[1]<odoMemR[2]) && ((odoMemR[1]/MITTER)<1.02 && (odoMemR[1]/MITTER)>0.98)){
      cntsR++;
      }

      if(cntsL>70 && cntsR>70){
      alr=1;
       StatusLED(RED);
       schlafen(250);
       StatusLED(GREEN);
      }
   cnts++;
      if(cntsR > 2000 || cntsL > 2000){
         cntsL=0;
         cntsR=0;
      }
   }//while cnts

 testErg[0]=(lrHi[0]+lrLo[0])/2; // Test Ergebnisse zuweisen,
 testErg[1]=(lrHi[1]+lrLo[1])/2; // Mitte der Funktion bei Hälfte der Amplitue

 MotorDir(BREAK,BREAK);
 MotorSpeed(0,0);

 schlafen(250);
 StatusLED(GREEN);
 schlafen(250);
 StatusLED(YELLOW);
 schlafen(250);
 StatusLED(GREEN);
 schlafen(250);
 StatusLED(YELLOW);
 schlafen(250);
 StatusLED(GREEN);
 schlafen(250);
 StatusLED(YELLOW);
 schlafen(250);
 StatusLED(GREEN);
 schlafen(250);

return alr;

}

/* Geradeausfahren */

void Forward(void){

 MotorDir(FWD,FWD);
 MotorSpeed(spd0,spd1);

if(alr == 1){    // Falls Vorprüfung stattgefunden hat, ausführen
      for(i=0; i<3; i++){  // 3 durchläufe für OdoWerte
      OdometrieData(odoData);
      odoData[0]=odoMemL[i];
      odoData[1]=odoMemR[i];

         if(odoMemL[i]>lrHi[0]){       // die Höchstwerte ermitteln LINKS
            lrHi[0]=odoMemL[i];
         }
         else if(odoMemL[i]<lrLo[0]){  // die Tiefstwerte ermitteln
            lrLo[0]=odoMemL[i];
         }

         if(odoMemR[i]>lrHi[1]){       // die Höchstwerte ermitteln RECHTS
            lrHi[1]=odoMemL[i];
         }
         else if(odoMemR[i]<lrLo[1]){  // die Tiefstwerte ermitteln
            lrLo[1]=odoMemL[i];
         }
      }

 testErg[0]=(lrHi[0]+lrLo[0])/2; // Test Ergebnisse zuweisen,
 testErg[1]=(lrHi[1]+lrLo[1])/2; // Mitte der Funktion bei Hälfte der Amplitue

      /* Hier wird geschaut, ob die Schwelle übertreten wird, falls ja hat Umdrehung stattgefunden */
      if((odoMemL[0]>odoMemL[1] && odoMemL[1]>odoMemL[2]) && ((odoMemL[1]/testErg[0])<1.02 && (odoMemL[1]/testErg[0])>0.98)){
         cntsL++;
      }
      else if((odoMemL[0]<odoMemL[1] && odoMemL[1]<odoMemL[2]) && ((odoMemL[1]/testErg[0]<1.02) && (odoMemL[1]/testErg[0])>0.98)){
      cntsL++;
      }

      if((odoMemR[0]>odoMemR[1] && odoMemR[1]>odoMemR[2]) && ((odoMemR[1]/testErg[1])<1.02 && (odoMemR[1]/testErg[1])>0.98)){
      cntsR++;
      }
      else if((odoMemR[0]<odoMemR[1] && odoMemR[1]<odoMemR[2]) && ((odoMemR[1]/testErg[1])<1.02 && (odoMemR[1]/testErg[1])>0.98)){
      cntsR++;
      }

if(spd1==255 || spd0==255){
   spd1=spd1-10;
   spd0=spd0-10;
}

if(cntsL>cntsR){
   spd1++;
}

if(cntsL<cntsR){
   spd0++;
}

if(cntsR > 2000 || cntsL > 2000){
   cntsL=0;
   cntsR=0;
}

   MotorDir(FWD,FWD);   // und go!!
   MotorSpeed(spd0,spd1);

}// if alr
}

/* Beginn des Hauptprogramms */

int main (void){

Init();            //Prozessor initialisieren
InitFwd();	   // Odo Initialisieren

while(1)         // Endlosschleife
{
  Forward();
}

/* Schluss ! */

return 0;
}

**radbruch** · 15.03.2008, 14:24

Ächz, das wird wohl etwas dauern bis man da durchgestiegen ist. Vielleicht solltest du dir angewöhnen Probleme aufs wesentliche zu reduzieren und dafür dann einen Testcode schreiben. Ich habe mich trotzdem mal rangewagt, der Code wird fehler- und warnungsfrei übersetzt, prima ;) Erwartungsgemäss fährt meine asuro damit auch eine Kurve.

Was mir sofort ins Auge gesprungen ist: In der Funktion InitFwd() liest du nur einen kurzen Augenblick lang die Werte ein, quasi noch im Stillstand. Bevor ich mich nun weiter in deine gedanklichen Untiefen stürze, war das vielleicht schon das Problem?

gruß

mic

**teHIngo** · 16.03.2008, 14:32

Das war leider nicht das Problem. Ich habe jetzt an mehreren Stellen Sleep Funktionen zum beschleunigen eingebaut, damit er Zeit zum Beschleunigen hat und erst dann anfängt zu messen.
Das klappt auch ganz gut. Manchmal korrigiert er was, fährt ein stück relativ gerade aus, doch dann - wie die meiste Zeit - fährt er wieder ne Kurve.
Daher habe ich den Eindruck dass irgendwas mit meiner Logik nicht funktioniert - vielleicht ist es aber auch nur das Finetuning der Werte, ihr habt da mehr Erfahrung. Zur Logik habe ich mal kurz eine Skizze gemacht.

Das folgende Programm wurde an einigen Stellen überarbeitet, also besser dieses anschauen : ). Ich wäre euch sehr dankbar wenn ihr mir helfen könntet diese Hürde endlich mal zu bewältigen ; )!

Danke und Gruß!

Code:

#include "asuro.h"      // Konfigurationsdateien laden
#define MITTER 155      // Die ca. Hälfte der Amplitude
#define MITTEL 140      // für die ersten Anpassungen

/* Funktionen definieren */

/* Globale Variablen */
unsigned char i;		// Zähler
unsigned char alr=0;			// schon (already) gelaufen?
unsigned char alr2=0;			// schon (already) gelaufen?
unsigned int cntsL=0;			// Zähler (counts)
unsigned int cntsR=0;			// Zähler
unsigned int cnts=0;			// Zähler
unsigned int odoData[2];     	// Array für Odo Werte
unsigned int lrHi[2]={MITTEL, MITTER};          // left right highest
unsigned int lrLo[2]={MITTEL, MITTER};          // left right lowest
unsigned int spd0=190; 		// Geschwindigkeit (speed)
unsigned int spd1=190; 		// Geschwindigkeit
unsigned int odoMemL[3];      	// Ododaten zwischenspeichern (ododmetrie memory)
unsigned int odoMemR[3];      	// Ododaten zwischenspeichern
unsigned int testErg[2];      	// Ergebnisse des Durchlaufes (test ergebnis)
                              	// 0=low, 1=hi

void schlafen(unsigned int wert){
   for(i=0; i<=wert; i++){
   Sleep(72);}
}

/* Geradeausfahren */

char InitFwd(void){

 schlafen(250);
 StatusLED(RED);
 schlafen(250);
 StatusLED(YELLOW);

   MotorDir(FWD,FWD);   // zunächst gasgeben damit er fährt und Werte sammelt, Vortest
   MotorSpeed(190,190);
   schlafen(250);

   while(cnts<500){       // 200 mal zählen
      OdometrieData(odoData);

      for(i=0; i<3; i++){  // 3 durchläufe für OdoWerte
      OdometrieData(odoData);
      
      odoData[0]=odoMemL[i];
      odoData[1]=odoMemR[i];

         if(odoData[0]>lrHi[0]){       // die Höchstwerte ermitteln LINKS
            lrHi[0]=odoData[0];}
         else if(odoData[0]<lrLo[0]){  // die Tiefstwerte ermitteln
            lrLo[0]=odoData[0];}

         if(odoData[1]>lrHi[1]){       // die Höchstwerte ermitteln RECHTS
            lrHi[1]=odoData[1];}
         else if(odoData[1]<lrLo[1]){  // die Tiefstwerte ermitteln
            lrLo[1]=odoData[1];}
      }
      
       testErg[0]=(lrHi[0]+lrLo[0])/2; // Test Ergebnisse zuweisen,
       testErg[1]=(lrHi[1]+lrLo[1])/2; // Mitte der Funktion bei Hälfte der Amplitue

      /* Hier wird geschaut, ob die Schwelle übertreten wird, falls ja hat Umdrehung stattgefunden */
      /* links */
      if((odoMemL[0]>odoMemL[1] && odoMemL[1]>odoMemL[2]) && (((odoMemL[1]/MITTEL)<1.05) && ((odoMemL[1]/MITTEL)>0.95))){
      cntsL++;}
      else if((odoMemL[0]<odoMemL[1] && odoMemL[1]<odoMemL[2]) && (((odoMemL[1]/MITTEL)<1.05) && ((odoMemL[1]/MITTEL)>0.95))){
      cntsL++;}

      /* rechts */
      if((odoMemR[0]>odoMemR[1] && odoMemR[1]>odoMemR[2]) && (((odoMemR[1]/MITTER)<1.05) && ((odoMemR[1]/MITTER)>0.95))){
      cntsR++;}
      else if((odoMemR[0]<odoMemR[1] && odoMemR[1]<odoMemR[2]) && (((odoMemR[1]/MITTER)<1.05) && ((odoMemR[1]/MITTER)>0.95))){
      cntsR++;}

      if(cntsL>420 && cntsR>420){
      alr=1;
       StatusLED(RED);
       schlafen(250);
       StatusLED(GREEN);}
   cnts++;
   }//while cnts

 MotorDir(BREAK,BREAK);
 MotorSpeed(0,0);

 schlafen(250);
 StatusLED(GREEN);
 schlafen(250);
 StatusLED(YELLOW);
 schlafen(250);
 StatusLED(GREEN);
 schlafen(250);
 StatusLED(YELLOW);
 schlafen(250);
 StatusLED(GREEN);
 schlafen(250);
 StatusLED(YELLOW);
 schlafen(250);
 StatusLED(GREEN);
 schlafen(250);

return alr;

}


void Forward(void){

if(alr2 == 0){            // Beschleunigen damit er in Fahrt Werte sammelt
   MotorDir(FWD,FWD);   
   MotorSpeed(spd0,spd1);
   schlafen(250);
   alr2 = 1;}


if(alr == 1){    // Falls Vorprüfung stattgefunden hat, ausführen
      
      for(i=0; i<3; i++){  // 3 durchläufe für OdoWerte
      OdometrieData(odoData);
      
      odoData[0]=odoMemL[i];
      odoData[1]=odoMemR[i];

         if(odoData[0]>lrHi[0]){       // die Höchstwerte ermitteln LINKS
            lrHi[0]=odoData[0];}
         else if(odoData[0]<lrLo[0]){  // die Tiefstwerte ermitteln
            lrLo[0]=odoData[0];}

         if(odoData[1]>lrHi[1]){       // die Höchstwerte ermitteln RECHTS
            lrHi[1]=odoData[1];}
         else if(odoData[1]<lrLo[1]){  // die Tiefstwerte ermitteln
            lrLo[1]=odoData[1];}
      }

 testErg[0]=(lrHi[0]+lrLo[0])/2; // Test Ergebnisse zuweisen,
 testErg[1]=(lrHi[1]+lrLo[1])/2; // Mitte der Funktion bei Hälfte der Amplitue

      /* Hier wird geschaut, ob die Schwelle übertreten wird, falls ja hat Umdrehung stattgefunden */
      /* links */
      if((odoMemL[0]>odoMemL[1] && odoMemL[1]>odoMemL[2]) && (((odoMemL[1]/testErg[0])<1.05) && ((odoMemL[1]/testErg[0])>0.95))){
      cntsL++;}
      else if((odoMemL[0]<odoMemL[1] && odoMemL[1]<odoMemL[2]) && (((odoMemL[1]/testErg[0])<1.05) && ((odoMemL[1]/testErg[0])>0.95))){
      cntsL++;}

      /* rechts */
      if((odoMemR[0]>odoMemR[1] && odoMemR[1]>odoMemR[2]) && (((odoMemR[1]/testErg[1])<1.05) && ((odoMemR[1]/testErg[1])>0.95))){
      cntsR++;}
      else if((odoMemR[0]<odoMemR[1] && odoMemR[1]<odoMemR[2]) && (((odoMemR[1]/testErg[1])<1.05) && ((odoMemR[1]/testErg[1])>0.95))){
      cntsR++;}


if(spd1==255 || spd0==255){  // Falls Maximalspd erreicht Spielraum schaffen
   spd1=spd1-10;
   spd0=spd0-10;}

if(cntsL>cntsR){	// Korrektur!
   spd1++;
   StatusLED(RED);}

if(cntsL<cntsR){
   spd0++;
   StatusLED(GREEN);}

if((cntsR > 10000 || cntsL > 10000) && (cntsR>7000 && cntsL>7000)){ // aufpassen dass Variable nicht überläuft
   cntsL=cntsL-7000;
   cntsR=cntsR-7000;}

   MotorDir(FWD,FWD);   // und go!!
   MotorSpeed(spd0,spd1);

}// if alr
}

/* Beginn des Hauptprogramms */

int main (void){

Init();            //Prozessor initialisieren
InitFwd();	   // Odo Initialisieren

while(1)         // Endlosschleife
{
  Forward();
}

/* Schluss ! */

return 0;
}

// EDIT:
Mittlerweile bin ich mir relativ sicher dass das Problem beim Messen liegt. Durch die Schleife

Code:

      for(i=0; i<3; i++){  // 3 durchläufe für OdoWerte
      OdometrieData(odoData);
      
      odoData[0]=odoMemL[i];
      odoData[1]=odoMemR[i];

wird der Fall wie in der Skizze dargestellt nur sehr selten auftreten. Ein neuer Algorithmus muss her

// EDIT die 2.:

So, jetzt habe ich einen neuen Algorithmus entwickelt. ASURO arbeitet jetzt mit durchlaufenden Werten und es sollten daher alle Übergänge verzeichnet werden.

Code:

      /* Neuer Algorithmus: "Durchlaufende Werte" */
      OdometrieData(odoData);
      
      odoMemL[0]=odoMemL[1];
      odoMemL[1]=odoMemL[2];
      odoMemL[2]=odoData[0];
      
      odoMemR[0]=odoMemR[1];
      odoMemR[1]=odoMemR[2];
      odoMemR[2]=odoData[1];

Klappen tut es allerdings noch nicht....

Ein bisschen Sorgen macht es mir, dass er bei folgender Kontrollfunktion auch niemals etwas ans Terminal sendet!!

Code:

if(cntsL>cntsR){	// Korrektur!
   spd1++;
   StatusLED(RED);
   SerWrite("L groesser R",12);}
else if(cntsL<cntsR){
   spd0++;
   StatusLED(GREEN);
   SerWrite("R groesser L",12);}
else if(cntsL==cntsR){
   spd0++;
   StatusLED(GREEN);
   SerWrite("R gleich L",10);}

Irgendeine Idee warum es immer noch nicht klappt? Hier nochmal das komplette Program:

Code:

#include "asuro.h"      // Konfigurationsdateien laden
#define MITTER 155      // Die ca. Hälfte der Amplitude
#define MITTEL 140      // für die ersten Anpassungen

/* Funktionen definieren */

/* Globale Variablen */
unsigned char i;		// Zähler
unsigned char alr=0;			// schon (already) gelaufen?
unsigned char alr2=0;			// schon (already) gelaufen?
unsigned int cntsL=0;			// Zähler (counts)
unsigned int cntsR=0;			// Zähler
unsigned int cnts=0;			// Zähler
unsigned int odoData[2];     	// Array für Odo Werte
unsigned int lrHi[2]={MITTEL, MITTER};          // left right highest
unsigned int lrLo[2]={MITTEL, MITTER};          // left right lowest
unsigned int spd0=190; 		// Geschwindigkeit (speed)
unsigned int spd1=190; 		// Geschwindigkeit
unsigned int odoMemL[3];      	// Ododaten zwischenspeichern (ododmetrie memory)
unsigned int odoMemR[3];      	// Ododaten zwischenspeichern
unsigned int testErg[2];      	// Ergebnisse des Durchlaufes (test ergebnis)
                              	// 0=low, 1=hi

void schlafen(unsigned int wert){
   for(i=0; i<=wert; i++){
   Sleep(72);}
}

/* Geradeausfahren */

char InitFwd(void){

 schlafen(250);
 StatusLED(RED);
 schlafen(250);
 StatusLED(YELLOW);

   MotorDir(FWD,FWD);   // zunächst gasgeben damit er fährt und Werte sammelt, Vortest
   MotorSpeed(190,190);
   schlafen(250);

   while(cnts<500){       // XXX mal zählen

      /* Neuer Algorithmus: "Durchlaufende Werte" */
      OdometrieData(odoData);
      
      odoMemL[0]=odoMemL[1];
      odoMemL[1]=odoMemL[2];
      odoMemL[2]=odoData[0];
      
      odoMemR[0]=odoMemR[1];
      odoMemR[1]=odoMemR[2];
      odoMemR[2]=odoData[1];

         if(odoData[0]>lrHi[0]){       // die Höchstwerte ermitteln LINKS
            lrHi[0]=odoData[0];}
         else if(odoData[0]<lrLo[0]){  // die Tiefstwerte ermitteln
            lrLo[0]=odoData[0];}

         if(odoData[1]>lrHi[1]){       // die Höchstwerte ermitteln RECHTS
            lrHi[1]=odoData[1];}
         else if(odoData[1]<lrLo[1]){  // die Tiefstwerte ermitteln
            lrLo[1]=odoData[1];}
           
       testErg[0]=(lrHi[0]+lrLo[0])/2; // Test Ergebnisse zuweisen,
       testErg[1]=(lrHi[1]+lrLo[1])/2; // Mitte der Funktion bei Hälfte der Amplitue

      /* Hier wird geschaut, ob die Schwelle übertreten wird, falls ja hat Umdrehung stattgefunden */
      /* links */
      if((odoMemL[0]>odoMemL[1] && odoMemL[1]>odoMemL[2]) && (((odoMemL[1]/MITTEL)<1.05) && ((odoMemL[1]/MITTEL)>0.95))){
      cntsL++;}
      else if(((odoMemL[0]<odoMemL[1]) && odoMemL[1]<odoMemL[2]) && (((odoMemL[1]/MITTEL)<1.05) && ((odoMemL[1]/MITTEL)>0.95))){
      cntsL++;}

      /* rechts */
      if((odoMemR[0]>odoMemR[1] && odoMemR[1]>odoMemR[2]) && (((odoMemR[1]/MITTER)<1.05) && ((odoMemR[1]/MITTER)>0.95))){
      cntsR++;}
      else if((odoMemR[0]<odoMemR[1] && odoMemR[1]<odoMemR[2]) && (((odoMemR[1]/MITTER)<1.05) && ((odoMemR[1]/MITTER)>0.95))){
      cntsR++;}

      if(cntsL>420 && cntsR>420){
      alr=1;
       StatusLED(RED);
       schlafen(250);
       StatusLED(GREEN);}
   cnts++;
   }//while cnts

 MotorDir(BREAK,BREAK);
 MotorSpeed(0,0);

 schlafen(250);
 StatusLED(GREEN);
 schlafen(250);
 StatusLED(YELLOW);
 schlafen(250);
 StatusLED(GREEN);
 schlafen(250);
 StatusLED(YELLOW);
 schlafen(250);
 StatusLED(GREEN);
 schlafen(250);
 StatusLED(YELLOW);
 schlafen(250);
 StatusLED(GREEN);
 schlafen(250);

return alr;

}


void Forward(void){

if(alr2 == 0){            // Beschleunigen damit er in Fahrt Werte sammelt
   MotorDir(FWD,FWD);   
   MotorSpeed(spd0,spd1);
   schlafen(250);
   alr2 = 1;}


if(alr == 1){    // Falls Vorprüfung stattgefunden hat, ausführen
      
      /* Neuer Algorithmus: "Durchlaufende Werte" */
      OdometrieData(odoData);
      
      odoMemL[0]=odoMemL[1];
      odoMemL[1]=odoMemL[2];
      odoMemL[2]=odoData[0];
      
      odoMemR[0]=odoMemR[1];
      odoMemR[1]=odoMemR[2];
      odoMemR[2]=odoData[1];

         if(odoData[0]>lrHi[0]){       // die Höchstwerte ermitteln LINKS
            lrHi[0]=odoData[0];}
         else if(odoData[0]<lrLo[0]){  // die Tiefstwerte ermitteln
            lrLo[0]=odoData[0];}

         if(odoData[1]>lrHi[1]){       // die Höchstwerte ermitteln RECHTS
            lrHi[1]=odoData[1];}
         else if(odoData[1]<lrLo[1]){  // die Tiefstwerte ermitteln
            lrLo[1]=odoData[1];}
      
 testErg[0]=(lrHi[0]+lrLo[0])/2; // Test Ergebnisse zuweisen,
 testErg[1]=(lrHi[1]+lrLo[1])/2; // Mitte der Funktion bei Hälfte der Amplitue

      /* Hier wird geschaut, ob die Schwelle übertreten wird, falls ja hat Umdrehung stattgefunden */
      /* links */
      if((odoMemL[0]>odoMemL[1] && odoMemL[1]>odoMemL[2]) && (((odoMemL[1]/testErg[0])<1.05) && ((odoMemL[1]/testErg[0])>0.95))){
      cntsL++;}
      else if((odoMemL[0]<odoMemL[1] && odoMemL[1]<odoMemL[2]) && (((odoMemL[1]/testErg[0])<1.05) && ((odoMemL[1]/testErg[0])>0.95))){
      cntsL++;}

      /* rechts */
      if((odoMemR[0]>odoMemR[1] && odoMemR[1]>odoMemR[2]) && (((odoMemR[1]/testErg[1])<1.05) && ((odoMemR[1]/testErg[1])>0.95))){
      cntsR++;}
      else if((odoMemR[0]<odoMemR[1] && odoMemR[1]<odoMemR[2]) && (((odoMemR[1]/testErg[1])<1.05) && ((odoMemR[1]/testErg[1])>0.95))){
      cntsR++;}


if(spd1==255 || spd0==255){  // Falls Maximalspd erreicht Spielraum schaffen
   spd1=spd1-10;
   spd0=spd0-10;}

if(cntsL>cntsR){	// Korrektur!
   spd1++;
   StatusLED(RED);
   SerWrite("L groesser R",12);}
else if(cntsL<cntsR){
   spd0++;
   StatusLED(GREEN);
   SerWrite("R groesser L",12);}
else if(cntsL==cntsR){
   spd0++;
   StatusLED(GREEN);
   SerWrite("R gleich L",10);}

if((cntsR > 10000 || cntsL > 10000) && (cntsR>7000 && cntsL>7000)){ // aufpassen dass Variable nicht überläuft
   cntsL=cntsL-7000;
   cntsR=cntsR-7000;}

   MotorDir(FWD,FWD);   // und go!!
   MotorSpeed(spd0,spd1);

}// if alr
}

/* Beginn des Hauptprogramms */

int main (void){

Init();            //Prozessor initialisieren
InitFwd();	   // Odo Initialisieren

while(1)         // Endlosschleife
{
  Forward();
}

/* Schluss ! */

return 0;
}

**teHIngo** · 21.03.2008, 17:17

Zwischenbericht: Überarbeitet, klappt immer noch nicht. : (
Soll ich vielleicht einfach nochmal alles schön ordentlich kurz und präzise hinschreiben und einen neuen Thread eröffnen? Wirkt vielleicht nicht so abschreckend und es würde evtl. mal jemand antworten ; )

Code:

/* ####################################################### */
/* ############# FUNKTIONEN DEFINIEREN ################### */
/* ####################################################### */

#include "asuro.h"	// Konfigurationsdateien laden

#define MITTER 155	// ca. die Hälfte der Amplitude
#define MITTEL 140	// der Odometriewerte. Benötigt für
			// Initialisierung

/* ####################################################### */
/* ############# FUNKTIONEN DEFINIEREN ################### */
/* ####################################################### */

/* Globale Variablen */
unsigned char i;				// Zähler
char alr=0;					// schon (already) gelaufen?
char alr2=0;				// schon (already) gelaufen?
unsigned int cntsL=0;				// Zähler (counts)
unsigned int cntsR=0;				// Zähler
unsigned int cnts=0;				// Zähler
unsigned int odoData[2];			// Array für Odo Werte
unsigned int lrHi[2]={MITTEL, MITTER};		// left right highest
unsigned int lrLo[2]={MITTEL, MITTER};		// left right lowest
unsigned char spd0=190; 			// Geschwindigkeit (speed)
unsigned char spd1=190; 			// Geschwindigkeit
unsigned int odoMemL[3]={0,0,0};;  	    	// Ododaten zwischenspeichern (ododmetrie memory)
unsigned int odoMemR[3]={0,0,0};;   	   	// Ododaten zwischenspeichern
unsigned int testErg[2]={MITTEL, MITTER};	// Ergebnisse des Durchlaufes (Testergebnis)
                              			// 0=low, 1=hi

/* --- Verbesserte Sleep Funktion --- */
void schlafen(unsigned int wert){
 for(i=0; i<=wert; i++) Sleep(72);
}

/* - Geradeausfahren Initialisieren - */
char InitFwd(void){

   schlafen(250);
   StatusLED(RED);
   schlafen(250);
   StatusLED(YELLOW);

   MotorDir(FWD,FWD);   // Zunächst gasgeben damit er fährt und Werte sammelt
   MotorSpeed(190,190);
   schlafen(250);	// Zeit zum Beschleunigen geben

   /* Messung beginnen */
   while(cnts<500){	// 500 Durchläufe zur Bestimmung der Hälfte
   			// der Amplitude
   			
      /* Neuer Algorithmus: "Durchlaufende Werte" */
      OdometrieData(odoData);
      
      odoMemL[0]=odoMemL[1];
      odoMemL[1]=odoMemL[2];
      odoMemL[2]=odoData[0];
      
      odoMemR[0]=odoMemR[1];
      odoMemR[1]=odoMemR[2];
      odoMemR[2]=odoData[1];

      if(odoData[0]>lrHi[0]){       // die Höchstwerte ermitteln LINKS
         lrHi[0]=odoData[0];}
      else if(odoData[0]<lrLo[0]){  // die Tiefstwerte ermitteln
         lrLo[0]=odoData[0];}

      if(odoData[1]>lrHi[1]){       // die Höchstwerte ermitteln RECHTS
         lrHi[1]=odoData[1];}
      else if(odoData[1]<lrLo[1]){  // die Tiefstwerte ermitteln
         lrLo[1]=odoData[1];}
           
      testErg[0]=(lrHi[0]+lrLo[0])/2; // Test Ergebnisse zuweisen,
      testErg[1]=(lrHi[1]+lrLo[1])/2; // Mitte der Funktion bei Hälfte der Amplitue

      /* Hier wird geschaut, ob die Schwelle übertreten wird, falls ja hat Umdrehung stattgefunden */
      /* links */
      if((odoMemL[0]>odoMemL[1] && odoMemL[1]>odoMemL[2]) && (((odoMemL[1]/MITTEL)<1.05) && ((odoMemL[1]/MITTEL)>0.95)))
      {
         cntsL++;
      }
      else if(((odoMemL[0]<odoMemL[1]) && odoMemL[1]<odoMemL[2]) && (((odoMemL[1]/MITTEL)<1.05) && ((odoMemL[1]/MITTEL)>0.95)))
      {
         cntsL++;
      }

      /* rechts */
      if((odoMemR[0]>odoMemR[1] && odoMemR[1]>odoMemR[2]) && (((odoMemR[1]/MITTER)<1.05) && ((odoMemR[1]/MITTER)>0.95)))
      {
         cntsR++;
      }
      else if((odoMemR[0]<odoMemR[1] && odoMemR[1]<odoMemR[2]) && (((odoMemR[1]/MITTER)<1.05) && ((odoMemR[1]/MITTER)>0.95)))
      {
         cntsR++;
      }

      if(cntsL>420 && cntsR>420){
      alr=1;
      StatusLED(RED);
      schlafen(250);
      StatusLED(GREEN);}
      cnts++;
   }//while cnts

   MotorDir(BREAK,BREAK);
   MotorSpeed(0,0);

   schlafen(250);
   StatusLED(GREEN);
   schlafen(250);
   StatusLED(YELLOW);
   schlafen(250);
   StatusLED(GREEN);
   schlafen(250);
   StatusLED(YELLOW);
   schlafen(250);
   StatusLED(GREEN);
   schlafen(250);
   StatusLED(YELLOW);
   schlafen(250);
   StatusLED(GREEN);
   schlafen(250);

return alr;
}


void Forward(void){

if(!alr2){		// Beschleunigen damit er in Fahrt Werte sammelt
   MotorDir(FWD,FWD);   
   MotorSpeed(spd0,spd1);
   schlafen(250);	// Zeit zum Beschleunigen
   alr2 = 1;
}


if(alr)
{    // Falls Vorprüfung stattgefunden hat, ausführen
      
   /* Dieser Programmteil ist weitestgehend identisch mit InitFwd(); */
   /* Neuer Algorithmus: "Durchlaufende Werte" */      
   OdometrieData(odoData);
      
   odoMemL[0]=odoMemL[1];
   odoMemL[1]=odoMemL[2];
   odoMemL[2]=odoData[0];
      
   odoMemR[0]=odoMemR[1];
   odoMemR[1]=odoMemR[2];
   odoMemR[2]=odoData[1];

   if(odoData[0]>lrHi[0]){		// die Höchstwerte ermitteln LINKS
      lrHi[0]=odoData[0];}
   else if(odoData[0]<lrLo[0]){		// die Tiefstwerte ermitteln
      lrLo[0]=odoData[0];}

   if(odoData[1]>lrHi[1]){		// die Höchstwerte ermitteln RECHTS
      lrHi[1]=odoData[1];}
   else if(odoData[1]<lrLo[1]){		// die Tiefstwerte ermitteln
      lrLo[1]=odoData[1];}
      
   testErg[0]=((lrHi[0]+lrLo[0])/2);	// Testergebnisse zuweisen,
   testErg[1]=((lrHi[1]+lrLo[1])/2);	// Mitte der Odometriewertekurve

      /* Hier wird geschaut, ob die Schwelle übertreten wird, falls ja hat Umdrehung stattgefunden */
      /* links */
   if((odoMemL[0]>odoMemL[1] && odoMemL[1]>odoMemL[2]) && (((odoMemL[1]/testErg[0])<1.05) && ((odoMemL[1]/testErg[0])>0.95)))
   {
      cntsL++;
   }
   else if((odoMemL[0]<odoMemL[1] && odoMemL[1]<odoMemL[2]) && (((odoMemL[1]/testErg[0])<1.05) && ((odoMemL[1]/testErg[0])>0.95)))
   {
      cntsL++;
   }

   /* rechts */
   if((odoMemR[0]>odoMemR[1] && odoMemR[1]>odoMemR[2]) && (((odoMemR[1]/testErg[1])<1.05) && ((odoMemR[1]/testErg[1])>0.95)))
   {
      cntsR++;
   }
   else if((odoMemR[0]<odoMemR[1] && odoMemR[1]<odoMemR[2]) && (((odoMemR[1]/testErg[1])<1.05) && ((odoMemR[1]/testErg[1])>0.95)))
   {
      cntsR++;
   }

   /* Dies ist ein neuer Programmteil. */
   if(spd1==255 || spd0==255)	// Falls Maximalspd erreicht Spielraum schaffen
   { 
      spd1=spd1-10;
      spd0=spd0-10;
   }

   /* Hier wird die Geschwindigkeit angepasst. */
   if (cntsL>cntsR)     spd1++;
   else if(cntsL<cntsR) spd0++;

   /* Variable darf nicht überlaufen */
   if((cntsR>5000 || cntsL>5000) && (cntsR>3000 && cntsL>3000))
   {
      cntsL=cntsL-2000;
      cntsR=cntsR-2000;
   }

   /* Und losfahren! */
   MotorDir(FWD,FWD);
   MotorSpeed(spd0,spd1);

}
}

/* ####################################################### */
/* ############ BEGINN DES HAUPTPROGRAMMS ################ */
/* ####################################################### */

int main (void){

Init();		// Prozessor initialisieren
InitFwd();	// Odo Initialisieren

while(1)
{
   Forward();
}

return 0;
}

/* ####################################################### */
/* ############### ENDE DES PROGRAMMS #################### */
/* ####################################################### */

**damaltor** · 22.03.2008, 19:17

wozu einen neuen thread für das gleiche thema öfnen? fasse deine erkenntnisse nochmal in einem post zusammen. dann kannst du wenn du möchtest "überabeitet" an den thread-titel anhängen (indem du bei deinem ersten beitrag auf edit klickst).

**radbruch** · 22.03.2008, 19:50

Hallo teHIngo

Mit der Zeit ist dein Programm einfach zu umfangreich um es mal "auf die Schnelle" zu korrigieren. Wir haben alle unsere eigenen Projekte mit denen wir uns beschäftigen und es kostet eben viel Zeit sich in fremde Programme einzudenken.

Wenn es nicht funktioniert must du das selbe machen wie wir auch: Das Programm zerlegen und die einzelnen Teile testen. Stück für Stück, Zeile für Zeile, Befehl für Befehl. Alles nochmal überdenken, Formulierungen, Klammern, Indexe, Grenzwerte, Datentypen... eben alles. Klar, das dauert Zeit und ist Mühselig, aber anders geht es nicht.

Inzwischen habe auch gemerkt dass beim Ermitteln der Startwerte ein Feldwechsel stattgefunden hat, der asuro sich also auch bewegte. Aber obwohl du recht umfangreiche Kommentare eingefügt hast, man muss sich echt bemühen um die Zusammenhänge zu verstehen. Ein Beispiel:

/* Hier wird geschaut, ob die Schwelle übertreten wird, falls ja hat Umdrehung stattgefunden */

Aha. Und dann kommt das:

if((odoMemL[0]>odoMemL[1] && odoMemL[1]>odoMemL[2]) && (((odoMemL[1]/testErg[0])<1.05) && ((odoMemL[1]/testErg[0])>0.95)))
{
cntsL++;
}
else if((odoMemL[0]<odoMemL[1] && odoMemL[1]<odoMemL[2]) && (((odoMemL[1]/testErg[0])<1.05) && ((odoMemL[1]/testErg[0])>0.95)))
{
cntsL++;
}

(Brauchst du nicht erklären, soll nur ein Beispiel sein ;)

Denn Rest können wir uns selbst erarbeiten. Woher soll ich wissen, was du dir gedacht hast? OK, auf- oder absteigende steigende Werte, Unterschied um einen Faktor 0,05. Aber warum? Warum drei Werte? Warum 0,05? Das ist bei dir gewachsen, wir müssen das nachvollziehen und dann noch erkennen, was an deiner Idee nicht funzt.

Ich mache nochmal einen Anlauf, aber ich befürchte, du mußt das letztlich selbst lösen. Die Grundlagen dazu hast du ja offensichlich, die Hilfsmittel auch. Viel Erfolg

Gruß

mic

**teHIngo** · 23.03.2008, 13:10

Hey zusammen,
kein Problem, ich kann natürlich verstehen dass es Zeitraubend ist mir dabei zu helfen und dass man dann nicht unbedingt soo Lust dazu hat. : ) Am besten löst man so ein Problem wohl in einer Gruppe, leider sind hier aber alle in den Osterurlaub verschwunden da dachte ich mir ich kontaktiere mal die Profis vom RoboterNetz in meiner Not.
Ich habe meine Programm bisher immer so konzipiert, dass ich mir ein umfangreiches Konzept mit Papier und Bleistift überlegt habe und dann auf dem PC alles abgewandelt und korrigiert habe bis es klappt. Vielleicht ist das ja nicht die optimale Lösung und ich werde das nächste mal alles in kleinere Projekte aufspalten ; ). Von dem her Vielen Dank!
Gruß

Thema: Neuling: Odometrie

Themen-Optionen

Thema durchsuchen

Anzeige

Lesezeichen

Lesezeichen

Berechtigungen