Programas para Robots.
  Cinematica I
 


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.


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

 
  19469 visitantes (37173 clics a subpáginas)  
 
Este sitio web fue creado de forma gratuita con PaginaWebGratis.es. ¿Quieres también tu sitio web propio?
Registrarse gratis