Programas para Robots.
  Cinematica II
 

Me he trasladado a este "Site":

http://sites.google.com/site/proyectosroboticos/

Esta web dejó de actualizarse el 3 de marzo de 2009 a las 21:51h.






Este es el segundo programa. El programa aplica la Cinemática Inversa para tres grados de libertad. 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.



Screen 12



Pincha aquí para bajarte el programa.

'------------------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)  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

 
  14911 visitantes (29084 clics a subpáginas)  
 
=> ¿Desea una página web gratis? Pues, haz clic aquí! <=