Pincha aquí para bajarte el programa
La imagen de arriba explica gráficamente la cinemática inversa contrastado con un pequeño programa. Los nombres de las variables que están en el programa también están en el gráfico. Las fórmulas trigonométricas las puedes extraer del propio programa. El código está escrito en FreeBasic IDE, pero es 100% compatible con QBasic.
' **************************** Dos Grados de Libertad *****************************"
Screen 12
Cls
Dim As Double pi, rad, grad
Dim As Double PYa, PYb, PXa, PXb, LadoA, Alfa, Beta, Gamma
Dim As Double AngAntBr,AngBrazo, BrazoPY, BrazoPX, AntBrazoPY,_
AntBrazoPX, Hipotenusa, x, y
Dim As Integer BaseY, BaseX, LongBrazo, LongAntBr
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 = 120 ' Longitud Brazo. Puedes modificar las longitudes del brazo o antebrazo.
LongAntBr = 100 ' Longitud AnteBrazo.
x=(100) ' Posicion Inicial X. Aqui damos las coordenadas iniciales de la punta del brazo.
y=(120)+BaseY ' Posicion Inicial Y Se puede modificar los valores que están dentro del parentesis.
' Procurar que esté dentro del area de trabajo.
tecla=""
while tecla<>Chr(27) And tecla<>Chr(13)
Locate 22,24: Print "Pulsa W S A D 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
LadoA=y-BaseY
Hipotenusa=sqr((LadoA*LadoA)+(x*x))
Alfa=ATan2(LadoA,x)
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=Gamma-(180*rad) ' ANGULO ANTEBRAZO (en radianes).
PYa=LongBrazo*-sin(AngBrazo)
PYb=LongAntBr*-sin(AngAntBr+AngBrazo)
PXa=LongBrazo*COS(AngBrazo)
PXb=LongAntBr*COS(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)
AntBrazoPY=PYb+PYa+BaseY ' Punto de coordenada Y del AnteBrazo.
AntBrazoPX=PXb+PXa+BaseX ' Punto de coordenada X del AnteBrazo.
'----------------
' 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 -(AntBrazoPX, AntBrazoPY), 9 ' Dibuja linea del AnteBrazo.
'-----------------------------------------------------------------
'Muestra en pantalla los resultados de la Cinematica Inversa (IK).
'-----------------------------------------------------------------
Locate 1,20: Print "Angulos ----> Brazo:"; Int(AngBrazo*grad); Chr(248);_
" "; "AnteBrazo:"; Int((AngAntBr*grad)+180); Chr(248);" "
Locate 2,20: Print "Posicion ----> ";x;"X"; " "; y-BaseY; "Y "
'-----------------------------------------------------------------
Wend
End
