Regelung für Winkelpositionierung

[MechMac]

Hallo,

da ich leider mathematisch nix auf dem Kasten habe, benötige ich etwas Hilfe.
Ich möchte gerne ein Objekt auf einen Winkel ausrichten.
Folgende Dinge sind gegeben:
Ist-Winkel
Soll-Winkel
Tolleranz bei der positionierung (tolleranz=2.0)

Winkelangaben kamen von einem Sensor in Form von
0° bis 180° (0° bis 180°)
-180° bis -0° (180° -360°)
In Klammern die Werte wie sie auf einen Kreis bezogen wären.
Das habe ich wie folgt umgerechnet:



ist_angle=ToDeg(yaw); //HIER DEN KOMPASSWERT AUSLESEN
if (ist_angle<0){
ist_angle=180+(180-(ist_angle*-1));
}


Damit bekomme ich Winkel von 0°-360°
Getestet und funktioniert.

Nun kommt mein Problem.
Mit folgendem Code gebe ich die PWM zum positionieren aus:



//Wenn ANGFIX-Modus aktiv dann selber regeln
if (angfix==1){
if (ist_angle<=(soll_angle-tolleranz)){
WritePWM(servo, 1400); //Rechtslauf
}
if (ist_angle>=(soll_angle+tolleranz)){
WritePWM(servo, 1600); //Linkslauf
}
if ((ist_angle>(soll_angle-tolleranz)) && (ist_angle<(soll_angle+tolleranz))) {
WritePWM(servo, 1500); //Neutral
}
}



Das funktioniert zwar, aber nur so lange, bis man zum Übergang zwischen 0 und 360 kommt.
Beispiel:
Ein Soll-Winkel von 1° gibt folgende Werte und deren Abweichungen in den o.g. Formeln:
-1° (-)
1° (0)
3° (+)
Da geht es los, denn es gibt keinen negativen Ist-Winkel.


Wie kann man das mathematisch bzw. programmiertechnisch möglichst clever lösen?




Edit:
Zum positionieren wird kein Servo verwendet, sondern ein Motor, welcher mittels Getriebe das Objekt positioniert.

[Che Guevara]

Hi,

ich hab mal sowas ähnliches programmiert, nämlich eine Datenfusionierung von Kompass & Gyro. Dabei hab ich den Wertebereich vom Kompass von 0° - 360° auf -Unendlich - +Unendlich verschoben (natürlich nur theoretisch). Ich weiß ja nicht, inwiefern du das in dein Programm einbauen kannst, aber es funktioniert sehr gut und benötigt sogut wie keine Rechenzeit.
Im Prinzip prüft man einfach immer, ob der letzte Messwert < 360° und der neue Messwert >= 0° bzw. andersrum ist. Wenn das der Fall ist, kann man davon ausgehen, dass der Kompass einmal über die 360° bzw. 0° hinausgedreht wurde. Dann inkrementiert bzw. dekrementiert man eine Variable, die die Umdrehung seit dem Start angibt. Somit kannst du dann einen Winkel bilden: Winkel = (Umdrehungen * 360°) + akt. Winkel
Das funktioniert allerdings nur, wenn der Kompass regelmäßig ausgelesen wird und sich das Objekt nicht zu schnell dreht, sodass die Erkennung der Umdrehung noch funktioniert.

Gruß
Chris

[MechMac]

Hi,

ich hab mal sowas ähnliches programmiert, nämlich eine Datenfusionierung von Kompass & Gyro. Dabei hab ich den Wertebereich vom Kompass von 0° - 360° auf -Unendlich - +Unendlich verschoben (natürlich nur theoretisch). Ich weiß ja nicht, inwiefern du das in dein Programm einbauen kannst, aber es funktioniert sehr gut und benötigt sogut wie keine Rechenzeit.
Im Prinzip prüft man einfach immer, ob der letzte Messwert < 360° und der neue Messwert >= 0° bzw. andersrum ist. Wenn das der Fall ist, kann man davon ausgehen, dass der Kompass einmal über die 360° bzw. 0° hinausgedreht wurde. Dann inkrementiert bzw. dekrementiert man eine Variable, die die Umdrehung seit dem Start angibt. Somit kannst du dann einen Winkel bilden: Winkel = (Umdrehungen * 360°) + akt. Winkel
Das funktioniert allerdings nur, wenn der Kompass regelmäßig ausgelesen wird und sich das Objekt nicht zu schnell dreht, sodass die Erkennung der Umdrehung noch funktioniert.

Gruß
Chris


So ganz habe ich das nicht verstanden. :confused:
Trotzdem Danke!


Habe mir mit Stift und Zettel ein paar Situationen aufgemalt und ich denke das ich eine Lösung gefunden habe.

GUZ = Gegenuhrzeigersinn
UZ = Uhrzeigersinn

Zunächst rechne ich
ERG = Soll_Winkel - Ist_Winkel

Das Ergebnis ERG ist entweder Positiv oder Negativ.

Dann überprüfe ich das Ergebnis ERG mit folgenden Abfragen:

