Taktzahl zwischen Flanken regeln
Auch wir haben schon einige Versuche mit dem Geradeausfahren gemacht. Zuerst haben wir auch einfach die Flanken (S/W-Übergänge mit Hysterese) gezählt und die Leistungszufuhr zu den Motoren so geregelt, dass die Flankenanzahl links und rechts gleich ist.
Das Problem dabei war, dass im Falle, dass beide Räder sich wunderbar gleichschnell drehen, trotzdem z.B. links die Flanke früher vorbeikommt als rechts (wäre ja ein Zufall, dass immer beide Flanken genau im selben Takt vorbeikommen). So glaubt der Regler, dass das linke Rad sich schneller dreht und dass er links weniger Leistung zuführen muss (obwohl eh beide sich gleich schnell drehen und es sich nur um eine Phasenverschiebung handelt). Asuro macht also eine Kurve nach links. Gleich danach kommt rechts auch die Flanke, somit ist die linke und die rechte Flankenzahl wieder gleich und der Regler inkrementiert wieder die Leistungszufuhr am Linken Rad und Asuro fährt wieder geradeaus.
Das wiederholt sich bei jeder Flanke - in Summe entsteht so ein Links-Drift.
Mein Sohn Jakob (er ist 14) hat die Idee gehabt, nicht die Flankenzahl zu zählen, sondern die Anzahl der Takte zwischen zwei Flanken (ein Maß für den Kehrwert der Geschwindigkeit). So spielt die Phasenverschiebung keine Rolle. Funktioniert bei uns sehr gut
Programm berücksichtigt (noch) nicht unterschiedliche Lichtverhältnisse; gibt am Schluss den Offset aus - den sollte man als Startwert für das nächste Mal eingeben, damit Asuro gleich von beginn an geradeaus fährt.
Damit der inkrementelle I-Regler nicht überschwingt, wird der I-Anteil durch 16 dividiert (also um 4 Bit nach rechts verschoben).
Wir haben damit auf 3m Fahrt 5-10cm seitliche Ablenkung und 0-3mm Längenfehler.
Bitte wundert Euch nicht über die Veriablennamen - ist kindergerecht.
Code:
#include "asuro.h"
#define MAXI 3825 //(durch 15= 255)
#define StartwertUmSoVielBrauchtLinksMehrAlsRechts 0 // wird durch 16 dividiert
#define SchwellwertLinksSchwarz 675 //Schwellwert über dem das linke Zahnrad als Schwarz gilt
#define SchwellwertLinksWeiss 625 //Schwellwert unter dem das linke Zahnrad als Weiß gilt
#define SchwellwertRechtsSchwarz 675 //Schwellwert über dem das rechte Zahnrad als Schwarz gilt
#define SchwellwertRechtsWeiss 625 //Schwellwert unter dem das rechte Zahnrad als Weiß gilt
#define AnfangskoordinateX 0 //Anfangskoordinate in x-Richtung in mm, Ziel ist 0
#define AnfangskoordinateY -5000 //Anfangskoordinate in y-Richtung in mm, Ziel ist 0
int KoordinateX=AnfangskoordinateX;
int KoordinateY=AnfangskoordinateY;
int MerkerLinksWarSchonWeiss;
int MerkerRechtsWarSchonWeiss;
int MerkerLinksWarSchonSchwarz;
int MerkerRechtsWarSchonSchwarz;
//int StartdurchgangLinks;
//int StartdurchgangRechts;
int MerkerLinksWarSchonEineGanzePeriode;
int MerkerRechtsWarSchonEineGanzePeriode;
int TakteZaehlerSchwarzLinks; // Zaehlt wie viele Takte zwischen zwei W/S-Übergängen liegen
int TakteZaehlerWeissLinks;
int TakteZaehlerSchwarzRechts;
int TakteZaehlerWeissRechts;
int TaktzahlLinks; // Ergebnis wie viele Takte zwischen zwei W/S-Übergängen Links liegen
int TaktzahlRechts;
int GeschwindigkeitsKorrekturLinks; //Zahl zwischen theoretisch -255 und + 255, die zur Sollgeschwindigkeit dazuaddiert wird, um die unterschiedlichen Motoren auszugleichen
unsigned int UebergangszaehlerLinks; //max. 65535; zaehlt wie viel S/W und W/S-Übergaenge seit dem letzten Rücksetzen links passiert sind
unsigned int UebergangszaehlerRechts; //max. 65535; zaehlt wie viel S/W und W/S-Übergaenge seit dem letzten Rücksetzen rechts passiert sind
int ReglerZurueckSetzen(void)
{
MerkerLinksWarSchonWeiss=FALSE;
MerkerRechtsWarSchonWeiss=FALSE;
MerkerLinksWarSchonSchwarz=FALSE;
MerkerRechtsWarSchonSchwarz=FALSE;
MerkerLinksWarSchonEineGanzePeriode=FALSE;
MerkerRechtsWarSchonEineGanzePeriode=FALSE;
//StartdurchgangLinks=TRUE;
//StartdurchgangLinks=TRUE;
TakteZaehlerSchwarzLinks=0;
TakteZaehlerWeissLinks=0;
TakteZaehlerSchwarzRechts=0;
TakteZaehlerWeissRechts=0;
TaktzahlLinks=0;
TaktzahlRechts=0;
GeschwindigkeitsKorrekturLinks=StartwertUmSoVielBrauchtLinksMehrAlsRechts;
UebergangszaehlerLinks=0;
UebergangszaehlerRechts=0;
return 0;
}
int fahr(int SollGeschwindigkeit, int SollKurve) //SollGeschwindigkeit = Zahl zwischen 0 (stop) und 255 (vorwaerts)
{ //SollKurve = .....muss noch definiert werden.......Derzeit: 0
unsigned int Zahnraddunkelheit[2]; // Zahnraddunkelheit[0]: Odometriedaten links, Zahnraddunkelheit[1]: rechts
int ZahnradstatusLinks=99; //weiß=0, undefiniert=1, schwarz=2, nicht zugewiesen=99
int ZahnradstatusRechts=99; //weiß=0, undefiniert=1, schwarz=2, nicht zugewiesen=99
OdometrieData(Zahnraddunkelheit);
TakteZaehlerSchwarzLinks++;
TakteZaehlerWeissLinks++;
TakteZaehlerSchwarzRechts++;
TakteZaehlerWeissRechts++;
//Abfrage nach Zahnradstatus Links:
if (Zahnraddunkelheit[0] > SchwellwertLinksSchwarz)
ZahnradstatusLinks=2; //links ist schwarz
else if (Zahnraddunkelheit[0] < SchwellwertLinksWeiss)
ZahnradstatusLinks=0; //links ist weiß
else
ZahnradstatusLinks=1; //links ist undefiniert
//Abfrage nach Zahnradstatus Rechts:
if (Zahnraddunkelheit[1] > SchwellwertRechtsSchwarz)
ZahnradstatusRechts=2; //rechts ist schwarz
else if (Zahnraddunkelheit[1] < SchwellwertRechtsWeiss)
ZahnradstatusRechts=0; //rechts ist weiß
else
ZahnradstatusRechts=1; //rechts ist undefiniert
if (ZahnradstatusLinks==0) MerkerLinksWarSchonWeiss=TRUE; //auf wahr setzen
if (ZahnradstatusRechts==0) MerkerRechtsWarSchonWeiss=TRUE; //auf wahr setzen
if (ZahnradstatusLinks==2) MerkerLinksWarSchonSchwarz=TRUE; //auf wahr setzen
if (ZahnradstatusRechts==2) MerkerRechtsWarSchonSchwarz=TRUE; //auf wahr setzen
//Feststellen ob S/W oder W/S-Übergang links oder rechts:
if ((ZahnradstatusLinks==2)&&(MerkerLinksWarSchonWeiss==TRUE))
{TaktzahlLinks=TakteZaehlerSchwarzLinks; TakteZaehlerSchwarzLinks=0;
MerkerLinksWarSchonWeiss=FALSE;
UebergangszaehlerLinks++;
if (MerkerLinksWarSchonEineGanzePeriode<3) MerkerLinksWarSchonEineGanzePeriode++;
}
//Linkes Zahnrad hat neu Schwarz erreicht (W/S-Übergang) => Zaehler++
else if ((ZahnradstatusLinks==0)&&(MerkerLinksWarSchonSchwarz==TRUE))
{TaktzahlLinks=TakteZaehlerWeissLinks; TakteZaehlerWeissLinks=0;
MerkerLinksWarSchonSchwarz=FALSE;
UebergangszaehlerLinks++;}
//Linkes Zahnrad hat neu Weiß erreicht (S/W-Übergang) => Zaehler++
else; //weder S/W noch W/S-Übergang
if ((ZahnradstatusRechts==2)&&(MerkerRechtsWarSchonWeiss==TRUE))
{TaktzahlRechts=TakteZaehlerSchwarzRechts; TakteZaehlerSchwarzRechts=0;
MerkerRechtsWarSchonWeiss=FALSE;
if (MerkerRechtsWarSchonEineGanzePeriode<3) MerkerRechtsWarSchonEineGanzePeriode++;
UebergangszaehlerRechts++;}
//Rechtes Zahnrad hat neu Schwarz erreicht (W/S-Übergang) => Zaehler--
else if ((ZahnradstatusRechts==0)&&(MerkerRechtsWarSchonSchwarz==TRUE))
{TaktzahlRechts=TakteZaehlerWeissRechts; TakteZaehlerWeissRechts=0;
MerkerRechtsWarSchonSchwarz=FALSE;
UebergangszaehlerRechts++;}
//Rechtes Zahnrad hat neu Weiß erreicht (S/W-Übergang) => Zaehler--
else; //weder S/W noch W/S-Übergang
//Geradeausregler (inkrementeller I-Regler):
if ((MerkerLinksWarSchonEineGanzePeriode > 2)&&(MerkerRechtsWarSchonEineGanzePeriode > 2))
{
if ((TaktzahlLinks < TaktzahlRechts)&&(SollGeschwindigkeit+GeschwindigkeitsKorrekturLinks > 0))
{GeschwindigkeitsKorrekturLinks--;StatusLED(GREEN);}
//wenn das linke Rad zu schnell dreht, dann soll es langsamer werden => Korrektur --
else if ((TaktzahlLinks > TaktzahlRechts)&&(SollGeschwindigkeit+GeschwindigkeitsKorrekturLinks < MAXI))
{GeschwindigkeitsKorrekturLinks++;StatusLED(RED);}
//wenn das link Rad sich zu langsam dreht und es nicht eh schon Vollgas gibt, dann soll es schneller werden => Korrektur ++
else;
};
MotorSpeed(SollGeschwindigkeit + (GeschwindigkeitsKorrekturLinks>>4) , SollGeschwindigkeit); //korrigiertes Motorkommando, auf vorgegebene Kurvenkrümmung geregelt
return SollGeschwindigkeit+GeschwindigkeitsKorrekturLinks;
}
int main(void)
{
// int p,i=0; //Zaehler initialisieren
int s=0;
// unsigned int j,k;
char string[]=" test \n\r"; // Ausgabestring initialisieren
// unsigned int odoleft[MAXI], odoright[MAXI];
Init();
ReglerZurueckSetzen();
MotorDir(FWD,FWD);
// SerWrite(string,20);
StatusLED(GREEN); //
while(UebergangszaehlerRechts<1000)
{
s=fahr(140,0); // Aufruf der Funktion fahre mit 130 Geradeaus (geregelt)
}
MotorDir(BREAK,BREAK);
sprintf(string, "Korr.= %4d\n\r",GeschwindigkeitsKorrekturLinks);
SerWrite(string,13);
sprintf(string, "ZahlL= %4d\n\r",UebergangszaehlerLinks);
SerWrite(string,13);
sprintf(string, "ZahlR= %4d\n\r",UebergangszaehlerRechts);
SerWrite(string,13);
while(1);
return 0;
}
Liste der Anhänge anzeigen (Anzahl: 1)
Hallo dorothea,
die Idee mit dem Kehrwert der Geschwindigkeit finde ich sehr gut. Dickes Lob an deinen 14-er.
Zu der Abweichung von 5-10 cm auf 300 cm mal eine kleine Rechnung:
Folgendes:
Unter der Annahme, dass sich die Räder bei einer perfekter Reglung gleich schnell drehen aber trotzdem einen Differenz zur geraden Linie vorliegt, muss der Asuro somit im Bogen fahren.
Wenn der Asuro also auf einem Kreis fährt, fährt er bei deiner Angabe 300 cm auf dem Umfang und bekommt einen Abstand von 10 cm zur Tangente.
Wenn ich mal wieder Excel bemühe, bekomme ich einen Radius für diesen Kreis von 4505 cm raus.
Somit hätten wir, bei einem Radstand am Asuro mit 10,4 cm einen inneren Radius von 4499,8 cm und einen Außenradius von 4510,2 cm.
Das ist ein Verhältnis von ca. 1,0023
Dieses Verhältnis müssen dann aber auch die Asuro-Räder untereinander haben.
Hat das eine Rad so ca. einen Durchmesser von 3,8 cm, hätte das andere dann etwa 3,8087 cm. Das liegt bestimmt bestens in der Fertigungstoleranz ;-)
Ich bin mir ziemlich sicher, dass unser Asuro niemals eine perfekte Linie abfahren wird. (Was habe ich hier falsch bedacht?)
Trotzdem muss man natürlich alles ausschöpfen um immer besser zu werden. Also weiter mit Kehrwerten und frischen Ideen.
@austin2power
Wenn ich deinen Link anklicke bekommen ich nur folgende Meldung:
'Das für diesen Befehl erforderliche Sicherheitsusatzmodul ist nicht verfügbar'
Ich nutze den Mozilla Firefox in Version 2.0.0.1. Weißt du hier weiter?
P.S.: Ich wäre auch an einer FFT interessiert, die in den Asuro passt. Hier würde ich sogar sehr gerne bei dir abschreiben, da ich mal versucht hatte Barcodes zu lesen die seitlich an einer vom Asuro zu verfolgenden Linie angebracht sind. Bitte um Code \:D/
Liste der Anhänge anzeigen (Anzahl: 2)
Hi Leute,
beschäftige micht jetzt auch mit dem geradeausfahren meines Asuro.
Hab eure ganzen Beiträge gelesen und mir sind ein paar Fragen entstanden:
1. Hysterese? Was ist das?
2. Was meint ihr mit Flanken.
Hab dann auch mal versucht die Werte die mir die Odometrie liefert zu loggen aber mir ist nicht ganz klar wie ich die Werte interpretieren soll.
Könnt ihr mir helfen?
Hänge das Programm mit dem ich die Werte erfasst habe und die Excel-Tabelle für 500 Werte der Linkensensoren an.
Viele Grüße
Tom
