So nachdem es jetzt einige Tage ruhiger war um meinen Hexa und ihr vermutlich heute schon im Fehlschlag-Thread gelesen habt, dass mir ein Servo abgeraucht ist, gibt es nun News (wie man auf altbayrisch so schön sagt).
1) Der Servo ist komplett hin, da hilft nur noch austauschen, daher bewegt sich Bein B auch so komisch.
2) Die inverse Kinematik ist nun vollständig implementiert.
Was bedeutet das?
Ich muss die Winkel der Servos nicht mehr ausrechnen sondern sie werden berechnet aus folgenden Größen:
Translatorisch:
dx - die x-Richtung der Fussspitze gemessen von der Hüfte
dy - die y-Richtung der Fussspitze auch gemessen von der Hüfte.
--> über den Atan2 wird daraus der Winkel für die Hüfte!
--> Mit der Höhe h werden daraus die Winkel für Schulter und Fuss
Rotatorisch:
hbasis - die Grundhöhe des Hexas über dem Tisch
Winkel Querachse -Rotation um die x-Achse also wie viel der Körper sich nach vorn oder hinten neigen soll
Winkel Längsachse - Rotation um die y-Achse, also wie viel der Körper sich links oder recht neigt.
wy - Wo sich der Drehpunkt für Q befindet auf der y Achse
wx - Wo sich der Drehpunkt für L befindet auf der x Achse
Was heißt das jetzt?
Aus den beiden Winkeln Q und L mit ihrer Position lässt sich der Körper beliebig kippen um die beiden Achsen x und y (drehen um z würde über die dx und dy werte gehen), wie bei einem Flugzeug. Durch die Parameter wx und wy lässt sich der Drehpunkt an eine beliebige Stelle auf dem Roboter (oder auch außerhalb) verschieben und so um jeden Punkt kippen.
Ich verstehs immer noch nicht?
Dann schaut euch das Video an:
Wie gesagt Bein B (das Mittlere rechts) hat einen defekten Schulterservo.
Die Kunst daran ist zum einen die reinen Formeln in Mathe aufzustellen, hier reicht aber Sinus, Tangens und Cosinus Wissen aus. Danach muss man diese Formeln schön im Programm umsetzen, was deutlich aufwendiger ist, da hier die Syntax des Programms zum tragen kommt:
Beispiel für Bein A
Code:
dQ := math.Sin(math.Radians(wQ))
dL := math.Sin(math.Radians(wL))
dhAF := math.FMul(dQ, math.FSub(+230.0, -wy))
dhABC := math.FMul(dL, math.FSub(-110.0, -wx))
hA := math.FAdd(hBasis, dhAF)
'~~~~~ BERECHNUNG SERVOWINKEL ALPHA GAMMA PHI
'----- Länge von Bein A ----
ca_2 := math.FAdd(math.FMul(hA, hA), math.FMul(rA,rA))
ca := math.FSqr(ca_2)
'----- Winkel von Hüfte A0 ----
phi := math.Degrees(math.Atan2(xA, yA))
phi := math.FAdd(math.FMul(math.FSub(phi, 5.0), 8.0), 809.0)
Aziel[0] := math.FRound(phi)
Aziel[0] := Aziel[0] +0
'----- Winkel Schulter A1 ----
alpha_P := math.ATan(math.FDiv(rA, hA))
alpha_PP := math.ACos(math.FDiv(math.FAdd(aa_m_bb, ca_2), math.FMul(ax2, ca)))
alpha := math.Degrees(math.FAdd(alpha_P, alpha_PP) )
Aziel[1] := math.FRound(math.FAdd(math.FMul(math.FSub(alpha, 5.0), 8.0),809.0))
Aziel[1] := Aziel[1] - 60
'----- Winkel Fuss A2 ----
gamma := math.ACos(math.FDiv(math.FSub(aa_p_bb, ca_2), abx2))
gamma := math.Degrees(gamma)
gamma := math.FAdd(math.FMul(math.FSub(gamma, 5.0), 8.0), 809.0)
Aziel[2] := math.FSub(2910, math.FRound(gamma))
Aziel[2] := Aziel[2] +80
Hier hilft es die Berechnung in Excel aufzubauen mit einer grafischen Darstellung zu prüfen ob das überhaupt funktionieren kann bzw. was bei den Werten herauskommen muss. Leider habe ich keine Möglichkeit die Werte nach der Berechnung am Hexa ausgeben zu lassen, da er kein Display oder eine Schnittstelle dafür hat.
Noch Fragen?
Berechtigungen
Zitieren
Lesezeichen