Abajo encontrarás el segundo programa. Aplica la Cinemática Inversa para tres grados de libertad: Brazo, ante-brazo y balanceo de muñeca. Puedes comprobar que los nombres que están en el gráfico corresponde con las variables del programa. De esta manera podrás razonar y extraer (si lo deseas) las funciones trigonométrica.
Está escrito en Freebasic, pero es perfectamente compatible con Qbasic. Si usas otro lenguaje de programación, traducir "Basic" y con las variables declaradas en "tipo", es lo más sencillo del mundo.
'----Cinemática Inversa para 3 grados de libertad.----
'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) Aquí 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. Aquí 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 paréntesis.
' Procurar que este dentro del área de trabajo.
balance=0 ' Ángulo relativo inicial de la Muñeca. 0 grados es horizontal, por ejemplo.
tecla=""
While tecla<>Chr(27) And tecla<>Chr(13)
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 "
'-----------------------------------------------------------------
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."
Wend
End
