johnny-five/wip/ik-basic.js

The JavaScript Arduino Programming Framework.

Private Sub Command1_Click()

  'inputs
  pi = 3.14159265359
  femurlength = 57
  tibialength = 108
  coxalength = 29

  x = Val(Text3.Text)
  y = Val(Text4.Text)
  z = Val(Text5.Text)

  'calculations
  knee = ACos(((Sqr(((Sqr(x ^ 2 + z ^ 2)) - coxalength) ^ 2 + y ^ 2)) ^ 2 - tibialength ^ 2 - femurlength ^ 2) / (-2 * femurlength * tibialength)) * 180 / pi
  swing = Atn(z / x) * 180 / pi
  lift = (((Atn(((Sqr(x ^ 2 + z ^ 2)) - coxalength) / y)) + (ACos((tibialength ^ 2 - femurlength ^ 2 - (Sqr(((Sqr(x ^ 2 + z ^ 2)) - coxalength) ^ 2 + y ^ 2)) ^ 2) / (-2 * femurlength * (Sqr(((Sqr(x ^ 2 + z ^ 2)) - coxalength) ^ 2 + y ^ 2)))))) * 180 / pi) - 90

  'display
  Label10.Caption = FormatNumber(lift, 1)
  Label11.Caption = FormatNumber(knee, 1)
  Label12.Caption = FormatNumber(swing, 1)

  Label13.Caption = 1436 + 9.7 * CInt(lift)
  Label14.Caption = 402 + 9.7 * CInt(knee)
  Label15.Caption = 1500 + 9.7 * CInt(swing)
End Sub


' arc sine
' error if value is outside the range [-1,1]

Function ASin(value As Double) As Double
    If Abs(CDbl(Val(value))) <> 1 Then
        ASin = Atn(value / Sqr(1 - value * value))
    Else
        ASin = 1.5707963267949 * Sgn(value)
    End If
End Function

' arc cosine
' error if NUMBER is outside the range [-1,1]

Function ACos(ByVal number As Double) As Double
    If Abs(CDbl(Val(number))) <> 1 Then
        ACos = 1.5707963267949 - Atn(number / Sqr(1 - number * number))
    ElseIf number = -1 Then
        ACos = 3.14159265358979
    End If
    'elseif number=1 --> Acos=0 (implicit)
End Function