IF (ERG>180) THEN GUZ
IF (ERG<180 AND ERG>0) THEN UZ
IF (ERG<0 AND ERG>-180) THEN GUZ
IF (ERG<-180) THEN UZ

Das sollte für Rechts- und Linkslauf alle Fälle abdecken.

Werde es testen und berichten.

[HeXPloreR]

Hallo MechMac,

also erstmal ist es schon komisch das Du oben die Winkel auf 360° umrechnen möchtest und dann unten auf 180°/-180° prüfst. Also mr würde das nur Kopfschmerzen bereiten.

Also ich würde es so anfangen, das man den Kompass-Wert hochrechnet so ähnlich wie in Deinem ersten Codeausschnitt und wohl so wieChe Guevara vorschlägt, also: Ist_Aktuell= 180 + ToDeg(yaw) // Da Du anscheinen mindestens in C programmierst; der ToDeg-Wert kann jetzt auch negativ sein - es kommt aber nur was zwischen 0° und 360° raus, also nur positive Werte.
Zusätzlich den Wert ggf speichern als Vergleichsvariable: z.B. Ist_Vorher = Ist_Aktuell
Du weißt das Du mit der Rrechnung " *-1 " den minus Wert umdrehst -also positiv? Allerdings drehst Du damit auch positive Werte ins negative. Deshalb auf das if davor, das ist ziemlicher Unsinn, wenn man beide Werte schon frei Haus geliefert bekommt.

Was ich daraus schliesse ist das Du möglicherwiese den Kompass falsch ausgerichtet hast bzw den anders ausrichten könntest um das Problem zu vereinfachen.

[MechMac]

Also den ersten Teil mit dem umrechen auf 0°-360° habe ich ja immer noch drin.
Hier mal der komplette Weg:

Der Winkel kommt vom Sensor im Format 0-180°, springt dann auf -180° und geht zurück nach -0°
Meine Formel behandelt nur den Bereich von -180° bis -0°. Also auf einen Kreis bezogen den Bereich 180° bis 360°

Beispiel:
-175° (wäre in echt ja 185°)
Eingesetzt in die Formel:
180+(180-(-175*-1))=185



ist_angle=ToDeg(yaw); //HIER DEN KOMPASSWERT AUSLESEN
if (ist_angle<0){
ist_angle=180+(180-(ist_angle*-1));
}


Ich erhalte also Winkel von 0°-360°

Im zweiten Schritt berechne ich die differenz zwischen Ist_Winkel und Soll_Winkel.
Das mache ich um den kürzesten Weg "zum Ziel" zu ermitteln (Bezogen auf einen Kreis)
Ein Voller Kreis besteht also aus ERG+X. X interessiert mich aber nicht, da ich weiß wenn ERG>180 ist, muss X kleiner sein.

Also:


ERG = Soll_Winkel - Ist_Winkel


Der kürzeste Weg ist der, wenn ERG < 180 ist.
Außerdem muss die Verfahrrichtung immer von Ist_Winkel nach Soll_Winkel sein.

Dazu habe ich die vier Abfragen, welche "den kürzesten Weg" filtern:

erg>=180 bedeutet das der Rest vom Kreis kürzer ist -> GUZ
(erg<180) && (erg>0) bedeutet das ERG der kürzeste Weg ist -> UZ
(erg<0) && (erg>-180) bedeutet das ERG der kürzeste Weg ist -> GUZ
erg<=-180 bedeutet das der Rest vom Kreis kürzer ist -> UZ

Zwei Abfragen reichen übrigens nicht, da erg negativ wird, wenn Ist_Winkel und Soll_Winkel
entsprechend ungünstig auf dem Kreis plaziert sind.
Beispiel:
Soll_Winkel=45°
Ist_Winkel=315°
erg=45-315=-270
Wäre also die 4te Abfrage, also UZ

[HeXPloreR]

Beispiel:
-175° (wäre in echt ja 185°)
Eingesetzt in die Formel:
180+(180-(-175*-1))=185



ist_angle=ToDeg(yaw); //HIER DEN KOMPASSWERT AUSLESEN
if (ist_angle<0){
ist_angle=180+(180-(ist_angle*-1));
}


Ich erhalte also Winkel von 0°-360°


ist_angle=180+(180-(ist_angle*-1)); - also wenigtens das würde ich schon mal ändern. ist_angle= ist_angel + 360; (- 175° + 360° = 185°).
'Psoudocode: Prüfen ob Ist als Soll dann sorum, sonst andersrum. Naja so würde ich es wenigsten versuchen wenn ich müsste.
Die Vierquadrantenlösung wäre mir etwas zu abstrakt um die auch im Programmcode mitzuführen - wo ich dann immer dran denken müsste wo ich denn nun bin.
Weiterhin würde ich eine Bit-Variable einführen die den Übergang von >0 über0 zu <0 abdeckt, wenn das passiert ist Bit-Variable = 1, wenn der Übergang von <360 über 0 zu >0 passiert ist dann Bit =0 - dazu würde ich die GUZ verwenden.