% NC_Programm_dat.m 
% Matlab-Script-Datei zur DIN-ISO-Code-ähnlichen Eingabe
% von NC-Sätzen mit den Funktionen
% - G01(Xi,Yi,vbahn,amax) zur Geradeninterpolation
% - G02/G03(Xi,Yi,XM,YM) zur Kreisinterpolation
% - G04(Wartezeit) zum zeitlich definierten Stillstand
% und anschliessender Grobinterpolationspunktgenerierung mit 
% der Funktion "Grobinterpolator.m" sowie der Darstellung von 
% Bahn und Zeitverlauf von Sollpositionen, -geschwindigkeiten und 
% -beschleunigungen.                                   Zirn 26.9.04
%clear all
% Maschinen- und Prozessparameter
vbahn=0.1;  % m/s Bahngeschwindigkeit
Tgrob=2e-3; % s Grobinterpolationszeit
amax=500;    % m/s2 Maximalbeschleunigung
% NC-Programm als Matrix 
G_CODE=[1 0 0 vbahn amax];
G_CODE=[G_CODE; 4 0.1 0 0 0];
G_CODE=[G_CODE; 1 0.1 0 vbahn amax];
G_CODE=[G_CODE; 1 0.1 0.1 vbahn amax];
G_CODE=[G_CODE; 1 0.2 0.1 vbahn amax];
G_CODE=[G_CODE; 4 0.5 0 0 0];
% G_CODE=[G_CODE; 4 0.1 0 0 0];
% G_CODE=[G_CODE; 1 -0.01 0.01 vbahn amax];
% G_CODE=[G_CODE; 1 0 0.01 vbahn amax];
% G_CODE=[G_CODE; 2 0 -0.01 0 0];
% G_CODE=[G_CODE; 2 0 0.01 0 0];
% G_CODE=[G_CODE; 2 0 -0.01 0 0];
% G_CODE=[G_CODE; 1 -0.01 -0.01 vbahn amax];
% G_CODE=[G_CODE; 4 0.1 0 0 0];
% G_CODE=[G_CODE; 1 -0.005 -0.01 vbahn amax];
% G_CODE=[G_CODE; 3 0 -0.005 -0.005 -0.005];
% G_CODE=[G_CODE; 1 0 0 vbahn amax];
% G_CODE=[G_CODE; 4 0.1 0 0 0];
% Grobinterpolationspunkte generieren
[Tsoll,Xsoll,Ysoll,vXsoll,vYsoll,aXsoll,aYsoll]=Grobinterpolator(G_CODE,Tgrob);
% % Kinematische Größen darstellen
% figure(1); hold off; plot(Xsoll,Ysoll,'k');hold on; plot(Xsoll,Ysoll,'ko');
% xlabel('x in m'); ylabel('y in m'); 
% title('Grobinterpolationspunkte mit vbahn=0.1 m/s und Tgrob=2ms');
% figure(2); hold off; plot(Tsoll,Xsoll,'b'); hold on; plot(Tsoll,Ysoll,'r');
% xlabel('t in s'); ylabel('Postion in m   x-blau, y-rot'); 
% figure(3); hold off; plot(Tsoll,vXsoll,'b'); hold on; plot(Tsoll,vYsoll,'r');
% xlabel('t in s'); ylabel('Geschwindigkeit in m/s   x-blau, y-rot'); 
% figure(4); hold off; plot(Tsoll,aXsoll,'b'); hold on; plot(Tsoll,aYsoll,'r');
% xlabel('t in s'); ylabel('Beschleunigung in m/s2   x-blau, y-rot'); 
% Filterung mit IIR-Filter erster Ordnung zur Ruckbegrenzung
rmax=20000;  % m/s3 Maximalruck
TF=amax/rmax;       % Filterzeitkonstante
% Filtervariable für identischen Amplitudengang bei der Grenzfrequenz 1/TF
mf=cot(Tgrob/(2*TF));
% Filterung nur, wenn die ruckbedingte Filterzeitkonstante größer als die 
% Grobinterpolationszeit ist; ansonsten Gefahr durch Aliasing;
if (TF >= Tgrob)
Xsollf(1)=Xsoll(1); Ysollf(1)=Ysoll(1); 
vXsollf(1)=vXsoll(1); vYsollf(1)=vYsoll(1); 
aXsollf(1)=aXsoll(1); aYsollf(1)=aYsoll(1); 
for i=2:1:length(Tsoll)
    Xsollf(i)=(Xsoll(i)+Xsoll(i-1)+(mf-1)*Xsollf(i-1))/(1+mf);
    Ysollf(i)=(Ysoll(i)+Ysoll(i-1)+(mf-1)*Ysollf(i-1))/(1+mf);
    vXsollf(i)=(vXsoll(i)+vXsoll(i-1)+(mf-1)*vXsollf(i-1))/(1+mf);
    vYsollf(i)=(vYsoll(i)+vYsoll(i-1)+(mf-1)*vYsollf(i-1))/(1+mf);
    aXsollf(i)=(aXsoll(i)+aXsoll(i-1)+(mf-1)*aXsollf(i-1))/(1+mf);
    aYsollf(i)=(aYsoll(i)+aYsoll(i-1)+(mf-1)*aYsollf(i-1))/(1+mf);
end % of for
% Ruckermittlung durch diskretes Differenzieren
rXsollf(i)=0; rYsollf(i)=0;
for i=2:1:length(Tsoll)
    rXsollf(i)=(aXsollf(i)-aXsollf(i-1))/Tgrob;
    rYsollf(i)=(aYsollf(i)-aYsollf(i-1))/Tgrob;
end % of for
else % ungefilterte Werte übertragen
    for i=2:1:length(Tsoll)
        Xsollf(i)=Xsoll(i);Ysollf(i)=Ysoll(i);
        vXsollf(i)=vXsoll(i); vYsollf(i)=vYsoll(i);
        aXsollf(i)=aXsoll(i); aYsollf(i)=aYsoll(i);
        rXsollf(i)=0; rYsollf(i)=0;
    end % of for
end % of if
% % Kinematische Größen darstellen
% figure(1); hold on; plot(Xsollf,Ysollf,'k--');
% figure(2); hold on; plot(Tsoll,Xsollf,'k--'); hold on; plot(Tsoll,Ysollf,'r--');
% figure(3); hold on; plot(Tsoll,vXsollf,'b--'); hold on; plot(Tsoll,vYsollf,'r--');
% figure(4); hold on; plot(Tsoll,aXsollf,'b--'); hold on; plot(Tsoll,aYsollf,'r--');
% figure(5); hold off; plot(Tsoll,rXsollf,'b--'); hold on; plot(Tsoll,rYsollf,'r--');
% xlabel('t in s'); ylabel('Ruck in m/s3   x-blau, y-rot'); 

