% Skript für Rast in Rast Bewegung für nomiertes Bewegungsgesetz Poly. 5
clear

N=300;                          % Anzahl Stuetzstellen der Kurve
anz_a=4;                        % Anzahl Bewegungsabschnitte
t_B1=0.2;
t_R1=0.4;
t_B2=0.7;

t_s=[0, t_B1, t_R1, t_B2, 1]';     % Zeitpunkte in Bewegungszeit t_s
s_H=0.2;

% RANDbedingungen fuer die einzelnen Abschnitte s,v,a ****
rb = [ 0 , s_H, s_H, 0;         % Rast und Bewgungszeiten über Rb einstellbar
       0 , 0 , 0 , 0;       
       0 , 0 , 0 , 0];
%Abschnittsende=Anfang vom naechsten (Zeile 4...6)
for p = 1:anz_a-1               % p nimmt werte von 1-3 an
    rb(4:6,p)=rb(1:3,p+1);      % Zeile 4-6  - 1-3 Spalte =  Zeile 1-3 - 2-4 Spalte ?
end
rb(4:6,anz_a)=rb(1:3,1);        % 3x4 matrix erweitert auf 6x4

% abschnittsweise Berechnung der Polynomkoeffizienten
for q=1:anz_a   % 4 Spalten 
    A(:,q)=inv(cm( t_s(q) , t_s(q+1) ))*rb(:,q);    % A in extra fkt definieren ?
end
% ***********************************************************
% Vektor als stueckweises Polynom 5. Grades
% aequidistante Zeitschritte (N Intervalle)

t = linspace(t_s(1),t_s(end),N);

k=1;
for i=1:N
    if (t(i)<=t_s(k+1)) % t_s(1-300) <= t_s(2) - 0.2
            %dann liegt der Zeitpunkt in diesem Abschnitt, alles OK
    else    % dann ist es schon der naechste Abschnitt
        k=k+1;
    end
    s(i) =A(1+0,k) + A(1+1,k)*t(i) + A(1+2,k)*t(i)^2 + A(1+3,k)*t(i)^3 + ...
    + A(1+4,k)*t(i)^4 + A(1+5,k)*t(i)^5;
    sp(i) = A(1+1,k) + 2*A(1+2,k)*t(i) + 3*A(1+3,k)*t(i)^2 + ...
    + 4*A(1+4,k)*t(i)^3 + 5*A(1+5,k)*t(i)^4;
    spp(i) = 2*A(1+2,k) + 6*A(1+3,k)*t(i) + ...
    + 12*A(1+4,k)*t(i)^2 +20*A(1+5,k)*t(i)^3;
end

figure()
subplot(3,1,1)
plot(t,s);
title("Wegverlauf")
xlabel("t in s")
ylabel("s in m")
grid on
box on

subplot(3,1,2)
plot(t,sp);
title("Geschwindigkeitsverlauf")
xlabel("t in s")
ylabel("v in m/s")
grid on
box on

subplot(3,1,3)
plot(t,spp);
title("Beschleunigungsverlauf")
xlabel("t in s")
ylabel("a in m/s^2")
grid on
box on