-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathplot_hover.m
131 lines (103 loc) · 2.96 KB
/
plot_hover.m
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
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
function [ ] = plot_hover(q, Vect_F, mode)
global a larg long larg_mot long_mot
persistent h1 h2 h3 h5 h6 h7 h8
hold on
%grid on
axis equal
% Dimension du robot
larg= 0.035; % largeur chassis
long= 0.15; % longueur chassis
a = 0.015;
larg_mot = 0.02; % largeur moteur
long_mot = 0.01; % longueur moteur
x_pos = q(2);
y_pos = q(4);
theta = q(6);
% Calcul de u, v, r:
Vect_v_abs = zeros(3,1);
Vect_v_abs(1) = q(1); % x_p = q(1)
Vect_v_abs(2) = q(3); % y_p = q(3)
Vect_v_abs(3) = q(5); % theta_p = q(5)
% On récupère les vecteurs u et v :
Vect_v_rob = McInv(Vect_v_abs,q(6));
u = Vect_v_rob(1);
v = Vect_v_rob(2);
% Plot des vitesses :
ROT = [ cos(theta),-sin(theta);
sin(theta),cos(theta)];
center = [ (long/2);
0 ];
center_abs = ROT*center + [x_pos; y_pos];
uu = [u, 0]'; % vecteur u si l'hover était en position de base
uuu = ROT*uu; % on le passe dans le repère de l'hover
vv = [0, v]';
vvv = ROT*vv;
if (mode==1) % si c'est la premièe fois on créer h5 et h6
h5 = quiver(x_pos, y_pos, uuu(1), uuu(2));
h6 = quiver(x_pos, y_pos, vvv(1), vvv(2));
else % sinon on les actualise
set(h5,'XData',center_abs(1),'YData', center_abs(2), 'Udata', uuu(1), 'Vdata', uuu(2));
set(h6,'XData',center_abs(1),'YData', center_abs(2), 'Udata', vvv(1), 'Vdata', vvv(2));
end
% Plot des forces :
pG = [ 0.001;
a+(larg_mot/2)-0.005];
pD = [ 0.001;
-a-(larg_mot/2)+0.005];
pG_abs = ROT*pG + [x_pos; y_pos];
pD_abs = ROT*pD + [x_pos; y_pos];
FFg = [-Vect_F(1), 0]';
FFFg = ROT*FFg;
FFd = [-Vect_F(2), 0]';
FFFd = ROT*FFd;
if (mode==1)
h7 = quiver(pG(1), pG(2), FFFg(1), FFFg(2));
h8 = quiver(pD(1), pD(2), FFFd(1), FFFd(2));
else
set(h7,'XData',pG_abs(1),'YData', pG_abs(2), 'Udata', FFFg(1), 'Vdata', FFFg(2));
set(h8,'XData',pD_abs(1),'YData', pD_abs(2), 'Udata', FFFd(1), 'Vdata', FFFd(2));
end
% Définition de l'axe :
%scale = 0.5;
%axis([x_pos-1*scale x_pos+1*scale y_pos-1*scale y_pos+1*scale])
%axis([0 8 -8 0])
% Titre et axes label :
xlabel('x(m)');
ylabel('y(m)');
title('Simulateur');
% Chassis
x = [0, (11/15)*long, long, (11/15)*long, 0, 0];
y = [larg, larg, 0, -larg, -larg, larg];
aux = ROT*[x;y] + [x_pos*ones(size(x));y_pos*ones(size(x))];
X = aux(1,:);
Y = aux(2,:);
if (mode==1)
h1=plot(X, Y,'b-');
set(h1,'EraseMode','xor','LineWidth',2);
else
set(h1,'XData',X,'YData',Y);
end
% Moteur droit
x = ([0,long_mot,long_mot,0,0]+0.001);
y = ([0,0,-long_mot,-long_mot,0]-a);
aux = ROT*[x;y] + [x_pos*ones(size(x));y_pos*ones(size(x))];
X = aux(1,:);
Y = aux(2,:);
if (mode==1)
h2=plot(X, Y,'k-');
set(h2,'EraseMode','xor','LineWidth',2);
else
set(h2,'XData',X,'YData',Y);
end
% Moteur gauche
x = ([0,long_mot,long_mot,0,0]+0.001);
y = ([0,0,long_mot,long_mot,0]+a);
aux = ROT*[x;y] + [x_pos*ones(size(x));y_pos*ones(size(x))];
X = aux(1,:);
Y = aux(2,:);
if (mode==1)
h3=plot(X, Y,'k-');
set(h3,'EraseMode','xor','LineWidth',2);
else
set(h3,'XData',X,'YData',Y);
end