Nachdem ich hier die Servos durch Tabellen angesteuert habe, soll jetzt eine 'berechnende' Version erstellt werden. Die Mechanik ist die gleiche, nur sollen die Servos unten angeordnet werden, so wie auf dem ersten Video zu sehen ist.
(Ich sag's gleich, die Rechenschritte und Formeln dazu hab' ich mir nicht ausgedacht wahrscheinlich alles mal gelernt, dann doch nie gebraucht und somit nur als eingetrockneter Bodensatz in meinem Gedächtnis vorhanden.)
Der erste Schritt ist eine Skizze, in der die relevanten Größen bezeichnet sind, die hab' ich aus copyright-Gründen selbst gemalt.
Inverse_Kinematik.png
Das entspricht meiner vorhandenen Mechanik
Die folgende Berechnung gibt die Winkel wieder, die die Servos einnehmen müssen um den Stift in seine Position zu bringen. Der Stift soll Stift_x,Stift_y anfahren, dazu dreht der Servo1 den Hebel L1 auf den Winkel Ws1 und der Servo2 den Hebel L3 auf den Winkel Ws2. Alle Winkel sind als radiant ausgedrückt. Die eingetragenen Werte entsprechen meinem Versuchsaufbau.
Alles anzeigen
(Ich sag's gleich, die Rechenschritte und Formeln dazu hab' ich mir nicht ausgedacht wahrscheinlich alles mal gelernt, dann doch nie gebraucht und somit nur als eingetrockneter Bodensatz in meinem Gedächtnis vorhanden.)
Der erste Schritt ist eine Skizze, in der die relevanten Größen bezeichnet sind, die hab' ich aus copyright-Gründen selbst gemalt.
Inverse_Kinematik.png
Das entspricht meiner vorhandenen Mechanik
Die folgende Berechnung gibt die Winkel wieder, die die Servos einnehmen müssen um den Stift in seine Position zu bringen. Der Stift soll Stift_x,Stift_y anfahren, dazu dreht der Servo1 den Hebel L1 auf den Winkel Ws1 und der Servo2 den Hebel L3 auf den Winkel Ws2. Alle Winkel sind als radiant ausgedrückt. Die eingetragenen Werte entsprechen meinem Versuchsaufbau.
BASCOM-Quellcode
- 'Inverse Kinematik
- 'Strecken
- Dim L1 As Single , L2 As Single , L3 As Single , L4 As Single , L5 As Single , L6 As Single , L7 As Single , L8 As Single
- 'Winkel
- Dim Wa1 As Single , Wb1 As Single , Wa2 As Single , Wb2 As Single , Wc2 As Single , Wd2 As Single , We2 As Single , Ws1 As Single , Ws2 As Single
- 'Koordinaten
- Dim Servo1_x As Single , Servo2_x As Single , Dp1_x As Single , Dp2_x As Single , Dp3_x As Single , Stift_x As Single
- Dim Servo1_y As Single , Servo2_y As Single , Dp1_y As Single , Dp2_y As Single , Dp3_y As Single , Stift_y As Single
- 'Hilfsvariable
- Dim Z1 As Single , Z2 As Single , Z3 As Single , Z4 As Single
- Const Pi = 3.1416
- 'konstante Werte haben:
- 'Hebelarme
- L1 = 130 : L2 = 130 : L3 = 130 : L4 = 130
- 'Stifthalter
- L5 = 157.7 : L6 = 40.4
- 'servos
- Servo1_x = 75 : Servo1_y = 0
- Servo2_x = 140 : Servo2_y = 15
- 'Wd2 mit L5, L6 und L4 nach dem Kosinussatz bestimmen
- Z1 = L5 ^ 2
- Z2 = L6 ^ 2
- Z3 = L4 ^ 2
- Z4 = Z1 + Z2
- Z4 = Z4 - z3
- Z4 = Z4 / 2
- Z4 = Z4 / L5
- Z4 = Z4 / L6
- Wd2 = Acos(z4)
- 'Aus Stiftposition Winkel Ws2 des Servo2 berechnen
- 'Wa2 mit arctan2 berechnen
- Z1 = Stift_y - servo2_y
- Z2 = Stift_x - Servo2_x
- Wa2 = Atn2(z1 , Z2)
- If Wa2 < 0 Then Wa2 = Wa2 + Pi
- 'L8 mit Pythagoras berechnen
- Z1 = Z1 ^ 2 'z1 und Z2 sind noch die gleichen Werte
- Z2 = Z2 ^ 2
- Z3 = Z1 + Z2
- L8 = Sqr(z3)
- 'Wb2 mit L3, L8 und L5 nach dem Kosinussatz bestimmen
- Z1 = L3 ^ 2
- Z2 = L8 ^ 2
- Z3 = L5 ^ 2
- Z4 = Z1 + Z2
- Z4 = Z4 - z3
- Z4 = Z4 / 2
- Z4 = Z4 / L3
- Z4 = Z4 / L8
- Wb2 = Acos(z4)
- Ws2 = Wa2 - Wb2
- 'um den Winkel Ws1 zu berechnen, wird die Position von Dp3 benötigt
- 'Wc2 mit L5, L8 und L3 nach dem Kosinussatz bestimmen
- Z1 = L5 ^ 2
- Z2 = L8 ^ 2
- Z3 = L3 ^ 2
- Z4 = Z1 + Z2
- Z4 = Z4 - Z3
- Z4 = Z4 / 2
- Z4 = Z4 / L5
- Z4 = Z4 / L8
- Wc2 = Acos(z4)
- We2 = Wa2 + Wc2
- We2 = We2 - Wd2
- We2 = We2 + Pi
- 'Koordinaten von Dp3 bestimmen
- Z1 = L6 * Cos(we2)
- Dp3_x = Stift_x + Z1
- Z1 = L6 * Sin(we2)
- Dp3_y = Stift_y + Z1
- 'Wa1 mit arctan2 berechnen
- Z1 = Dp3_y - Servo1_y
- Z2 = Dp3_x - Servo1_x
- Wa1 = Atn2(z1 , Z2)
- If Wa1 < 0 Then Wa1 = Wa1 + Pi
- 'L7 mit Pythagoras berechnen
- Z1 = Z1 ^ 2 'z1 und Z2 sind noch die gleichen Werte
- Z2 = Z2 ^ 2
- Z3 = Z1 + Z2
- L7 = Sqr(z3)
- 'Wb1 mit L1, L7 und L2 nach dem Kosinussatz bestimmen
- Z1 = L1 ^ 2
- Z2 = L7 ^ 2
- Z3 = L2 ^ 2
- Z4 = Z1 + Z2
- Z4 = Z4 - Z3
- Z4 = Z4 / 2
- Z4 = Z4 / L1
- Z4 = Z4 / L7
- Wb1 = Acos(z4)
- Ws1 = Wa1 + Wb1
Raum für Notizen
-----------------------------------------------------------------------------------------------------
-----------------------------------------------------------------------------------------------------
-----------------------------------------------------------------------------------------------------
-----------------------------------------------------------------------------------------------------
Dieser Beitrag wurde bereits 1 mal editiert, zuletzt von tschoeatsch () aus folgendem Grund: paar Formeln waren falsch und negative Winkel werden jetzt zu positiven umgerechnet. Eine Kontrolle mit Taschenrechner und zeichnerische Konstruktion gibt übereinstimmende Ergebnisse.