-
Notifications
You must be signed in to change notification settings - Fork 0
/
trajektorie.m
47 lines (45 loc) · 1.13 KB
/
trajektorie.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
x1 = x(:, 1);
x2 = x(:, 2);
theta1 = x(:, 3);
theta2 = x(:, 4);
figure
subplot(3, 2, 1)
plot(t, x1)
xlabel('Czas [s]')
ylabel('Odleg這 [m]')
title(sprintf('Po這瞠nie (x(0) = %.2f m)', x0(1)))
hold on
plot(t, parametry.xT, 'r')
legend('Po這瞠nie [m]', 'Zadane po這瞠nie ko鎍owe [m]')
subplot(3, 2, 2)
plot(t, x2)
xlabel('Czas [s]')
ylabel('Pr璠ko [m/s]')
title(sprintf('Pr璠ko (x''(0) = %.2f m/s)', x0(2)))
subplot(3, 2, 3)
plot(t, (theta1*180/pi))
xlabel('Czas [s]')
ylabel(sprintf('Wychylenie [%c]', 176))
title(sprintf('Wychylenie (Theta(0) = %.2f%c)', x0(3)*180/pi, 176))
subplot(3, 2, 4)
plot(t, (theta2*180/pi))
xlabel('Czas [s]')
ylabel(sprintf('Pr璠ko [%c/s]', 176))
title(sprintf('Pr璠ko k靖owa (Theta''(0) = %.2f %c/s)', x0(4)*180/pi, 176))
subplot(3, 2, [5 6])
u = zeros(size(t));
dtau = diff([0; xmin; czas_symulacji]);
N = ceil(dtau./odstep_czasu);
cn = cumsum([1; N]);
ster = u0;
u(1) = u0;
for j = 1:length(dtau)
u(cn(j)+1:cn(j+1)) = ster;
ster = -ster;
end
plot(t, u, ':')
lim = abs(u0) * 1.5;
ylim([-lim lim])
xlabel('Czas [s]')
ylabel('Sterowanie')
title('Sterowanie u')