Rechenkette? / Winkelberechnung mit Cosinus?

[Bammel]

So sry für doppelpost aber ich wollte nicht beides in einem packen!

Erstmal hab ich nen bild angehangen das ihr euch erstmal ein eindruck machen könnt was ich genau will.

So der code sieht so aus:

Declare Sub Berechnung

Dim Hoehe As Byte
Dim Abstand As Byte

Dim Hoehe_q As Word
Dim Abstand_q As Word
Dim Dis_q As Word
Dim Dis As Byte

Dim Knie_wi As Byte
Dim Kniea As Word
Dim Knieb As Word

Dim Knie_zw_a As Word
Dim Knie_zw_b As Word
Dim Knie_zw_c As Word
Dim Knie_zw_d As Integer
Dim Knie_zw_e As Single
Dim Knie_zw_wi As Single

Const Oberschenkel = 67
Const Unterschenkel = 87
Const Oberschenkel_q = 4489
Const Unterschenkel_q = 7569

Hoehe = 50
Abstand = 100

...

Sub Berechnung

If Hoehe > 110 Then Hoehe = 110
If Abstand < 37 Then Abstand = 37
Hoehe_q = Hoehe * Hoehe
Abstand_q = Abstand * Abstand
Dis_q = Hoehe_q + Abstand_q
Dis = Sqr(dis_q)


'-------------- Knie ----------------------

Knie_zw_a = Dis_q - Unterschenkel_q
Knie_zw_b = Knie_zw_a - Oberschenkel_q

Knie_zw_c = Unterschenkel * Oberschenkel
Knie_zw_d = -2 * Knie_zw_c

Knie_zw_e = Knie_zw_b / Knie_zw_d
Knie_zw_wi = Cos(knie_zw_e)
Knie_wi = Knie_zw_wi * 100

If Knie_wi < 45 Then Knie_wi = 45
If Knie_wi > 180 Then Knie_wi = 180
Kniea = 6400 / 180
Knieb = Kniea * Knie_wi
Servo(2) = 63200 - Knieb

Hab den code ein wenig verkürzt hoffe das okay so!?

formel hab ich diese verwendet: gamma = cos((c² - a² - b²) / (-2 * a * b))
a ist Unterschenkel
b ist Oberschenkel
c ist Dis
(siehe auch angehängte zeichnung)

danke schonmal fürs anschauen!

MfG Bammel