Programa de Matlab, para la comprobación de la cinemática directa:
--- %Las medidas están en milímetros
VC=[300 250 250 150]; d1= input ('Valor de d1 = '); t2= input ('Valor de t2 = '); t3= input ('Valor de t3 = '); t4= input ('Valor de t4 = '); t5= input ('Valor de t5 = '); t6= input ('Valor de t6 = '); VV=[d1 t2 t3 t4 t5 t6]; --- function [M]=CineDirecta(VV,VC)
%Para calcular la cinemática directa de un robot de 6gdl TRRRRR a partir de %vectores de entrada.
%Programa basado en el trabajo del Dr. Emmanuel Alejandro Merchán Cruz %VV=Vector Variable, VC=Vector constante
d1=[VV(1)]; t2=[VV(2)]; t3=[VV(3)]; t4=[VV(4)]; t5=[VV(5)]; t6=[VV(6)]; a3=[VC(1)]; a4=[VC(2)]; d2=[VC(3)]; d6=[VC(4)];
fprintf('Los parámetros son: \n')
fprintf('a3=%f, a4=%f , d2=%f , d6=%f \n',a3,a4,d2,d6)
fprintf('d1=%f, t2=%f , t3=%f , t4=%f, t5=%f , t6=%f \n',d1,t2,t3,t4,t5,t6) M(1,1)=(-cos(t2)*cos(t3+t4+t5)*cos(t6)+sin(t2)*sin(t6)); M(1,2)=(-cos(t2)*cos(t3+t4+t5)*sin(t6)+sin(t2)*cos(t6)); M(1,3)=(-cos(t2)*sin(t3+t4+t5)); M(1,4)=(-cos(t2)*sin(t3+t4+t5)*d6+cos(t2)*(-a4*cos(t3+t4)-a3*sin(t3))); M(2,1)=(-sin(t3+t4+t5)*cos(t6)); M(2,2)=(sin(t3+t4+t5)*sin(t6)); M(2,3)=(cos(t3+t4+t5)); M(2,4)=(cos(t3+t4+t5)*d6-a4*sin(t3+t4)-a3*sin(t3)+d2); M(3,1)=(sin(t2)*cos(t3+t4+t5)*cos(t6)+cos(t2)*sin(t6)); M(3,2)=(-sin(t2)*cos(t3+t4+t5)*sin(t6)+cos(t2)*cos(t6)); M(3,3)=(sin(t2)*sin(t3+t4+t5)); M(3,4)=(sin(t2)*sin(t3+t4+t5)*d6+sin(t2)*(a4*cos(t3+t4)+a3*cos(t3))+d1); M(4,1)=0; M(4,2)=0;
M(4,3)=0; M(4,4)=1;
--- La salida sería entonces:
>> CineDirecta(VV,VC) Los parámetros son:
a3=300.000000, a4=250.000000 , d2=250.000000 , d6=150.000000 d1=0.000000, t2=1.570796 , t3=0.000000 , t4=0.000000, t5=1.570796 , t6=0.000000 ans = -0.0000 1.0000 -0.0000 -0.0000 -1.0000 0 0.0000 250.0000 0.0000 0.0000 1.0000 700.0000 0 0 0 1.0000
El resultado corresponde a la matriz previamente calculada.
Sección 2
Programa de Mathematica 5, para determinar el torque de los motores:
Sección 3
Código del programa para controlar un robot de 6 grados de libertad (PC)
'variable para bucles Public i As Integer 'Direccion de puertos Public Const Datos = &H378 Public Const Estados = &H379 Public Const Control = &H37A
'definiciones para el control del puerto paralelo
Declare Sub PortOut Lib "IO.DLL" (ByVal Port As Integer, ByVal Data As Byte) Declare Sub PortWordOut Lib "IO.DLL" (ByVal Port As Integer, ByVal Data As Integer) Declare Sub PortDWordOut Lib "IO.DLL" (ByVal Port As Integer, ByVal Data As Long) Declare Function PortIn Lib "IO.DLL" (ByVal Port As Integer) As Byte
Declare Function PortWordIn Lib "IO.DLL" (ByVal Port As Integer) As Integer Declare Function PortDWordIn Lib "IO.DLL" (ByVal Port As Integer) As Long Declare Sub SetPortBit Lib "IO.DLL" (ByVal Port As Integer, ByVal Bit As Byte) Declare Sub ClrPortBit Lib "IO.DLL" (ByVal Port As Integer, ByVal Bit As Byte) Declare Sub NotPortBit Lib "IO.DLL" (ByVal Port As Integer, ByVal Bit As Byte)
Declare Function GetPortBit Lib "IO.DLL" (ByVal Port As Integer, ByVal Bit As Byte) As Boolean Declare Function RightPortShift Lib "IO.DLL" (ByVal Port As Integer, ByVal Val As Boolean) As Boolean Declare Function LeftPortShift Lib "IO.DLL" (ByVal Port As Integer, ByVal Val As Boolean) As Boolean Declare Function IsDriverInstalled Lib "IO.DLL" () As Boolean
Public Sub EXECUTE() Main!Timer1.Enabled = True End Sub
Public Sub DETENER() Main!Timer1.Enabled = False Main!Timer2.Enabled = False End Sub
Public Sub WRITE_PORT() i = i + 1
Main!ProgressBar.Value = i Dim BUS As Byte
msg = MsgBox("Ningun dispositivo conectado.", vbCritical, "Error...") DETENER
End Sub
Public vPUNTO As New Collection Public PuntoSel As Integer Public vPG1 As New Collection Public vPG2 As New Collection Public vPG3 As New Collection Public vPG4 As New Collection Public vPG5 As New Collection Public vPG6 As New Collection Public vPE As New Collection Public vVEL As New Collection Public Sub RECORD()
vPUNTO.Add (vPUNTO.Count + 1) Main!LPuntos.Caption = vPUNTO.Count vPG1.Add (Val(Main!LPosicionG1.Caption)) vPG2.Add (Val(Main!LPosicionG2.Caption)) vPG3.Add (Val(Main!LPosicionG3.Caption)) vPG4.Add (Val(Main!LPosicionG4.Caption)) vPG5.Add (Val(Main!LPosicionG5.Caption)) vPG6.Add (Val(Main!LPosicionG6.Caption)) vPE.Add (Val(Main!LPosicionE.Caption))
vVEL.Add (Val(Main!LVelocidad.Caption)) Main!List1.AddItem "Punto: " & vPUNTO.Count
Main!List1.AddItem " Grado1: " & vPG1.Item(vPG1.Count) Main!List1.AddItem " Grado2: " & vPG2.Item(vPG2.Count) Main!List1.AddItem " Grado3: " & vPG3.Item(vPG3.Count) Main!List1.AddItem " Grado4: " & vPG4.Item(vPG4.Count) Main!List1.AddItem " Grado5: " & vPG5.Item(vPG5.Count) Main!List1.AddItem " Grado6: " & vPG6.Item(vPG6.Count) Main!List1.AddItem " Efector: " & vPE.Item(vPE.Count) Main!List1.AddItem " Velocidad: " & vVEL.Item(vVEL.Count) Main!LP.Caption = vPUNTO.Count
Main!List1.ListIndex = (vPUNTO.Count * 9) - 1 End Sub
Public Sub DELETE()
PuntoSel = Val(Main!LPuntos.Caption) Select Case PuntoSel
Case 0: Exit Sub Case Else: vPUNTO.Remove (PuntoSel) Main!LPuntos.Caption = vPUNTO.Count vPG1.Remove (PuntoSel) vPG2.Remove (PuntoSel) vPG3.Remove (PuntoSel) vPG4.Remove (PuntoSel) vPG5.Remove (PuntoSel) vPG6.Remove (PuntoSel) vPE.Remove (PuntoSel) vVEL.Remove (PuntoSel) Main!List1.RemoveItem (PuntoSel + 7) Main!List1.RemoveItem (PuntoSel + 6) Main!List1.RemoveItem (PuntoSel + 5) Main!List1.RemoveItem (PuntoSel + 4) Main!List1.RemoveItem (PuntoSel + 3) Main!List1.RemoveItem (PuntoSel + 2) Main!List1.RemoveItem (PuntoSel + 1) Main!List1.RemoveItem (PuntoSel) Main!List1.RemoveItem (PuntoSel - 1) Main!LP.Caption = vPUNTO.Count End Select End Sub
Public Sub INSERT()
PuntoSel = Val(Main!LP.Caption) Select Case PuntoSel
Case 0: Exit Sub Case 1: Exit Sub Case Else: vPUNTO.Add (vPUNTO.Count + 1) Main!LPuntos.Caption = vPUNTO.Count vPG1.Add (Val(Main!LPosicionG1.Caption)) vPG2.Add (Val(Main!LPosicionG2.Caption)) vPG3.Add (Val(Main!LPosicionG3.Caption)) vPG4.Add (Val(Main!LPosicionG4.Caption)) vPG5.Add (Val(Main!LPosicionG5.Caption)) vPG6.Add (Val(Main!LPosicionG6.Caption)) vPE.Add (Val(Main!LPosicionE.Caption)) vVEL.Add (Val(Main!LVelocidad.Caption))
Main!List1.AddItem " Grado1: " & vPG1.Item(vPG1.Count), PuntoSel * 9 - 8 Main!List1.AddItem " Grado2: " & vPG2.Item(vPG2.Count), PuntoSel * 9 - 7 Main!List1.AddItem " Grado3: " & vPG3.Item(vPG3.Count), PuntoSel * 9 - 6 Main!List1.AddItem " Grado4: " & vPG4.Item(vPG4.Count), PuntoSel * 9 - 5 Main!List1.AddItem " Grado5: " & vPG5.Item(vPG5.Count), PuntoSel * 9 - 4 Main!List1.AddItem " Grado6: " & vPG6.Item(vPG6.Count), PuntoSel * 9 - 3 Main!List1.AddItem " Efector: " & vPE.Item(vPE.Count), PuntoSel * 9 - 2 Main!List1.AddItem " Velocidad: " & vVEL.Item(vVEL.Count), PuntoSel * 9 - 1 'Main!LP.Caption = vPUNTO.Count
'Main!List1.ListIndex = (vPUNTO.Count * 9) - 1 End Select
End Sub
Private Sub Abrir_Efector_Click(Index As Integer) Spe.Value = 1
End Sub
Private Sub Acerca_de_Click(Index As Integer) About.Show vbModal
End Sub
Private Sub Banterior_Click() Select Case Val(LP.Caption) > 1 Case True: LP.Caption = Val(LP.Caption) - 1 List1.ListIndex = (Val(LP.Caption) * 9) - 9 Case False: Exit Sub End Select End Sub
Private Sub Bdelete_Click() DELETE
End Sub
Private Sub Bexe_Click() EXECUTE
End Sub
Private Sub Binsert_Click() INSERT
End Sub
Private Sub Borrar_punto_Click(Index As Integer) DELETE
End Sub
Private Sub Brecord_Click() RECORD
End Sub
Private Sub Bsiguiente_Click()
Select Case Val(LP.Caption) < vPUNTO.Count Case True: LP.Caption = Val(LP.Caption) + 1 List1.ListIndex = (Val(LP.Caption) * 9) - 1 Case False: Exit Sub End Select End Sub
DETENER End Sub
Private Sub Cerrar_Click(Index As Integer) End
End Sub
Private Sub Cerrar_Efector_Click(Index As Integer) Spe.Value = 0
End Sub
Private Sub Configuracion_Click(Index As Integer) SetUp.Show vbModal
End Sub
Private Sub Decrementar_G1_Click(Index As Integer) On Error Resume Next
Spg1.Value = Spg1.Value - 1 End Sub
Private Sub Decrementar_G2_Click(Index As Integer) On Error Resume Next
Spg2.Value = Spg2.Value - 1 End Sub
Private Sub Decrementar_G3_Click() On Error Resume Next
Spg3.Value = Spg3.Value - 1 End Sub
Private Sub Decrementar_G4_Click(Index As Integer) On Error Resume Next
Spg4.Value = Spg4.Value - 1 End Sub
Private Sub Decrementar_G5_Click(Index As Integer) On Error Resume Next
Spg5.Value = Spg5.Value - 1 End Sub
Private Sub Decrementar_G6_Click(Index As Integer) On Error Resume Next
Spg6.Value = Spg6.Value - 1 End Sub
Private Sub Executar_Click(Index As Integer) EXECUTE
End Sub
Private Sub Form_Load()
LPosicionG1.Caption = Spg1.Value LPosicionG2.Caption = Spg2.Value LPosicionG3.Caption = Spg3.Value LPosicionG4.Caption = Spg4.Value LPosicionG5.Caption = Spg5.Value LPosicionG6.Caption = Spg6.Value
LPosicionE.Caption = Spe.Value '& " Cerrado" LVelocidad.Caption = SVel.Value '& " Normal" End Sub
Private Sub Frame2_MouseMove(Button As Integer, Shift As Integer, x As Single, y As Single) StatusBar1.Panels(1).Text = "ok"
End Sub
Private Sub Guardar_Punto_Click(Index As Integer) RECORD
End Sub
Private Sub Incrementar_G1_Click(Index As Integer) On Error Resume Next
Spg1.Value = Spg1.Value + 1 End Sub
Private Sub Incrementar_G2_Click(Index As Integer) On Error Resume Next
Spg2.Value = Spg2.Value + 1 End Sub
Private Sub Incrementar_G3_Click(Index As Integer) On Error Resume Next
Spg3.Value = Spg3.Value + 1 End Sub
Private Sub Incrementar_G4_Click(Index As Integer) On Error Resume Next
Spg4.Value = Spg4.Value + 1 End Sub
Private Sub Incrementar_G5_Click(Index As Integer) On Error Resume Next
Spg5.Value = Spg5.Value + 1 End Sub
Private Sub Incrementar_G6_Click(Index As Integer) On Error Resume Next
Spg6.Value = Spg6.Value + 1 End Sub
Private Sub Insetar_punto_Click(Index As Integer) INSERT
End Sub
Private Sub List1_Click() Dim Psel As Double Psel = List1.ListIndex / 9 Psel = ((Psel - 0.4) \ 1) + 1 LP.Caption = Psel End Sub
Private Sub LP_Change() Select Case Val(LP.Caption) Case 0: Bdelete.Enabled = False Bexe.Enabled = False Case Else: Bdelete.Enabled = True Bexe.Enabled = True End Select End Sub
Private Sub LPosicionE_Change() Select Case Spe.Value
Case 0:
Case 1:
ProgressBar7.Value = 100 End Select
End Sub
Private Sub LPosicionG1_Change() Dim Val As Double
Val = (Spg1.Value / Spg1.Min) * 100 ProgressBar1.Value = Val
End Sub
Private Sub LPosicionG1_Click() Dim POS As Integer
On Error Resume Next
POS = InputBox("Valor para el grado 1.", "Introducir datos...", LPosicionG1.Caption) Spg1.Value = Val(POS)
End Sub
Private Sub LPosicionG1_MouseMove(Button As Integer, Shift As Integer, x As Single, y As Single) StatusBar1.Panels(1).Text = "Click para introducr el dato manualmente..."
End Sub
Private Sub LPosicionG2_Change() Dim Val As Double
Val = (Spg2.Value / Spg2.Min) * 100 ProgressBar2.Value = Val
End Sub
Private Sub LPosicionG2_Click() Dim POS As Integer
On Error Resume Next
POS = InputBox("Valor para el grado 2.", "Introducir datos...", LPosicionG2.Caption) Spg2.Value = Val(POS)
End Sub
Private Sub LPosicionG2_MouseMove(Button As Integer, Shift As Integer, x As Single, y As Single) StatusBar1.Panels(1).Text = "Click para introducr el dato manualmente..."
End Sub
Private Sub LPosicionG3_Change() Dim Val As Double
Val = (Spg3.Value / Spg3.Min) * 100 ProgressBar3.Value = Val
End Sub
Private Sub LPosicionG3_Click() Dim POS As Integer
On Error Resume Next
POS = InputBox("Valor para el grado 3.", "Introducir datos...", LPosicionG3.Caption) Spg3.Value = Val(POS)
End Sub
Private Sub LPosicionG3_MouseMove(Button As Integer, Shift As Integer, x As Single, y As Single) StatusBar1.Panels(1).Text = "Click para introducr el dato manualmente..."
End Sub
Private Sub LPosicionG4_Change() Dim Val As Double
Val = (Spg4.Value / Spg4.Min) * 100 ProgressBar4.Value = Val
Private Sub LPosicionG4_Click() Dim POS As Integer
On Error Resume Next
POS = InputBox("Valor para el grado 4.", "Introducir datos...", LPosicionG4.Caption) Spg4.Value = Val(POS)
End Sub
Private Sub LPosicionG4_MouseMove(Button As Integer, Shift As Integer, x As Single, y As Single) StatusBar1.Panels(1).Text = "Click para introducr el dato manualmente..."
End Sub
Private Sub LPosicionG5_Change() Dim Val As Double
Val = (Spg5.Value / Spg5.Min) * 100 ProgressBar5.Value = Val
End Sub
Private Sub LPosicionG5_Click() Dim POS As Integer
On Error Resume Next
POS = InputBox("Valor para el grado 5.", "Introducir datos...", LPosicionG5.Caption) Spg5.Value = Val(POS)
End Sub
Private Sub LPosicionG5_MouseMove(Button As Integer, Shift As Integer, x As Single, y As Single) StatusBar1.Panels(1).Text = "Click para introducr el dato manualmente..."
End Sub
Private Sub LPosicionG6_Change() Dim Val As Double
Val = (Spg6.Value / Spg6.Min) * 100 ProgressBar6.Value = Val
End Sub
Private Sub LPosicionG6_Click() Dim POS As Integer
On Error Resume Next
POS = InputBox("Valor para el grado 6.", "Introducir datos...", LPosicionG6.Caption) Spg6.Value = Val(POS)
End Sub
Private Sub LPosicionG6_MouseMove(Button As Integer, Shift As Integer, x As Single, y As Single) StatusBar1.Panels(1).Text = "Click para introducr el dato manualmente..."
End Sub
Private Sub LPuntos_Change() On Error Resume Next Dim Val As Double Val = (LPuntos.Caption) ProgressBar9.Value = Val End Sub
Private Sub LVelocidad_Change() Select Case SVel.Value
Case 1:
LVelocidad.Caption = SVel.Value '& " Baja" mVelBaja.Item(2).Checked = True
mVelNorm.Item(2).Checked = False mVelAlta.Item(2).Checked = False Case 2:
LVelocidad.Caption = SVel.Value '& " Normal" mVelBaja.Item(2).Checked = False
mVelNorm.Item(2).Checked = True mVelAlta.Item(2).Checked = False Case 3:
LVelocidad.Caption = SVel.Value '& " Alta" mVelBaja.Item(2).Checked = False
mVelNorm.Item(2).Checked = False mVelAlta.Item(2).Checked = True End Select
Dim Val As Double
Val = (SVel.Value / SVel.Min) * 100 ProgressBar8.Value = Val
End Sub
Private Sub mVelAlta_Click(Index As Integer) SVel.Value = 3
End Sub
Private Sub mVelBaja_Click(Index As Integer) SVel.Value = 1
End Sub
Private Sub mVelNorm_Click(Index As Integer) SVel.Value = 2
End Sub
Private Sub Spe_Change() Select Case Spe.Value Case 0:
LPosicionE.Caption = Spe.Value '& " Cerrado" Abrir_Efector.Item(1).Checked = False Cerrar_Efector.Item(1).Checked = True Case 1:
LPosicionE.Caption = Spe.Value '& " Abierto" Abrir_Efector.Item(1).Checked = True Cerrar_Efector.Item(1).Checked = False End Select
End Sub
Private Sub Spg1_Change() LPosicionG1.Caption = Spg1.Value End Sub
Private Sub Spg2_Change() LPosicionG2.Caption = Spg2.Value End Sub
Private Sub Spg3_Change() LPosicionG3.Caption = Spg3.Value End Sub
Private Sub Spg4_Change() LPosicionG4.Caption = Spg4.Value End Sub
Private Sub Spg5_Change() LPosicionG5.Caption = Spg5.Value End Sub
Private Sub Spg6_Change() LPosicionG6.Caption = Spg6.Value End Sub