% SCHWENK_DAT.M        Script-File zur Simulation des 2DOF
% Schwenkarm-Portalroboters   Zirn 26.5.08
% 
% Simulink- Modell:         "Schwenk_mod.mdl"
% Dynamik:                  "Schwenk_dyn.m"
clear all

%%%%%%%%%%%%%
% Modellparameter       %
%%%%%%%%%%%%%
global m J mphi Jphi ma Ja h L
h=0.5;             % m Portalbalkenhöhe
L=0.68;                % m Schwenkarmlänge
% a-Achse
ma=30;          % kg Masse von Schlitten und Antrieb der a-Achse
Ja=0;               % kgm2 kein Trägheitsmoment zu berücksichtigen
Fmax=3500;  % N Maximalkraft in a
FR=100;          % N Reibung der a-Achse
% phi-Achse
mphi=1;             % kg Masse der phi-Achse
Jphi=0.02;        % kgm2  phi-Achsenträgheitsmoment
Mmax=350;   % Nm Maximalmoment in phi
MR=1;           % Nm Reibungsmoment
% TCP
m=5;          % kg TCP Lastmasse
J=0.001;        % kgm^2 Lastträgheitsmoment

%%%%%%%%%%%
% Bahngenerierung %
%%%%%%%%%%%
Tgrob=0.02;   % s Grobinterpolationszeit für die animierte Darstellung
vpath=1.5;    % m/s Bahngeschwindigkeit
Tset=[0 0.2/vpath 0.4/vpath 0.6/vpath 3/vpath];
Xset=[0 0 0.2 0.2 0.2];            % Sollwerte in X
Yset=[0 0.2 0.2 0 0];              % Sollwerte in Y
% Anfangsbedingungen für die Achsen
phiset0=asin((Yset(1)-h)/L);
aset0=Xset(1)-L*cos(phiset0);

%%%%%%%%%%%%
% Reglereinstellung  %
%%%%%%%%%%%%
Om=250;         % 1/s Polvorgabe zur gemeinsamen Reglereinstellung
Kv=Om/2;          % Lageregler;
Kpa=2*(ma+mphi+m)*Om;      % Geschwindigkeits- bzw.
Kpphi=2*(J*Jphi+m*L^2)*Om; % Drehzahlregler

%%%%%%%%
% Simulation  %
%%%%%%%%
% Simulationsparameter  
ein_opts=simset('Solver','ode5','FixedStep',1e-3);
Tsim=max(Tset);     % Simulationszeit
% Simulationsaufruf
[x,y,t]=sim('schwenk_mod',Tsim, ein_opts);

%%%%%%%%%%%%%%%%%%%%%
% Darstellung der Simulationsresultate  %
%%%%%%%%%%%%%%%%%%%%%
figure(1); hold off; plot(Xset,Yset,'k');
hold on; plot(xs,ys,'r');
xlabel('x in m'); ylabel('y in m'); title('Bahndarstellung -schwarz: Soll, - rot: Ist')
figure(2); subplot(2,1,1); hold off; plot(ts,asets,'k');
hold on; plot(ts,as,'r');
xlabel('t in s'); ylabel('a in m'); title('Zeitverlauf der a-Achse')
figure(2); subplot(2,1,2); hold off; plot(ts,phisets,'k');
hold on; plot(ts,phis,'r');
xlabel('t in s'); ylabel('phi in m'); title('Zeitverlauf der phi-Achse')
% Animierte Darstellung 
for i=1:1:length(ts_grob)
    figure(7); hold off; plot([-0.7 1],[0 0],'k'); hold on; text(1,0,'X');
    plot([0 0],[-0.1 1],'k'); text(0,1,'Y');
    plot([-0.8 0.8 0.8 -0.8 -0.8],[0.05 0.05 -0.05 -0.05 0.05]+h,'k'); 
    plot(Xset,Yset,'k--','LineWidth',1);
    plot(xs_grob(1:i),ys_grob(1:i),'r','LineWidth',1);
    plot([-0.1 0.1 0.1 -0.1 -0.1]+as_grob(i),[0.08 0.08 -0.08 -0.08 0.08]+h,'k','LineWidth',1);
    plot([as_grob(i) xs_grob(i)],[h ys_grob(i)],'k','LineWidth',2);
    plot([as_grob(i) xs_grob(i)],[h ys_grob(i)],'ko','LineWidth',2);
    plot(xs_grob(i),ys_grob(i),'kx','LineWidth',3);
    title(['Schwenk-Animation mit v_p_a_t_h= ',num2str(vpath),' m/s'])
    xlabel('X in m'); ylabel('Y in m'); axis([-0.9 1.1 -0.1 1.1]); 
    B(i) = getframe;   % Momentaufnahme für die movie-Funktion
end
figure(7);  movie(B,2,50)





