'Color 0,7
Cls
   
Dim As Double   pi, rad, grad
Dim As Double   PYa, PYb,PYc, PXa, PXb, PXc, Afx, Afy, LadoA, LadoB, Alfa, Beta, Gamma
Dim As Double   AngAntBr,AngBrazo, BrazoPY, BrazoPX, AntBrPY,_
                AntBrPX, MunecPY, MunecPX, Hipotenusa, x, y, balance, AngMunec
       
Dim As Integer  BaseY, BaseX, LongBrazo, LongAntBr, LongMunec

Dim As String   tecla

pi   = Atn(1) * 4
rad  = pi  / 180
grad = 180 /  pi

BaseX  = 300      ' Punto X Base (hombro)  Aqui situamos el brazo en pantalla.
BaseY  = 250      ' punto Y Base (hombro)

LongBrazo  = 100  ' Longitud Brazo.    
LongAntBr  =  90  ' Longitud AnteBrazo.
LongMunec  =  40  ' Longitud Muñeca.

x=(100)           ' Posicion Inicial X.    Aqui damos las coordenadas iniciales de la punta del brazo.
y=(150)+BaseY     ' Posicion Inicial Y.    Se puede modificar los valores que están dentro del parentesis.
                  '                        Procurar que este dentro del area de trabajo.
balance=0         ' Angulo Relativo Inicial de la Muñeca. 0 grados es horizontal, por ejemplo.

tecla=""
While tecla<>Chr(27) And tecla<>Chr(13)
 
  Locate 22,24: Print "Pulsa W S A D Q E para mover el Brazo."
  Locate 23,27: Print "   Para salir pulsa Esc o Enter."
  
  tecla=LCase(InKey)
  
  If tecla="a" Then x=x-1
  If tecla="d" Then x=x+1
  If tecla="s" Then y=y-1
  If tecla="w" Then y=y+1
  If tecla="q" Then balance=balance-1
  If tecla="e" Then balance=balance+1
  
  Afx=Cos(RAD*balance)*LongMunec
  LadoB=x-Afx
  
  Afy=Sin(RAD*balance)*LongMunec
  LadoA=y-Afy-BaseY
     
  Hipotenusa=Sqr((LadoA*LadoA)+(LadoB*LadoB))
  
  Alfa=ATan2(LadoA,LadoB)
  
  Beta=ACos(((LongBrazo*LongBrazo)-(LongAntBr*LongAntBr)+(Hipotenusa*Hipotenusa))/((2*LongBrazo)*Hipotenusa))
  
  AngBrazo= Alfa+Beta                                   ' ANGULO BRAZO     (en radianes).
  
  Gamma=ACos(((LongBrazo*LongBrazo)+(LongAntBr*LongAntBr)-(Hipotenusa*Hipotenusa))/((2*LongBrazo)*LongAntBr))
  AngAntBr=-((180*rad)-Gamma)                           ' ANGULO ANTEBRAZO (en radianes).
  
  AngMunec= ((rad*balance)-AngBrazo-AngAntBr)           ' ANGULO MUÑECA    (en radianes).
  
  PXa=LongBrazo* Cos(AngBrazo)
  PYa=LongBrazo*-Sin(AngBrazo)
  
  PXb=LongAntBr* Cos(AngAntBr+AngBrazo)
  PYb=LongAntBr*-Sin(AngAntBr+AngBrazo)
  
  PXc=LongMunec* Cos(AngMunec+AngAntBr+AngBrazo)
  PYc=LongMunec*-Sin(AngMunec+AngAntBr+AngBrazo)
  
  '---------------------
  ' PUNTOS PARA DIBUJAR.
  '---------------------
  
  'BRAZO (x,y)
  BrazoPY=PYa+BaseY           ' Punto de coordenada Y del Brazo.
  BrazoPX=PXa+BaseX           ' Punto de coordenada X del Brazo.
  
  'ANTEBRAZO (x,y)
  AntBrPY=PYb+PYa+BaseY       ' Punto de coordenada Y del AnteBrazo.
  AntBrPX=PXb+PXa+BaseX       ' Punto de coordenada X del AnteBrazo.
  
  'MUÑECA (x, y)
  MunecPY=PYc+PYb+PYa+BaseY   ' Punto de coordenada Y de la Muñeca.
  MunecPX=PXc+PXb+PXa+BaseX   ' Punto de coordenada X de la Muñeca.

  '----------------  
  ' Dibuja Lineas.
  '----------------
  
  If tecla<>"" Then Cls               ' Una manera simple de borrar las lineas anteriores.          
  
  PSet   (BaseX,      BaseY     ), 0  ' Punto inicial.
  
  Line  -(BrazoPX,    BrazoPY   ), 4  ' Dibuja linea del Brazo.
  
  Line  -(AntBrPX,    AntBrPY   ), 9  ' Dibuja linea del AnteBrazo.
  
  Line  -(MunecPX,    MunecPY   ), 11 ' Dibuja linea de la muñeca. 
  
  '-----------------------------------------------------------------   
  'Muestra en pantalla los resultados de la Cinematica Inversa (IK).     
  '-----------------------------------------------------------------
  
  Locate 1,10:  Print "Angulos  ----> Brazo:"; Int(AngBrazo*grad); Chr(248);_
                      "  "; "AnteBrazo:"; Int((AngAntBr*grad)+180); Chr(248);_
                      "  "; "Muneca:"; Int((AngMunec*grad)+180); Chr(248);"  "
  Locate 2,10:  Print "Posicion ---->";x;"X"; "  "; y-BaseY;   "Y   "
  
  '-----------------------------------------------------------------
Wend

End