http://www.fbide.freebasic.net
Apenas mide 5 megas y es intuitivo (tiene semenjanzas con QBasic).
Asegurarse de que tenéis las librerías GLU32.DLL y GLUT32.DLL en el directorio WINDOWS, o bien en, SYSTEM, porque sinó no funcionará el interface gráfico.
En caso de no tener esas librerías podéis encontrarlas aquí:
http://www.dlldump.com/download-dll-fil ... nload.html
http://www.dlldump.com/download-dll-fil ... nload.html
La cinemática inversa la resolví gracias a una hoja de cálculo que encontré en la siguiente web:
http://www.lynxmotion.com/images/files/inversek.xls
Implementé los datos de esta hoja de cálculo y lo convertí en un programa con el que podía moverme libremente con el teclado. La parte gráfica la resolví con OpenGL.
Cuando ejecutéis el programa, podéis mover el brazo de la siguiente forma:
Teclas:
A D —–> Giro de todo el brazo.
Q E —–> Mueve linealmente el brazo en el eje de las X con respecto a la pantalla.
W S —-> Mueve linealmente el brazo en el eje de las Y con respecto a la pantalla.
Z X —–> Cabeceo de la muñeca.
C V —–> Balanceo de la muñeca.
N M —-> Abre o cierra la mano.
Pulsar "Esc" para salir.
Este es el programa:
Code: Select all
--------------------------------------------------------------------------------
#include once "GL/gl.bi"
#include once "GL/glu.bi"
#include once "GL/glut.bi"
'————————– Variables para la parte de OpenGL ————————–
Dim As single a, b, c, d, e, f=.11
Dim As String t
'——————— Variables para la parte de Cinemática Inversa ——————-
Dim As Single pi, rad, grad, k7, k9, j7, j9, i14, i15, i16, i17, b6, i18, b8, c8, c6
Dim As Single d6, d8, c10, d10, e6, e8, e10, f5, f7, f9, f11, g7, g9, g11, f18, g18
Dim As Single f19, g19, f20, g20, f21, g21, b10, j17, j18, f17, g17, j15, j16, f13, g13
Dim As Integer i5, j5, k5, b5, b7, b9, b11, g5
Dim As Integer x, y, z, o, xx, yy, oo
'——————– Inicio de variables para la Cinemática Inversa ——————–
pi = atn(1) * 4
rad = pi / 180
grad = 180 / pi
' Medidas en milímetros. (Dimensiones de las articulaciones)
b5 = 200 ' Punto Y base (hombro)
g5 = 0 ' Punto X base (hombro)
b7 = 300 ' Longitud brazo
b9 = 200 ' Longitud antebrazo
b11 = 100 ' Longitud muñeca
' Distancias iniciales en milímetros.
x = 270 ' Punto inicial de X en milimetros.
y = 130 ' Punto inicial de Y en milimetros.
z = 0 ' Ángulo inicial de Z en grados.
o = 0 ' Ángulo del cabeceo. (Muñeca).
e = 0 ' Ángulo del balanceo. (Muñeca).
'——————————— Configuración de escena para el OpenGL —————
Screen 12, 16, , 2
cls
' ReSizeGLScene————————————-
glViewport 0, 0, 640, 480 ' Reset The Current Viewport
glMatrixMode GL_PROJECTION ' Select The Projection Matrix
glLoadIdentity ' Reset The Projection Matrix
gluPerspective 45.0, 800.0/600.0, 0.2, 255.0
' Calculate The Aspect Ratio Of The Window
glMatrixMode GL_MODELVIEW ' Select The Modelview Matrix
glLoadIdentity ' Reset The Modelview Matrix
'—————————————————-
glClearColor 0.0, 0.0, 0.0, 0.5 ' Black Background
glClearDepth 1.0 ' Depth Buffer Setup
glEnable GL_DEPTH_TEST ' Enables Depth Testing
glDepthFunc GL_LEQUAL ' The Type Of Depth Testing To Do
glHint GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST
' Really Nice Perspective Calculations
'——————————Inicio del programa——————————
While t<>Chr(27) ' Termina programa pulsando "Esc".
'—————– Control del robot mediante teclado, ——————–
'—————– y delimitación del área de trabajo. ——————–
xx=x
yy=y
oo=o
t = lcase( InKey() )
If t="a" Then z=z-1
if z<0 then z=359
If t="d" Then z=z+1
if z>359 then z=0
If t="s" Then y=y-1
If t="w" Then y=y+1
if t="q" then x=x-1
if t="e" then x=x+1
if t="z" then o=o-1
if t="x" then o=o+1
if t="c" then e=e-1
if t="v" then e=e+1
if e<0 then e=359
if t="n" Then
If f>0.11 Then f=f-.01
endif
if t="m" then
If f<.4 Then f=f+.01
endif
' ———————–Calculo de la cinemática Inversa—————-
k5=x
j5=y
i5=o
f5=b5
K7=COS(RAD*i5)*B11
K9=K5-K7
J7=sin(RAD*i5)*B11
J9=J5-J7-B5
i14=sqr((J9*J9)+(K9*K9))
if i14 > (b7+b9) then
x=xx
y=yy
o=oo
k5=xx
j5=yy
i5=oo
K7=COS(RAD*i5)*B11
K9=K5-K7
J7=sin(RAD*i5)*B11
J9=J5-J7-B5
i14=sqr((J9*J9)+(K9*K9))
endif
i15=ATAN2(j9,k9)
j15=grad*i15
i16=ACOS(((B7*B7)-(B9*B9)+(i14*i14))/((2*B7)*i14))
j16=grad*i16
i17=i15+i16
J17=grad*i17
B6=J17 'B6 ANGULO BRAZO (en grados)
C6=RAD*B6 'C6 ANGULO BRAZO (en radianes)
i18=ACOS(((B7*B7)+(B9*B9)-(i14*i14))/((2*B7)*B9))
J18= -(180-(grad*i18))
B8=J18 'B8 ANGULO ANTEBRAZO (en grados)
C8=RAD*B8 'C8 ANGULO ANTEBRAZO (en radianes)
D6=sin(C6)
D8=sin(C8+C6)
B10=i5-B8-B6 'B10 ANGULO MUÑECA (en grados)
C10=RAD*B10 'C10 ANGULO MUÑECA (en radianes)
'——————————————————
a=z
b=b6
c=b8
d=b10
'——————– Animación con OpenGL—————————————
glClear(GL_COLOR_BUFFER_BIT Or GL_DEPTH_BUFFER_BIT) ' Borra pantalla.
glLoadIdentity ' Reset a la matriz de proyección.
'————————–hombro——————–
glDisable GL_BLEND
glTranslatef 0.0, 0.0, -16.0 ' Pone en posición de pantalla: x=0, Y=0, Z=-16
glRotatef -90, 0.0, 1.0, 0.0 ' y lo gira todo a -90 en el eje X.
glBegin(GL_QUADS) ' Plano base Eje Y=0
glColor3f 0.2, 0.2, 0.3 ' Pone color (RGB) a este lado del rectángulo.
glVertex3f 5.5, -2.0, 5.5 ' Top Right Of The Quad (Bottom)
glVertex3f -5.5, -2.0, 5.5 ' Top Left Of The Quad (Bottom)
glVertex3f -5.5, -2.0, -5.5 ' Bottom Left Of The Quad (Bottom)
glVertex3f 5.5, -2.0, -5.5 ' Bottom Right Of The Quad (Bottom)
glend
glRotatef a, 0.0, 1.0, 0.0 ' Rota figura en el eje X según ángulo "a"
glBegin(GL_QUADS)
glColor3f 0.0, 0.2, 1.0 ' Poner color (RGB) a este lado del rectángulo.
glVertex3f 0.7, 0.0, -0.6 ' Arriba derecha del cuadrado.
glVertex3f -0.7, 0.0, -0.6 ' Arriba izquierda del cuadrado.
glVertex3f -0.7, 0.0, 0.6 ' Abajo izquierda del cuadrado.
glVertex3f 0.7, 0.0, 0.6 ' Abajo derecha del cuadrado.
glColor3f 0.2, 0.2, 1.0 ' Pone color (RGB) a este lado del rectángulo.
glVertex3f 0.7, -2.0, 0.6 ' Top Right Of The Quad (Bottom)
glVertex3f -0.7, -2.0, 0.6 ' Top Left Of The Quad (Bottom)
glVertex3f -0.7, -2.0, -0.6 ' Bottom Left Of The Quad (Bottom)
glVertex3f 0.7, -2.0, -0.6 ' Bottom Right Of The Quad (Bottom)
glColor3f 0.4, 0.1, 1.0 ' Set The Color To Red
glVertex3f 0.7, 0.0, 0.6 ' Top Right Of The Quad (Front)
glVertex3f -0.7, 0.0, 0.6 ' Top Left Of The Quad (Front)
glVertex3f -0.7, -2.0, 0.6 ' Bottom Left Of The Quad (Front)
glVertex3f 0.7, -2.0, 0.6 ' Bottom Right Of The Quad (Front)
glColor3f 0.1, 0.5, 1.0 ' Set The Color To Yellow
glVertex3f 0.7, -2.0, -0.6 ' Top Right Of The Quad (Back)
glVertex3f -0.7, -2.0, -0.6 ' Top Left Of The Quad (Back)
glVertex3f -0.7, 0.0, -0.6 ' Bottom Left Of The Quad (Back)
glVertex3f 0.7, 0.0, -0.6 ' Bottom Right Of The Quad (Back)
glColor3f 0.2, 0.4, 1.0 ' Set The Color To Blue
glVertex3f -0.7, 0.0, 0.6 ' Top Right Of The Quad (Left)
glVertex3f -0.7, 0.0, -0.6 ' Top Left Of The Quad (Left)
glVertex3f -0.7, -2.0, -0.6 ' Bottom Left Of The Quad (Left)
glVertex3f -0.7, -2.0, 0.6 ' Bottom Right Of The Quad (Left)
glColor3f 0.0, 0.3, 1.0 ' Set The Color To Violet
glVertex3f 0.7, 0.0, -0.6 ' Top Right Of The Quad (Right)
glVertex3f 0.7, 0.0, 0.6 ' Top Left Of The Quad (Right)
glVertex3f 0.7, -2.0, 0.6 ' Bottom Left Of The Quad (Right)
glVertex3f 0.7, -2.0, -0.6
glEnd()
'————————-brazo———————-
glTranslatef 0.0, 0.0, 0.0
glRotatef b, 0.0, 0.0, 1.0
glColor3f 0.0, 1.0, 0.0
glutSolidSphere 0.6, 11, 11
glBegin(GL_QUADS)
glColor3f 1.0, 0.0, 0.0
glVertex3f 3.0, 0.3, -0.4 ' Top Right Of The Quad (Top)
glVertex3f 0.0, 0.3, -0.4 ' Top Left Of The Quad (Top)
glVertex3f 0.0, 0.3, 0.4 ' Bottom Left Of The Quad (Top)
glVertex3f 3.0, 0.3, 0.4 ' Bottom Right Of The Quad (Top)
glColor3f 1.0, 0.5, 0.0 ' Set The Color To Orange
glVertex3f 3.0, -0.3, 0.4 ' Top Right Of The Quad (Bottom)
glVertex3f 0.0, -0.3, 0.4 ' Top Left Of The Quad (Bottom)
glVertex3f 0.0, -0.3, -0.4 ' Bottom Left Of The Quad (Bottom)
glVertex3f 3.0, -0.3, -0.4 ' Bottom Right Of The Quad (Bottom)
glColor3f 1.0, 0.0, 0.5 ' Set The Color To Red
glVertex3f 3.0, 0.3, 0.4 ' Top Right Of The Quad (Front)
glVertex3f 0.0, 0.3, 0.4 ' Top Left Of The Quad (Front)
glVertex3f 0.0, -0.3, 0.4 ' Bottom Left Of The Quad (Front)
glVertex3f 3.0, -0.3, 0.4 ' Bottom Right Of The Quad (Front)
glColor3f 1.0, 0.2, 0.0 ' Set The Color To Yellow
glVertex3f 3.0, -0.3, -0.4 ' Top Right Of The Quad (Back)
glVertex3f 0.0, -0.3, -0.4 ' Top Left Of The Quad (Back)
glVertex3f 0.0, 0.3, -0.4 ' Bottom Left Of The Quad (Back)
glVertex3f 3.0, 0.3, -0.4 ' Bottom Right Of The Quad (Back)
glColor3f 1.0, 0.7, 0.2 ' Set The Color To Blue
glVertex3f 0.0, 0.3, 0.4 ' Top Right Of The Quad (Left)
glVertex3f 0.0, 0.3, -0.4 ' Top Left Of The Quad (Left)
glVertex3f 0.0, -0.3, -0.4 ' Bottom Left Of The Quad (Left)
glVertex3f 0.0, -0.3, 0.4 ' Bottom Right Of The Quad (Left)
glColor3f 1.0, 0.8, 0.3 ' Set The Color To Violet
glVertex3f 3.0, 0.3, -0.4 ' Top Right Of The Quad (Right)
glVertex3f 3.0, 0.3, 0.4 ' Top Left Of The Quad (Right)
glVertex3f 3.0, -0.3, 0.4
glVertex3f 3.0, -0.3, -0.4
glEnd()
'————————Ant.Brazo——————-
glTranslatef 3.0, 0.0, 0.0
glRotatef c, 0.0, 0.0, 1.0
glColor3f 0.0, 0.0, 1.0
glutSolidSphere 0.5, 11, 11
glBegin(GL_QUADS)
glColor3f 0.0, 1.0, 0.0
glVertex3f 2.0, 0.3, -0.3
glVertex3f 0.0, 0.3, -0.3
glVertex3f 0.0, 0.3, 0.3
glVertex3f 2.0, 0.3, 0.3
glColor3f 0.1, 1.0, 0.2
glVertex3f 2.0, -0.3, 0.3
glVertex3f 0.0, -0.3, 0.3
glVertex3f 0.0, -0.3, -0.3
glVertex3f 2.0, -0.3, -0.3
glColor3f 0.2, 1.0, 0.5
glVertex3f 2.0, 0.3, 0.3
glVertex3f 0.0, 0.3, 0.3
glVertex3f 0.0, -0.3, 0.3
glVertex3f 2.0, -0.3, 0.3
glColor3f 0.4, 1.0, 0.2
glVertex3f 2.0, -0.3, -0.3
glVertex3f 0.0, -0.3, -0.3
glVertex3f 0.0, 0.3, -0.3
glVertex3f 2.0, 0.3, -0.3
glColor3f 0.0, 1.0, 0.5
glVertex3f 0.0, 0.3, 0.3
glVertex3f 0.0, 0.3, -0.3
glVertex3f 0.0, -0.3, -0.3
glVertex3f 0.0, -0.3, 0.3
glColor3f 0.3, 1.0, 0.2
glVertex3f 2.0, 0.3, -0.3
glVertex3f 2.0, 0.3, 0.3
glVertex3f 2.0, -0.3, 0.3
glVertex3f 2.0, -0.3, -0.3
glEnd()
'———————— Mano ———————
glTranslatef 2.0, 0.0, 0.0
glRotatef d, 0.0, 0.0, 1.0
glrotatef e, 1.0, 0.0, 0.0
glColor3f 1.0, 0.0, 0.0
glutSolidSphere 0.4, 13, 13
glBegin(GL_QUADS)
glColor3f 0.0, 0.0, 1.0
glVertex3f 0.5, 0.3, -0.2
glVertex3f 0.0, 0.3, -0.2
glVertex3f 0.0, 0.3, 0.2
glVertex3f 0.5, 0.3, 0.2
glColor3f 0.2, 0.0, 1.0
glVertex3f 0.5, -0.3, 0.2
glVertex3f 0.0, -0.3, 0.2
glVertex3f 0.0, -0.3, -0.2
glVertex3f 0.5, -0.3, -0.2
glColor3f 0.0, 0.3, 1.0
glVertex3f 0.5, 0.3, 0.2
glVertex3f 0.0, 0.3, 0.2
glVertex3f 0.0, -0.3, 0.2
glVertex3f 0.5, -0.3, 0.2
glColor3f 0.0, 0.4, 1.0
glVertex3f 0.5, -0.3, -0.2
glVertex3f 0.0, -0.3, -0.2
glVertex3f 0.0, 0.3, -0.2
glVertex3f 0.5, 0.3, -0.2
glColor3f 0.5, 0.0, 1.0
glVertex3f 0.0, 0.3, 0.2
glVertex3f 0.0, 0.3, -0.2
glVertex3f 0.0, -0.3, -0.2
glVertex3f 0.0, -0.3, 0.2
glColor3f 0.2, 0.2, 1.0
glVertex3f 0.5, 0.3, -0.2
glVertex3f 0.5, 0.3, 0.2
glVertex3f 0.5, -0.3, 0.2
glVertex3f 0.5, -0.3, -0.2
glEnd()
glTranslatef 0.5, 0.0, f
glBegin(GL_QUADS)
glColor3f 0.0, 0.0, 1.0
glVertex3f 0.5, 0.2, -0.1
glVertex3f 0.0, 0.2, -0.1
glVertex3f 0.0, 0.2, 0.1
glVertex3f 0.5, 0.2, 0.1
glColor3f 0.2, 0.0, 1.0
glVertex3f 0.5, -0.2, 0.1
glVertex3f 0.0, -0.2, 0.1
glVertex3f 0.0, -0.2, -0.1
glVertex3f 0.5, -0.2, -0.1
glColor3f 0.0, 0.2, 1.0
glVertex3f 0.5, 0.2, 0.1
glVertex3f 0.0, 0.2, 0.1
glVertex3f 0.0, -0.2, 0.1
glVertex3f 0.5, -0.2, 0.1
glColor3f 0.0, 0.4, 1.0
glVertex3f 0.5, -0.2, -0.1
glVertex3f 0.0, -0.2, -0.1
glVertex3f 0.0, 0.2, -0.1
glVertex3f 0.5, 0.2, -0.1
glColor3f 0.5, 0.0, 1.0
glVertex3f 0.0, 0.2, 0.1
glVertex3f 0.0, 0.2, -0.1
glVertex3f 0.0, -0.2, -0.1
glVertex3f 0.0, -0.2, 0.1
glColor3f 0.2, 0.2, 1.0
glVertex3f 0.5, 0.2, -0.1
glVertex3f 0.5, 0.2, 0.1
glVertex3f 0.5, -0.2, 0.1
glVertex3f 0.5, -0.2, -0.1
glEnd()
gltranslatef 0.0, 0.0, f*(-2) ' En esta posición, F*(-2) invierte
' la posición F de arriba.
glBegin(GL_QUADS)
glColor3f 0.0, 0.0, 1.0
glVertex3f 0.5, 0.2, 0.1
glVertex3f 0.0, 0.2, 0.1
glVertex3f 0.0, 0.2, -0.1
glVertex3f 0.5, 0.2, -0.1
glColor3f 0.2, 0.0, 1.0
glVertex3f 0.5, -0.2, -0.1
glVertex3f 0.0, -0.2, -0.1
glVertex3f 0.0, -0.2, 0.1
glVertex3f 0.5, -0.2, 0.1
glColor3f 0.0, 0.1, 1.0
glVertex3f 0.5, 0.2, -0.1
glVertex3f 0.0, 0.2, -0.1
glVertex3f 0.0, -0.2, -0.1
glVertex3f 0.5, -0.2, -0.1
glColor3f 0.0, 0.4, 1.0
glVertex3f 0.5, -0.2, 0.1
glVertex3f 0.0, -0.2, 0.1
glVertex3f 0.0, 0.2, 0.1
glVertex3f 0.5, 0.2, 0.1
glColor3f 1.0, 0.0, 1.0
glVertex3f 0.0, 0.2, -0.1
glVertex3f 0.0, 0.2, 0.1
glVertex3f 0.0, -0.2, 0.1
glVertex3f 0.0, -0.2, -0.1
glColor3f 0.2, 0.2, 1.0
glVertex3f 0.5, 0.2, 0.1
glVertex3f 0.5, 0.2, -0.1
glVertex3f 0.5, -0.2, -0.1
glVertex3f 0.5, -0.2, 0.1
glEnd()
Flip()'<—– Muestra el gráfico por pantalla——-
Wend
end