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 66 67 68 69
|
for i=1:1:5 %size(t)
x_pos = etat(i,1)
y_pos = etat(i,2)
Omega = etat(i,3)
teta1 = etat(i,5)
phi1 = etat(i,6)
teta2 = etat(i,8)
phi2 = etat(i,9)
teta3 = etat(i,11)
phi3 = etat(i,12)
% % definition de la base
% points de la base
Cg = [x_pos,y_pos];
Rot = [cos(Omega),-sin(Omega);sin(Omega),cos(Omega)];
Cg = (Rot * Cg')';
A1 = [Cg(1)-a,Cg(2)-l/2];
A2 = [Cg(1)+a,Cg(2)-l/2];
A3 = [Cg(1),Cg(2)+b];
% definition des contours
line([A1(1) A3(1)],[A1(2) A3(2)],'Marker','.','LineStyle','-');
line([A1(1) A2(1)],[A1(2) A2(2)],'Marker','.','LineStyle','-');
line([A2(1) A3(1)],[A2(2) A3(2)],'Marker','.','LineStyle','-');
% definition des branches portant les roues
D1 = [A1(1)+d*cos(phi1) A1(2)+d*sin(phi1)];
D2 = [A2(1)+d*cos(phi2) A2(2)+d*sin(phi2)];
D3 = [A3(1)+d*cos(phi3) A3(2)+d*sin(phi3)];
line([A1(1) D1(1)],[A1(2) D1(2)],'Marker','.','LineStyle','-','Color','r')
line([A2(1) D2(1)],[A2(2) D2(2)],'Marker','.','LineStyle','-','Color','r')
line([A3(1) D3(1)],[A3(2) D3(2)],'Marker','.','LineStyle','-','Color','r')
% % definitions des roues
% def bord
R1 = [D1(1)-r,D1(1)+r,D1(2)+e/2,D1(2)-e/2]; % roue 1
R2 = [D2(1)-r,D2(1)+r,D2(2)+e/2,D2(2)-e/2]; % roue 2
R3 = [D3(1)-r,D3(1)+r,D3(2)+e/2,D3(2)-e/2]; % roue 2
% roue 1
line([R1(1) R1(2)],[R1(3) R1(3)],'Marker','.','LineStyle','-','Color','g')
line([R1(1) R1(2)],[R1(4) R1(4)],'Marker','.','LineStyle','-','Color','g')
line([R1(1) R1(1)],[R1(3) R1(4)],'Marker','.','LineStyle','-','Color','g')
line([R1(2) R1(2)],[R1(3) R1(4)],'Marker','.','LineStyle','-','Color','g')
% roue 2
line([R2(1) R2(2)],[R2(3) R2(3)],'Marker','.','LineStyle','-','Color','g')
line([R2(1) R2(2)],[R2(4) R2(4)],'Marker','.','LineStyle','-','Color','g')
line([R2(1) R2(1)],[R2(3) R2(4)],'Marker','.','LineStyle','-','Color','g')
line([R2(2) R2(2)],[R2(3) R2(4)],'Marker','.','LineStyle','-','Color','g')
% roue 3
line([R3(1) R3(2)],[R3(3) R3(3)],'Marker','.','LineStyle','-','Color','g')
line([R3(1) R3(2)],[R3(4) R3(4)],'Marker','.','LineStyle','-','Color','g')
line([R3(1) R3(1)],[R3(3) R3(4)],'Marker','.','LineStyle','-','Color','g')
line([R3(2) R3(2)],[R3(3) R3(4)],'Marker','.','LineStyle','-','Color','g')
drawnow;
pause(0.01)
end |
Partager