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;
}
Zitat von austin2power
/
Berechtigungen
Zitieren
Lesezeichen