1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65
|
function []= res(n) %fonction res, affiche les trajectoire, n = nombre points pour une période
for i=1:n
Phi1(i)= 2*pi*(i-1)/(n-1); %création du vecteur de l'angle moteur valeur: 0:2pi
end
syms x1 x2 x3 y1 y2 y3 phi2 phi3 phi1; %déclarations des variables sym pour le système d'équation
e1='x1-650';
e2='y1-100';
e3='x2-x1-100*cos(phi1)';
e4='y2-y1-100*sin(phi1)';
e5='x3-x2-700*sin(phi2)'; %système de 8 équations à 8 inconnus et un paramètre (phi1)
e6='y3-y2-700*cos(phi2)';
e7='x3-1000*cos(phi3)';
e8='y3-1000*sin(phi3)';
S = solve(e1,e2,e3,e4,e5,e6,e7,e8,'x1','y1','x2','y2','x3','y3','phi2','phi3'); %résolution du système en fonction de phi1
X1=zeros(n,1);
X2=zeros(n,1);
X3=zeros(n,1);
Y1=zeros(n,1);
Y2=zeros(n,1);
Y3=zeros(n,1); %déclaration des vecteurs solutions
PHI2=zeros(n,1);
PHI3=zeros(n,1);
x2c=zeros(n,1);
y2c=zeros(n,1);
x3d=zeros(n,1);
y3d=zeros(n,1);
for i=1:n
X1(i)=subs(S.x1(1), Phi1(i));
X2(i)=subs(S.x2(1), Phi1(i));
Y1(i)=subs(S.y1(1), Phi1(i));
Y2(i)=subs(S.y2(1), Phi1(i)); %boucle pour assigner au vecteurs solution les valeurs en fonction de
X3(i)=subs(S.x3(1), Phi1(i)); % chaque angle Phi1
Y3(i)=subs(S.y3(1), Phi1(i));
PHI2(i)=subs(S.phi2(1), Phi1(i));
PHI3(i)=subs(S.phi3(1), Phi1(i));
x2c(i)=X2(i)-600*cos(PHI2(i))-800*sin(PHI2(i));
y2c(i)=Y2(i)+800*cos(PHI2(i))-600*sin(PHI2(i));
x3d(i)=X3(i)-350*cos(PHI3(i))-500*sin(PHI3(i));
y3d(i)=Y3(i)+500*cos(PHI3(i))-350*sin(PHI3(i));
end
%R=[x2c y2c x3d y3d Phi1]
subplot(3,1,1) %affichage de graphiques
plot(x2c,y2c,'b')
subplot(3,1,2)
plot(x3d, y3d, 'r')
subplot(3,1,3)
plot(x2c,y2c, 'b', x3d, y3d, 'r')
%subplot(3,1,3) % course x2(phi) (degré)
%plot(Phi1*180/pi, x2c, 'g')
end |
Partager