% Listing 7.1  Anwendungsdaten für das Palettiern

function anwd=anw_palettieren_daten()
    anwd.geschw=0.4; % Bahngeschwindigkeit
    anwd.pal= [ 1.0   0    0    0.4;
                0	 -1.0	0     0;
                0     0   -1.0  0.4;
                0     0    0    1.0];
    ber=      [ 1.0   0    0    0.2;
                0    -1.0  0    0.2;
                0     0   -1.0  0.4;
                0     0    0    1.0];
    anwd.zn=2;   anwd.sn=2;
    anwd.zd=0.1; anwd.sd=0.1;

    phi=0;                  %Variation der Objektorientierung bei
    ber_trafo=rotz(phi);    % der Bereitstellung durch Drehung um
    anwd.ber=ber*ber_trafo; % die z-Achse von ber
    
    
    
% Listing 7.2  Steuerprogramm für Palettieren
     
% Initialisierung
global q_akt rob_para ipo_takt q_l_obj
 
w_anf=[0 -90 0 0 -90 0]; % Startwinkel
robot_init(w_anf);
anf_pos=vtraf(w_anf*pi/180, rob_para);
d1=0.03; % Verschiebung in Richtung der z-Achse von Welt
kf=2; % Armkonfiguration für alle Bahnen
 
% Einlesen Anwendungsdaten
anw_pal=anw_palettieren_daten;
ber_oben=anw_pal.ber;
ber_oben(3,4)= ber_oben(3,4)+d1;

% erste Anfahrt von BER
clear bahn;
bahn.P1=anf_pos;
bahn.P2=ber_oben;
bahn.kf1=kf; bahn.kf2=kf;
bahn.geschw=anw_pal.geschw; 
bahn.typ=1; % PTP
[q_l s_l stat]=robot_bew(bahn);
anz=robot_ausf(q_l);
zyklen_gesamt=anz;
 
for iz=0:anw_pal.zn-1       % Schleife über alle Zeilen
    for is=0:anw_pal.sn-1   % Schleife über alle Spalten

        % Absenken BER
        bahn.P1=ber_oben;
        bahn.P2=anw_pal.ber;
        bahn.typ=2; % LIN
        [q_l s_l stat]=robot_bew(bahn);
        anz=robot_ausf(q_l);
        zyklen_gesamt=zyklen_gesamt+anz;
 
        % Greifen Objekt und Anheben BER
        bahn.P1=anw_pal.ber;
        bahn.P2=ber_oben;
        bahn.typ=2; % LIN
        [q_l s_l stat]=robot_bew(bahn);
        anz=robot_ausf(q_l);
        zyklen_gesamt=zyklen_gesamt+anz;
        
        % Anfahrt  OBJ
        obj_akt=anw_pal.pal;
        obj_akt(1,4)=obj_akt(1,4)+anw_pal.sd*is;
        obj_akt(2,4)=obj_akt(2,4)+anw_pal.zd*iz;
        obj_akt_oben=obj_akt;
        obj_akt_oben(3,4)=obj_akt_oben(3,4)+d1;
        bahn.P1=ber_oben;
        bahn.P2=obj_akt_oben;
        bahn.typ=1; % PTP
        [q_l s_l stat]=robot_bew(bahn);
        anz=robot_ausf(q_l);
        zyklen_gesamt=zyklen_gesamt+anz;
		if (is==0 & iz==0)  % Achswerte beim ersten Anfahren
            q_l_obj=q_l;    % von OBJ
        end
        
        % Absenken OBJ und Ablegen Objekt
        bahn.P1=obj_akt_oben;
        bahn.P2=obj_akt;
        bahn.typ=2; % PTP
        [q_l s_l stat]=robot_bew(bahn);
        anz=robot_ausf(q_l);
        zyklen_gesamt=zyklen_gesamt+anz;
        
        % Anheben OBJ
        bahn.P1=obj_akt;
        bahn.P2=obj_akt_oben;
        bahn.typ=2; % PTP
        [q_l s_l stat]=robot_bew(bahn);
        anz=robot_ausf(q_l);
        zyklen_gesamt=zyklen_gesamt+anz;
        
        % Anfahrt BER
        bahn.P1=obj_akt_oben;
        bahn.P2=ber_oben;
        bahn.typ=1; % PTP
        [q_l s_l stat]=robot_bew(bahn);
        anz=robot_ausf(q_l);
        zyklen_gesamt=zyklen_gesamt+anz;
   end
end
 	
