% Delta2DOF_DAT.M        Script-File zur Simulation des 2DOF
% Delta   Zirn 26.1.10
% 
% Simulink- Modell:         "Delta2DOF_mod.mdl"
% Dynamik:                  "Delta2DOF_dyn.m"
clear all
global mp L
% Modellparameter
L=0.11; % m Beinlänge servoseitig
l=0.13;   % m Beinlänge platformseitig
LS=0.01;   % m Stiftservohornlänge
d1=0.023;  % m Versatz Gelenk 1 in x
d2=-0.023;  % m Versatz Gelenk 2 in x
mp=0.05; % kg Platformmasse
% Modellparameter Servomotor
JM=12e-7;    % kgm2 Motorträgheit
JL=5e-4;    % kgm2 Lastträgheit
LM=5e-3;     % H Induktivität
RM=1;        % Ohm Widerstand
KM=3.75e-3; % Nm/A Momentenkonstante
KS=(2.1e-2)/(2*pi);  % Vs/rad Spannungskonstante; 
Ttot=5e-4;  % s PWM-Totzeit
dphi=pi/5;    % rad Spiel motorseitig
kg=0.5;      % Nm/rad Getriebesteifigkeit
dg=2e-4;      % Nms/rad Getriebedämpfung
ue=200;     % Getriebeübersetzung
Umax=5;    % V Maximalspannung
MRM=1e-3;   % Nm Motorlagerreibung
MR=0.2;     % Nm Reibung Abtrieb
Viscoeff=1e-8;
% Reglereinstellung PID
Kv=30;          % V/m   P-Anteil
Tn=0.8;         % s Nachstellzeit
TD=0.06;        % s D-Anteil
% Maschinennullpunkt
X0=-0.00; Y0=0.125; Z0=0;
% Führungsgrößen (Doppelecke)
R=0.04; % m Eckenlänge
vbahn=0.0; % m/s Bahngeschwindigkeit
Tgrob=0.02; % 20 ms Grobinterpolationszeit
ZS=0.003;   % m Stifthub
Tsoll=[0 1 2 3 4 6]*abs(R)/vbahn;   % Zeitbasis in s
Xsoll=[0 R R 0 0 0]+X0;       % X-Positionen
Ysoll=[0 0 R R 0 0]+Y0;       % Y-Positionen
% T_CIRC=2*pi*R/vbahn;    % Kreisdauer
% for i=1:1:90
%     Tsoll(i)=i*T_CIRC/90;   % Zeitbasis in s
%     Xsoll(i)=-R+R*cos(2*pi*i/90)+X0;       % X-Positionen
%     Ysoll(i)=R*sin(2*pi*i/90)+Y0;       % Y-Positionen
% end
% Xsoll(i+1)=X0; Ysoll(i+1)=Y0; Tsoll(i+1)=T_CIRC+0.06;
Zsoll=Ysoll;%[0 ZS ZS 0]+Z0;       % Stift
% Interpolation mit Repeating Sequences in Simulink
simulation_opt=simset('Solver','ode5','FixedStep',Tgrob);
Tsim=max(Tsoll);   % Simulations time
[x,y,t]=sim('Interpolator_mod',Tsim,simulation_opt);
Tsolls=ts;
% Rücktransformation (...Baustelle)
for i=1:1:length(ts)
    % Hilfspunkt 1
    d=d1;
    if (Xsolls(i) == d)
        yh=(Ysolls(i)^2+L^2-l^2)/(2*Ysolls(i));
    else
        c=(l^2-L^2+d^2-Xsolls(i)^2-Ysolls(i)^2)/(2*(d-Xsolls(i)));
        m=Ysolls(i)/(d-Xsolls(i));
        if (Xsolls(i) < d)
        yh=(-2*m*(c-d)+sqrt((2*m*(c-d))^2-4*(1+m^2)*((c-d)^2-L^2)))/(2*(1+m^2));
        else
        yh=(-2*m*(c-d)-sqrt((2*m*(c-d))^2-4*(1+m^2)*((c-d)^2-L^2)))/(2*(1+m^2));
        end
    end
    phi1soll(i)=asin(yh/L);
    % Hilfspunkt 2
    d=d2;
    if (Xsolls(i) == d)
        yh=(Ysolls(i)^2+L^2-l^2)/(2*Ysolls(i));
    else
        c=(l^2-L^2+d^2-Xsolls(i)^2-Ysolls(i)^2)/(2*(d-Xsolls(i)));
        m=Ysolls(i)/(d-Xsolls(i));
        if (Xsolls(i) > d)
        yh=(-2*m*(c-d)+sqrt((2*m*(c-d))^2-4*(1+m^2)*((c-d)^2-L^2)))/(2*(1+m^2));
        else
        yh=(-2*m*(c-d)-sqrt((2*m*(c-d))^2-4*(1+m^2)*((c-d)^2-L^2)))/(2*(1+m^2));
        end
    end
    phi2soll(i)=pi-asin(yh/L);
