%Berechnung der Antriebsbewegung aus Bewegung des TCP
%Vorgabe von geraden Bewegungen Angabe in "m"
%Anzahl Messwerte = Simulationszeit in ms

%clear;

N = 400;

%Geschwindigkeitsvorgabe m/s
Motion_v = 1 ;

%Bewegungsvorgabe Kreisbahn
%t = linspace(0, 2*pi,N);

%x-Richtungsvorgabe:
x_Start = 0.20;
x_Ziel = -0.20;
Motion_x = linspace(x_Start,x_Ziel,N)';
%Motion_x = 0.2*cos(t)';

%y-Richtungsvorgabe:
y_Start = 0;
y_Ziel = 0;
Motion_y = linspace(y_Start,y_Ziel,N)';
%Motion_y = 0.2*sin(t)';

%z-Richtungsvorgabe:
z_Start = -0.8;
z_Ziel = -0.8;
Motion_z = linspace(z_Start,z_Ziel,N)';

%Berechnung der Weglänge für die Geschwindigkeit
Motion_Weg = sqrt((z_Ziel - z_Start)^2 + (y_Ziel - y_Start)^2 + (x_Ziel - x_Start)^2 );
%Motion_Weg = 0.4 * pi;

%Berechnung der Bewegungszeit aus Bahnlänge und Geschwindigkeitsvorgabe
Motion_t_end = Motion_Weg / Motion_v;
Motion_t_delta = Motion_t_end / (N - 1);
Motion_t = linspace(0,Motion_t_end,N)';

Body = inverse_Kinematik(N, Motion_x, Motion_y, Motion_z);

%Aufbau der Bewegungsvorgaben aus den Messwerten 
%1 = Zeit, 2 = Weg (in ° umgerechnet), 3 = Geschwindigkeit, 4 = Beschleunigung

%Bestimmung der Kurbelbewegung
%Achse 1 = Kurbel generalisierten Koordinaten = 7
%Achse 2 = Kurbel generalisierten Koordinaten = 8
%Achse 3 = Kurbel generalisierten Koordinaten = 9

%Funktion Winkelstellung() berechnet mit Hilfe der
%Inversen Roboterkinematik die Winkel für die Stellmotoren

for i = 1 : 3;
    Bewegungsvorgabe_Achse(i).Data(:,1) = Motion_t;
    Bewegungsvorgabe_Achse(i).Data(:,5) = Winkelstellung(i) / (2*pi) * 360; 
    Bewegungsvorgabe_Achse(i).Data(:,6) = wrapTo360(Bewegungsvorgabe_Achse(i).Data(:,5));
    
    %Bestimmung der Kurbellaufrichtung 
    %Laufrichtung = 1 (Rechtslauf) 
    %Laufrichtung = 2 (Linkslauf)
    if (Bewegungsvorgabe_Achse(i).Data(2,6) - Bewegungsvorgabe_Achse(i).Data(1,6)) > 0
        Laufrichtung = 1;
    else
        Laufrichtung = 2;
    end    
        
    
    Korrekturwert = 0;
    
    for k = 2 : length(Bewegungsvorgabe_Achse(i).Data(:,6))
        
        if (Bewegungsvorgabe_Achse(i).Data(k-1,6) > 300) && (Bewegungsvorgabe_Achse(i).Data(k,6) < 100) && (Laufrichtung == 1)
            Korrekturwert = 360;            
        else if (Bewegungsvorgabe_Achse(i).Data(k-1,6) < 100) && (Bewegungsvorgabe_Achse(i).Data(k,6) > 300) && (Laufrichtung == 2)
            Korrekturwert = -360;
            end
        
        end
        %Auslgeich der Periodendurchläufe
        Bewegungsvorgabe_Achse(i).Data(k,6) = Bewegungsvorgabe_Achse(i).Data(k,6) + Korrekturwert;
    end
    %Ausgleich des Startwertes
    Nulllage = Bewegungsvorgabe_Achse(i).Data(1,6);
    Bewegungsvorgabe_Achse(i).Data(:,2) = Bewegungsvorgabe_Achse(i).Data(:,6) - Bewegungsvorgabe_Achse(i).Data(1,6);
    
    Bewegungsvorgabe_Achse(i).Geschwindigkeit = diff(Bewegungsvorgabe_Achse(i).Data(:,2)) ./ diff(Bewegungsvorgabe_Achse(i).Data(:,1));    %Berechnung der Geschwindigkeit
    Bewegungsvorgabe_Achse(i).Geschwindigkeit(end+1) = Bewegungsvorgabe_Achse(i).Geschwindigkeit(end);
    Bewegungsvorgabe_Achse(i).Data(:,3) = Bewegungsvorgabe_Achse(i).Geschwindigkeit;
    Bewegungsvorgabe_Achse(i).Beschleunigung = diff(Bewegungsvorgabe_Achse(i).Geschwindigkeit) ./ diff(Bewegungsvorgabe_Achse(i).Data(:,1));    %Berechnung der Beschleunigung    
    Bewegungsvorgabe_Achse(i).Beschleunigung(end) = Bewegungsvorgabe_Achse(i).Beschleunigung(end-1);
    Bewegungsvorgabe_Achse(i).Beschleunigung(end+1) = Bewegungsvorgabe_Achse(i).Beschleunigung(end);
    Bewegungsvorgabe_Achse(i).Data(:,4) = Bewegungsvorgabe_Achse(i).Beschleunigung; 
    
end

    
% % %     Nulllage = Bewegungsvorgabe_Achse(i).Data(1,2);
% % %         for k = 1 : length(Bewegungsvorgabe_Achse(i).Data(:,2))
% % %         if Bewegungsvorgabe_Achse(i).Data(k,2) > 0
% % %             Bewegungsvorgabe_Achse(i).Data(k,2) = Bewegungsvorgabe_Achse(i).Data(k,2) - Nulllage;
% % %         else
% % %             Bewegungsvorgabe_Achse(i).Data(k,2) = (360 + Bewegungsvorgabe_Achse(i).Data(k,2)) - Nulllage;
% % %         end
% % %     end