tic
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%% DEFINICION DE VARIABLES SIMBOLICAS %%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Coordenadas y sus derivadas
Rx3=sym('Rx3','real');
Ry3=sym('Ry3','real');
Rz3=sym('Rz3','real');
phi=sym('phi','real');
psi=sym('psi','real');
tet=sym('tet','real');
gam=sym('gam','real');
eps=sym('eps','real');
nu=sym('nu','real');
bet=sym('bet','real');
dRx3=sym('dRx3','real');
dRy3=sym('dRy3','real');
dRz3=sym('dRz3','real');
dphi=sym('dphi','real');
dpsi=sym('dpsi','real');
dtet=sym('dtet','real');
dgam=sym('dgam','real');
deps=sym('deps','real');
dnu=sym('dnu','real');
% VECTOR DE COORDENADAS Y DE "VELOCIDADES" %
p=[Rx3;Ry3;Rz3;phi;psi;tet;gam;eps;nu];
dp=[dRx3;dRy3;dRz3;dphi;dpsi;dtet;dgam;deps;dnu];
%Tiempo
t=sym('t','real');
%Radio rueda trasera
Rt=sym('Rt','real');
%Radio rueda delantera
Rd=sym('Rd','real');
%Posicion de centros de gravedad
x3=sym('x3','real');
z3=sym('z3','real');
x4=sym('x4','real');
z4=sym('z4','real');
x5=sym('x5','real');
z5=sym('z5','real');
%Vectores extra
r2x=sym('r2x','real');
r2z=sym('r2z','real');
r4x=sym('r4x','real');
r4z=sym('r4z','real');
r5x=sym('r5x','real');
r5z=sym('r5z','real');
%Vectores contacto en respectivos locales
rp2x=sym('rp2x','real');
rp2y=sym('rp2y','real');
rp2z=sym('rp2z','real');
rp5x=sym('rp5x','real');
rp5y=sym('rp5y','real');
rp5z=sym('rp5z','real');
%Parametros de masa e inercia
m2=sym('m2','real');
m3=sym('m3','real');
m4=sym('m4','real');
m5=sym('m5','real');
I2x=sym('I2x','real');
I2y=sym('I2y','real');
I3x=sym('I3x','real');
I3y=sym('I3y','real');
I3z=sym('I3z','real');
I3zx=sym('I3zx','real');
I4x=sym('I4x','real');
I4y=sym('I4y','real');
I4z=sym('I4z','real');
I4xy=sym('I4xy','real');
I4xz=sym('I4xz','real');
I4yz=sym('I4yz','real');
I5x=sym('I5x','real');
I5y=sym('I5y','real');
%Tensores de inercia
I2=[[I2x; 0; 0] [0; I2y; 0] [0; 0; I2x]];
I3=[[I3x; 0; I3zx] [0; I3y; 0] [I3zx; 0; I3z]];
I4=[[I4x;0; I4xz] [0; I4y; 0] [I4xz; 0; I4z]];
I5=[[I5x; 0; 0] [0; I5y; 0] [0; 0; I5x]];
%Aceleracion de la gravedad
g=sym('g','real');
%%%%%%%%%%%%%%%%%%%%%%%
%%%% CINEMATICA %%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%
%MATRICES DE ROTACION SIMPLE
A_phi=[[cos(phi);sin(phi);0] [-sin(phi);cos(phi);0] [0;0;1]];
A_psi=[[1;0;0] [0; cos(psi); sin(psi)] [0 ; -sin(psi); cos(psi)]];
A_tet=[[cos(tet);0;-sin(tet)] [0;1;0] [sin(tet);0;cos(tet)]];
A_nu=[[cos(nu);0;-sin(nu)] [0;1;0] [sin(nu);0;cos(nu)]];
A_bet=[[cos(bet);0;-sin(bet)] [0;1;0] [sin(bet);0;cos(bet)]];
A_gam=[[cos(gam);sin(gam);0] [-sin(gam);cos(gam);0] [0;0;1]];
A_eps=[[cos(eps);0;-sin(eps)] [0;1;0] [sin(eps);0;cos(eps)]];
%MATRICES DE ROTACION DE LOS SISTEMAS DE REFERENCIA DE LOS SOLIDOS E INTERMEDIOS
Ai1=(A_phi);
Ai2=(A_phi*A_psi);
A3=(Ai2*A_tet);
A2=A3*A_nu;
A4=(A3*A_bet*A_gam);
A5=(A4*A_eps);
matlabFunction(A2,'file', 'A2');
matlabFunction(A3,'file', 'A3');
matlabFunction(A4,'file', 'A4');
matlabFunction(A5,'file', 'A5');
%VELOCIDADES ANGULARES EN LOCALES, es respecto al t
dA2 = (diff(A2,phi)*dphi +diff(A2,tet)*dtet+ diff(A2,psi)*dpsi+ diff(A2,nu)*dnu+ diff(A2,gam)*dgam+ diff(A2,eps)*deps);
dA3 = (diff(A3,phi)*dphi +diff(A3,tet)*dtet+ diff(A3,psi)*dpsi+ diff(A3,nu)*dnu+ diff(A3,gam)*dgam+ diff(A3,eps)*deps);
dA4 = (diff(A4,phi)*dphi +diff(A4,tet)*dtet+ diff(A4,psi)*dpsi+ diff(A4,nu)*dnu+ diff(A4,gam)*dgam+ diff(A4,eps)*deps);
dA5 = (diff(A5,phi)*dphi +diff(A5,tet)*dtet+ diff(A5,psi)*dpsi+ diff(A5,nu)*dnu+ diff(A5,gam)*dgam+ diff(A5,eps)*deps);
w2Lsk = (A2'*dA2);
w3Lsk = (A3'*dA3);
w4Lsk = (A4'*dA4);
w5Lsk = (A5'*dA5);
w2sk = (dA2*A2');
w3sk = (dA3*A3');
w4sk = (dA4*A4');
w5sk = (dA5*A5');
w2L = [-w2Lsk(2,3) w2Lsk(1,3) -w2Lsk(1,2)]';
w3L = [-w3Lsk(2,3) w3Lsk(1,3) -w3Lsk(1,2)]';
w4L = [-w4Lsk(2,3) w4Lsk(1,3) -w4Lsk(1,2)]';
w5L = [-w5Lsk(2,3) w5Lsk(1,3) -w5Lsk(1,2)]';
w2G = [-w2sk(2,3) w2sk(1,3) -w2sk(1,2)]';
w3G = [-w3sk(2,3) w3sk(1,3) -w3sk(1,2)]';
w4G = [-w4sk(2,3) w4sk(1,3) -w4sk(1,2)]';
w5G = [-w5sk(2,3) w5sk(1,3) -w5sk(1,2)]';
matlabFunction(w2L,'file', 'w2L');
matlabFunction(w5L,'file', 'w5L');
matlabFunction(w2G, 'file', 'w2G');
matlabFunction(w5G, 'file', 'w5G');
%POSICIONES DE LOS CENTROS DE GRAVEDAD EN LOCALES
rG2=[0 0 0]'; %centro rueda trasera
rG3=[x3 0 z3]';
rG4=[x4 0 z4]';
rG5=[0 0 0]'; %centro rueda delantera
%VECTORES DE 3 A CADA UNO DE LOS STMAS REF
r2=[r2x 0 r2z]'; %en 3 de 3 a 2
r4=[r4x 0 r4z]'; %en 3 de 3 a 4
r5=[r5x 0 r5z]'; %en 4 de 4 a 5
%POSICIONES DE LOS CENTROS DE GRAVEDAD EN GLOBALES
RG3=([Rx3 Ry3 Rz3]'+A3*rG3);
RG2=([Rx3 Ry3 Rz3]'+A3*r2+A2*rG2);
RG4=([Rx3 Ry3 Rz3]'+A3*r4+A4*rG4);
RG5=([Rx3 Ry3 Rz3]'+A3*r4+A4*r5+A5*rG5);
matlabFunction(RG2,'file', 'RG2');
matlabFunction(RG3,'file', 'RG3');
matlabFunction(RG4,'file', 'RG4');
matlabFunction(RG5,'file', 'RG5');
%VECTORES A PUNTOS CONTACTO
rp2=[rp2x rp2y rp2z]';
rp5=[rp5x rp5y rp5z]';
RP2=RG2+A2*rp2;
RP5=RG5+A5*rp5;
% MATRICES DE CÁLCULO DE RELACIONES CINEMÁTICAS ENTRE VELOCIDADES Y
% ACELERACIONES DE LOS SÓLIDOS Y COORDENADAS GENERALIZADAS
%Vp=Hp*dq
H2 = (jacobian(RG2,p)); h2 = (jacobian(H2*dp,p));
H3 = (jacobian(RG3,p)); h3 = (jacobian(H3*dp,p));
H4 = (jacobian(RG4,p)); h4 = (jacobian(H4*dp,p));
H5 = (jacobian(RG5,p)); h5 = (jacobian(H5*dp,p));
G2 = (jacobian(w2L,dp)); g2 = (jacobian(w2L,p));
G3 = (jacobian(w3L,dp)); g3 = (jacobian(w3L,p));
G4 = (jacobian(w4L,dp)); g4 = (jacobian(w4L,p));
G5 = (jacobian(w5L,dp)); g5 = (jacobian(w5L,p));
G2g = (jacobian(w2G,dp));
G3g = (jacobian(w3G,dp));
G4g = (jacobian(w4G,dp));
G5g = (jacobian(w5G,dp));
H2p=jacobian(RP2,p);
H5p=jacobian(RP5,p);
matlabFunction(H2,'file', 'H2');
matlabFunction(H3,'file', 'H3');
matlabFunction(H4,'file', 'H4');
matlabFunction(H5,'file', 'H5');
matlabFunction(G2,'file', 'G2');
matlabFunction(G3,'file', 'G3');
matlabFunction(G4,'file', 'G4');
matlabFunction(G5,'file', 'G5');
matlabFunction(G2g,'file', 'G2g');
matlabFunction(G3g,'file', 'G3g');
matlabFunction(G4g,'file', 'G4g');
matlabFunction(G5g,'file', 'G5g');
matlabFunction(H2p,'file', 'H2p');
matlabFunction(H5p,'file', 'H5p');
if(1)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% ECUACIONES DE NEWTON-EULER %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
MM(3*(1-1)+1:3*(1-1)+3, 3*(1-1)+1:3*(1-1)+3) = m2*eye(3);
MM(3*(2-1)+1:3*(2-1)+3, 3*(2-1)+1:3*(2-1)+3) = m3*eye(3);
MM(3*(3-1)+1:3*(3-1)+3, 3*(3-1)+1:3*(3-1)+3) = m4*eye(3);
MM(3*(4-1)+1:3*(4-1)+3, 3*(4-1)+1:3*(4-1)+3) = m5*eye(3);
MM(3*4+3*(1-1)+1:3*4+3*(1-1)+3, 3*4+3*(1-1)+1:3*4+3*(1-1)+3) = I2;
MM(3*4+3*(2-1)+1:3*4+3*(2-1)+3, 3*4+3*(2-1)+1:3*4+3*(2-1)+3) = I3;
MM(3*4+3*(3-1)+1:3*4+3*(3-1)+3, 3*4+3*(3-1)+1:3*4+3*(3-1)+3) = I4;
MM(3*4+3*(4-1)+1:3*4+3*(4-1)+3, 3*4+3*(4-1)+1:3*4+3*(4-1)+3) = I5;
% QQv(3*4+3*(1-1)+1:3*4+3*(1-1)+3,1) = -cross(w2L, I2*w2L);
% QQv(3*4+3*(2-1)+1:3*4+3*(2-1)+3,1) = -cross(w3L, I3*w3L);
% QQv(3*4+3*(3-1)+1:3*4+3*(3-1)+3,1) = -cross(w4L, I4*w4L);
% QQv(3*4+3*(4-1)+1:3*4+3*(4-1)+3,1) = -cross(w5L, I5*w5L);
a = w2L; b = I2*w2L; QQv(3*4+3*(1-1)+1,1) = -(a(2)*b(3)-a(3)*b(2)); QQv(3*4+3*(1-1)+2,1) = -(a(3)*b(1)-a(1)*b(3)); QQv(3*4+3*(1-1)+3,1) = -(a(1)*b(2)-a(2)*b(1));
a = w3L; b = I3*w3L; QQv(3*4+3*(2-1)+1,1) = -(a(2)*b(3)-a(3)*b(2)); QQv(3*4+3*(2-1)+2,1) = -(a(3)*b(1)-a(1)*b(3)); QQv(3*4+3*(2-1)+3,1) = -(a(1)*b(2)-a(2)*b(1));
a = w4L; b = I4*w4L; QQv(3*4+3*(3-1)+1,1) = -(a(2)*b(3)-a(3)*b(2)); QQv(3*4+3*(3-1)+2,1) = -(a(3)*b(1)-a(1)*b(3)); QQv(3*4+3*(3-1)+3,1) = -(a(1)*b(2)-a(2)*b(1));
a = w5L; b = I5*w5L; QQv(3*4+3*(4-1)+1,1) = -(a(2)*b(3)-a(3)*b(2)); QQv(3*4+3*(4-1)+2,1) = -(a(3)*b(1)-a(1)*b(3)); QQv(3*4+3*(4-1)+3,1) = -(a(1)*b(2)-a(2)*b(1));
QQgrav(3*(1-1)+1:3*(1-1)+3,1) = [0 0 -m2*g]'; %ok x5
QQgrav(3*(2-1)+1:3*(2-1)+3,1) = [0 0 -m3*g]';
QQgrav(3*(3-1)+1:3*(3-1)+3,1) = [0 0 -m4*g]';
QQgrav(3*(4-1)+1:3*(4-1)+3,1) = [0 0 -m5*g]';
QQgrav(2*3*4) = 0;
%%%%% MATRICES DE TRANSFORMACIÓN CINEMÁTICA %%%%%
L = [H2' H3' H4' H5' G2' G3' G4' G5']';
l = [h2' h3' h4' h5' g2' g3' g4' g5']';
%al L'* se estan haciendo simetricas
M = L'*MM*L;
Qv = L'*(QQv-MM*l*dp);
Qgrav = L'*QQgrav;
%%%%%%%%%%%%%%%%%%%%%%%
% FUERZA AERODINÁMICA %
%%%%%%%%%%%%%%%%%%%%%%%
syms cv real;
VG3 = H3*dp;
Qaero = (-cv*(VG3'*VG3)^0.5)*H3'*VG3; % la aplica prop a vcuadro^2 -cv*|vg3| es el modulo fza que va en direc vg3, por eso H3' * Fza
matlabFunction(M,'file', 'MatrizMasaMoto');
matlabFunction(Qv,'file', 'FuerzaQvMoto');
matlabFunction(Qgrav,'file', 'FuerzaQgravMoto');
matlabFunction(Qaero,'file', 'FuerzaQaeroMoto');
toc
end
e-REdING. Biblioteca de la Escuela Superior de Ingenieros de Sevilla.