fprintf('Zykluszeit: %f  sec\n',zyklen_gesamt*ipo_takt);


% Listing 7.3  Anwendungsdaten für Bearbeiten Langloch

function anwd=anw_langloch_daten(versch)
    
llo=        [   1.0000      0.0000     0.0000     0.4000;
                0.0000      1.0000     0.0000     0.0000;
                0           0          1.0000     0.4000;
                0           0          0          1.0000];
anwd.br=0.1;
anwd.la=0.2;
anwd.geschw=0.4; % Bahngeschwindigkeit
     llo2=llo; 
     llo2(1:3,4)=llo2(1:3,4)+versch’; % Verschiebevektor
     anwd.llo=llo2;
     
     
     
% Listing 7.4 Steuerprogramm für Bearbeiten Langloch

function anw_langloch(vek)
% vek: Verschiebevektor für die Position des Werkstücks in m

     % Initialisierung
global q_akt rob_para ipo_takt

kf=2;
ori=[1 0 0 0; 0 -1 0 0; 0 0 -1 0; 0 0 0 1];
oberhalb=0.02; % z-Abstand zum Anfahren
w_anf=[0 -90 0 0 -90 0]; % Anfangswinkel des Roboters
robot_init(w_anf);
anf_pos=vtraf(w_anf*pi/180, rob_para);

anwd=anw_langloch_daten(vek);
    
% Berechne Punkte P1-P6, basierend auf dem lokalen 
% Koordinatensystem LLO
% Alle Bahnpunkte haben die gleiche Orientierung wie LLO.
geschw=anwd.geschw; la=anwd.la; br=anwd.br;
llo=anwd.llo;
P1=ori; P1(2,4)=br/2;
P1_oberhalb=P1; P1_oberhalb(3,4)= P1_oberhalb(3,4)+oberhalb;
P2=ori; P2(1,4)=br/2;
P3=ori; P3(1,4)=br; P3(2,4)=br/2;
P4=ori; P4(1,4)=br; P4(2,4)=la-br/2;
P5=ori; P5(1,4)=br/2; P5(2,4)=la;
P6=ori; P6(2,4)=la-br/2;

% Anfahrt P1-oberhalb

bahn.P1=anf_pos;
ROB_P1_oberhalb=llo*P1_oberhalb;
bahn.P2=ROB_P1_oberhalb;
bahn.typ=1; % PTP
[q_l s_l stat]=robot_bew(bahn);
anz=robot_ausf(q_l);
zyklen_gesamt=anz;

% Absenken
bahn.P1=llo*P1_oberhalb;
bahn.P2=llo*P1;
bahn.typ=2; % CPLIN
[q_l s_l stat]=robot_bew(bahn);
anz=robot_ausf(q_l);
zyklen_gesamt=zyklen_gesamt+anz;

% Beginn Bearbeitung, 1. Halbkreis
bahn.P1=llo*P1;;
bahn.P2=llo*P3;
bahn.P3=llo*P2;
bahn.typ=3; % CPCIR
[q_l s_l stat]=robot_bew(bahn);
anz=robot_ausf(q_l);
zyklen_gesamt=zyklen_gesamt+anz;
        
% 1. Linearbahn
bahn.P1=llo*P3;;
bahn.P2=llo*P4;
bahn.typ=2; % CPLIN
[q_l s_l stat]=robot_bew(bahn);
anz=robot_ausf(q_l);
zyklen_gesamt=zyklen_gesamt+anz;      
        
  
% 2. Halbkreis
bahn.P1=llo*P4;;
bahn.P2=llo*P6;
bahn.P3=llo*P5;
bahn.typ=3; % CPCIR
[q_l s_l stat]=robot_bew(bahn);
anz=robot_ausf(q_l);
zyklen_gesamt=zyklen_gesamt+anz;
        
% 2. Linearbahn
bahn.P1=llo*P6;;
bahn.P2=llo*P1;
bahn.typ=2; % CPL
[q_l s_l stat]=robot_bew(bahn);
anz=robot_ausf(q_l);
zyklen_gesamt=zyklen_gesamt+anz;      

% Ende der Bearbeitung, Abheben
clear bahn
bahn.P1=llo*P1;;
bahn.P2=llo*P1_oberhalb;
bahn.typ=2; % CPL
[q_l s_l stat]=robot_bew(bahn);
anz=robot_ausf(q_l);
zyklen_gesamt=zyklen_gesamt+anz;  

fprintf('Gesamtzeit: %f  sec\n\n',zyklen_gesamt*ipo_takt);








    
