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ß!
// EDIT: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; }
Mittlerweile bin ich mir relativ sicher dass das Problem beim Messen liegt. Durch die Schleifewird der Fall wie in der Skizze dargestellt nur sehr selten auftreten. Ein neuer Algorithmus muss herCode:for(i=0; i<3; i++){ // 3 durchläufe für OdoWerte OdometrieData(odoData); odoData[0]=odoMemL[i]; odoData[1]=odoMemR[i];
// 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.Klappen tut es allerdings noch nicht....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];Ein bisschen Sorgen macht es mir, dass er bei folgender Kontrollfunktion auch niemals etwas ans Terminal sendet!!
Irgendeine Idee warum es immer noch nicht klappt? Hier nochmal das komplette Program: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);}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; }
Zitieren
Lesezeichen