% Z-Achse
phi3soll(i)=0;%asin(Zsolls(i)/LS);
end
% Kontrollplot
figure(1); hold off; plot(Xsolls,Ysolls,'b--')
figure(2); subplot(2,1,1); hold off; plot(ts,phi1soll,'k'); ylabel('phi1');
figure(2); subplot(2,1,2); hold off; plot(ts,phi2soll,'k'); ylabel('phi2');

% Simulationsparameter  
ein_opts=simset('Solver','ode5','FixedStep',1e-4);
% Simulationsaufruf
[x,y,t]=sim('Delta2DOF_mod',Tsim, ein_opts);

% Off-line-Vorwärtstrafo
for i=1:1:length(ts_grob)       
    y1h=L*sin(phi1s_grob(i)); x1h=d1+L*cos(phi1s_grob(i)); % Hilfspunkte
    y2h=L*sin(phi2s_grob(i)); x2h=d2+L*cos(phi2s_grob(i));
    m=(y1h-y2h)/(x2h-x1h); c=(x2h^2-x1h^2+y2h^2-y1h^2)/(2*(x2h-x1h));
    ys_grob(i)=(-2*(m*(c-x1h)-y1h)+sqrt((2*(m*(c-x1h)-y1h))^2-4*(1+m^2)*((c-x1h)^2+y1h^2-l^2)))/(2*(1+m^2));
    xs_grob(i)=m*ys_grob(i)+c;
end
figure(1); hold on; plot(xs_grob,ys_grob,'r')
xlabel('x in m'); ylabel('y in m'); title('Bahndarstellung -schwarz: Soll, - rot: Ist')
figure(2); 
figure(2); subplot(2,1,1); hold on; plot(ts,phi1s,'r'); 
figure(2); subplot(2,1,2); hold on; plot(ts,phi2s,'r'); 

% Animierte Darstellung 
for i=1:1:length(ts_grob)
    figure(7); hold off; plot([-0.1 0.1],[0 0],'k'); hold on; text(0.1,0,'X');
    plot([0 0],[0 0.2],'k'); text(0,0.2,'Y');
    plot([-0.08 0.08 0.08 -0.08 -0.08],[0.01 0.01 -0.01 -0.01 0.01],'k'); 
    plot(Xsolls,Ysolls,'k--','LineWidth',1);
    plot(xs_grob(1:i),ys_grob(1:i),'r','LineWidth',1);
    plot([d1 d1+L*cos(phi1s_grob(i))],[0 L*sin(phi1s_grob(i))],'k','LineWidth',2);
    plot([d2 d2+L*cos(phi2s_grob(i))],[0 L*sin(phi2s_grob(i))],'k','LineWidth',2);
    plot([d1+L*cos(phi1s_grob(i)) xs_grob(i)],[L*sin(phi1s_grob(i)) ys_grob(i)],'k','LineWidth',1);
    plot([d2+L*cos(phi2s_grob(i)) xs_grob(i)],[L*sin(phi2s_grob(i)) ys_grob(i)],'k','LineWidth',1);
    plot(xs_grob(i),ys_grob(i),'ko','LineWidth',3);
    plot(d1,0,'ko','LineWidth',2); plot(d2,0,'ko','LineWidth',2);
    plot(d1+L*cos(phi1s_grob(i)),L*sin(phi1s_grob(i)),'ko','LineWidth',2);
    plot(d2+L*cos(phi2s_grob(i)),L*sin(phi2s_grob(i)),'ko','LineWidth',2);
    title(['2DOF-Delta Animation mit v_b_a_h_n= ',num2str(vbahn),' m/s'])
    xlabel('X in m'); ylabel('Y in m'); axis([-0.15 0.15 -0.015 0.25]); 
    B(i) = getframe;   % Momentaufnahme für die movie-Funktion
end
figure(7);  movie(B,5,50)