Ich finde das Lenkrad-Modell einfach umzusetzen.
Die Idee: Das Lenkrad bestimmt, ob der Roboter geradeaus fährt oder eine Kurve fährt, unabhängig von der Geschwindigkeit. Stell Dir vor, die Stellung des Lenkrades ergibt sich aus dem Verhältnis der Strecken beider Seiten.
Die Strecken der beiden Seiten kannst Du direkt an den Odometrie Sensoren ablesen.
Für die Lenkung gäbe es folgende Extrem-Werte:
0= extrem links: Nur das Rechte Rad dreht sich
1000= extrem rechts: Nur das linke Rad dreht sich
500=geradeaus: Beide Räder drehen sich gleich schnell und fahren die gleiche Strecke.
Um mit reiner Integer-Arithmetik auszukommen, empfehle ich, für die Lenkung Werte im Bereich 0-1000 zu verwenden, wobei 500 geradeaus bedeutet.
Wenn Du in regelmäßigen Zeitintervallen die aktuelle Lenkung misst und mit dem Soll vergleichst, kannst Du die Drehzahl der Motoren entsprechend anpassen.
Die Regelung kann so funktionieren (pseudo-code):
Code:
alle_100ms {
int16_t fehler=(int32_t) soll_lenkung*odometer_right/(1000-soll_lenkung)-odometer_left;
if (fehler>0) {
ein_bischen_mehr_nach_rechts();
}
else if (fehler<0) {
ein_bischen_mehr_nach_links();
}
}
Die obige Formel muss als 32 Bit berechnet werden, weil 16 bit nicht ausreichen, um das Ergebnis der Multiplikation zwischenzuspeichern. Das Ergebnis wiederum passt aber in 16 bit rein.
Wieviel "ein bischen mehr" ist, musst Du experimentiell herausfinden. Wenn zu stark korrigiert wird, entsteht eine Schwingung. Also fang erstmal ganz klein an. An dieser Stelle würde ich eine PI Regelung empfehlen (siehe Wissens-Sammlung).
Du bist jetzt imstande, unabhängig von der Geschwindigkeit, Kurven in bestimmten Radien zu fahren. Die beiden Extrem-Werte 0 und 1000 führen zur engsten möglichen Kurve.
Wenn Du jetzt um die Kurve fahren willst, um dann an einem bestimmten Winkel anzuhalten (z.B 90 Grad), geht das so:
Die Geschwindigkeit ist erstmal egal.
Die Lenkung bestimmt den Kurvenradius.
Die mittlere Distanz bestimmt den Winkel, den Du erreichen willst. Ich keine damit den Mittelwert zwischen dem rechten und linken Sensor.
Bei einer rechtskurve: int16_t distanz=(int32_t)winkel*K*500/(lenkung-500)
Bei einer linkskurve: int16_t soll_distanz=(int32_t)winkel*K*500/(500-lenkung)
Dabei ist K eine Konstante, die vom Rad-Durchmesser, dem Rad-Abstand und den Odometrie-Takten pro Radumdrehung abhängt. In meinem Fall ist K ungefähr 1/3. Da dieser Bruchin den obigen Formeln noch mit 500 multipliziert wird, ergeben sich so große Zahlen, daß ich mit dem Rundungsfehler durch die Integer Arithmetik leben kann. Beispiel:
lenkung=300
winkel=90 Grad
K=1/3
soll_distanz=90*1/3*500/(500-300)
soll_distanz=90*166/200
soll_distanz=74 Odometrie Take
Du fährst in diesem Fall also mit dem bestimmten Lenker-Einschlag so weit, bis Du 74 Odometrie-Takte gefahren bist (und zwar im Mittel).
Code:
fortlaufend {
ist_distanz=(odometer_links+odometer_rechts)/2;
if (ist_distanz>=soll_distanz) {
stop();
}
}
Während Du überprüfst, ob das Ziel erreicht wurde, muss die Drehzahl oder Leistung der Motoren wie oben beschrieben geregelt werden, damit der gewünschte Lenker-Einschlag auch beibehalten wird.
Pass ein bischen auf mit integer Größen. Manche Formeln erfordern 32 bit für Zwischenergebnisse, währen das Endergebnis locker in 8 oder 16 Bit rein passt. Pass auch auf, daß Du keine Divison durch null bekommst. Bei den extremem Lenkereinschlägen 0 und 1000, sowie zu Beginn der Fahrt, wenn die Odometrie Sensoren noch auch 0 stehen, kann das passieren.
Wenn die Berechnungen wegen drohender Division durch null nicht möglich sind, regelst Du einfach nicht. Du wartest ab, bis die Odometrie Sensoren wenigstens ein paar Takte (z.B. 2) gezählt haben und im Fall von extremen Lenkungen (0 und 1000) kanst Du generell auf die Regelung der Lenkung verzichten, da sich ja sowieso nur ein Rad drehen soll.
Zitieren
Lesezeichen