% Listing 5.1  Initialisierung der Bahnsteuerung

function  stat=robot_init(achswerte)
global q_akt rob_para ipo_takt
 
q_akt=		achswerte;
rob_para=	ROB6GL_DAT;
ipo_takt=	DEF_IPO;
stat=		99;




% Listing 5.2  Funktion robot_bew , Implementierung der Planung

function  [q_l s_l stat]=robot_bew(bs)

global q_akt rob_para ipo_takt
% Konstanten 
kleinsterWeg=0.0001 % [m]; kleinsterWinkel= pi/1000; % [rad]
PTP=1; CPL=2; CPC=3; % Verfahrarten
 
typ=bs.typ;
P1=bs.P1;
P2=bs.P2;
if typ==CPC
   P3=bs.P3; 
end  % Kreisinterpolation
kf1=bs.kf1;
kf2=bs.kf2;
prog_g=bs.geschw;
if prog_g<0 | prog_g>1     % zulässiger Wertebereich
    prog_g=1;
end
  
[n_achsen sp]=size(rob_para.dhp);
 
switch typ             % Planung Trajektorie
    case PTP %PTP
        q1= rtraf_6_gelenk(P1,kf1,rob_para);
        q2= rtraf_6_gelenk(P2,kf2,rob_para);
        sg=q2-q1;
    case CPL %CPLIN
        ul1=P1(1:3,4);
        ul2=P2(1:3,4);
        sg(1)=norm(ul2-ul1);
        [sg(2), sg(3)]=ori_plan(P1,P2);
    case CPC %CPCIR
        [sg(1) KR r]=kreis_plan(P1, P2, P3); 
        [sg(2), sg(3)]=ori_plan(P1,P2);
end
 
                            % Planung Geschwindigkeitsprofil
if typ==PTP %PTP  
    vm_prg=rob_para.vm*prog_g; % programmierte Geschwindigkeit
    am=rob_para.am;
    for dd=1:n_achsen 
        [tb(dd),tk(dd),tv(dd)]=geschw_lin_plan(sg(dd),
                               vm_prg(dd), am(dd));
    end
else        % CPL, CPC
    posv_prg=rob_para.posv*prog_g;
    posa=rob_para.posa;
    oriv_prg=rob_para.oriv*prog_g;
    oria=rob_para.oria;
    [tb(1),tk(1),tv(1)]=geschw_lin_plan(sg(1), 
                        posv_prg, posa);
    [tb(2),tk(2),tv(2)]=geschw_lin_plan(sg(2), 
                        oriv_prg, oria);
    [tb(3),tk(3),tv(3)]=geschw_lin_plan(sg(3), 
                        oriv_prg, oria);
end

tbm=max(tb); tkm=max(tk); tvm=max(tv); % max. Segmentzeiten
 
tba=aufrund(tbm, ipo_takt);       % Berücksichtigung IPO-Takt
tka=aufrund(tkm, ipo_takt); 
tva=aufrund(tvm, ipo_takt); 
 
sb=sg*tba/(2*(tba+tka)); sv=sb;     % Anpassung Segmentlängen 
sk=sg*tka/(tba+tka);               



% Listing 5.3  Funktion robot_bew , Implementierung der Interpolation

tg=tba+tka+tva;          %Interpolation Geschwindigkeitsprofil
ix=1;
for tint=ipo_takt:ipo_takt:tg     
    if tint <= tba              
        sint=sb*tint^2/tba^2;        % Beschleunigungssegment
    elseif tint >tba & tint <= tka+tba
        sint=sk*(tint-tba)/tka + sb; % Konstantsegment
    else
        sint=sv*(2*(tint-tba-tka)/tva-(tint-tba-tka)^2/tva^2)
             + sb + sk;              % Verzögerungssegment
    end                              
    s_l(ix,:)=sint(:);               % Liste Bahninkremente
    

    switch typ                 % Interpolation Trajektorie
    case PTP %PTP
        qint=q1+sint;
    case CPL %CPL
        if sg(1)>kleinsterWeg
            ul_int=ul1+(ul2-ul1)*sint(1)/sg(1); 
        else                        % interpolierter Bahnweg
            ul_int=ul1;
        end
        
        Ori1=P1; Ori1(1:3,4)=[0 0 0]'; 
        if sg(2)>kleinsterWinkel
            z1=P1(1:3,3); z2=P2(1:3,3); dvz=cross(z1,z2);
            POZ=rotv(dvz,sint(2))*Ori1; % Rotation um dvz
        else
            POZ=Ori1;
        end
        
        if sg(3)>kleinsterWinkel
            xint=POZ(1:3,1); x2=P2(1:3,1); dvx=cross(xint,x2);
            POX=rotv(dvx, sint(3))*POZ; % Rotation um dvx
        else
            POX=POZ;
        end
        
        P_int=POX;                % interpolierte Orientierung
        P_int(1:3,4)=ul_int;
        qint=rtraf_6_gelenk(P_int,kf1,rob_para);
          
    case CPC %CPC
       if sg(1)>kleinsterWeg
            PKR=KR*rotz(sint(1)/r);
            ukr_int_h=PKR*[r,0,0,1]'; % interpolierter Bahnweg
            ukr_int=ukr_int_h(1:3);
       else ukr_int=KR(1:3,4);
       end                
                      % Orientierungsinterpolation wie bei CPL
        Ori1=P1; Ori1(1:3,4)=[0 0 0]'; 
        if sg(2)>kleinsterWinkel
            z1=P1(1:3,3); z2=P2(1:3,3); dvz=cross(z1,z2);
            POZ=rotv(dvz,sint(2))*Ori1; % Rotation um dvz
        else
            POZ=Ori1;
        end
        
        if sg(3)>kleinsterWinkel
            xint=POZ(1:3,1); x2=P2(1:3,1); dvx=cross(xint,x2);
            POX=rotv(dvx, sint(3))*POZ; % Rotation um dvx
        else
            POX=POZ;
        end
        
        P_int=POX;                % interpolierte Orientierung
        P_int(1:3,4)=ukr_int;
        qint=rtraf_6_gelenk(P_int,kf1,rob_para);
                   
    end
    q_l(ix,:)=qint(:);
    ix=ix+1;
  
end % Schleife Interpolation
 
q_akt=qint;  % Roboter-Zustandsvariable
             % Aktuelle Roboterposition am Ende der Bahn
             
             
             
             
% Listing 5.4  Funktion  ori_plan  zur Planung der Orientierungsinterpolation

function  [s2 s3]=ori_plan(P1, P2)

kleinsterWinkel= pi/1000; % [rad]

z1=P1(1:3,3); z2=P2(1:3,3); dvz=cross(z1,z2);
s2=acos(dot(z1,z2));    % Drehwinkel um raumfesten Vektor dvz
 
PO1=P1; PO1(1:3,4)=[0 0 0]';
if s2> kleinsterWinkel;
    POZ=rotv(dvz,s2)*PO1;
else
    POZ=PO1;
end
x1_z=POZ(1:3,1); x2=P2(1:3,1);
s3=acos(dot(x1_z,x2));


% Listing 5.5  Planungsfunktion für die Kreisinterpolation

function  [s1 KR r]=kreis_plan(P1, P2, P3)

u1=P1(1:3,4); u2=P2(1:3,4); u3=P3(1:3,4);
zk=cross(u3-u1,u2-u1); 
zk=zk/norm(zk);         % Normalenvektor der Kreisebene

                        % Berechnung des Mittelpunkts
if norm(zk)<10*eps
    error('Kreispunkte liegen zu nahe bei einander');
    return
end
 
hv1=(u1+u2)/2; rv1=cross(u2-u1,zk);
hv2=(u1+u3)/2; rv2=cross(u3-u1,zk);
 
A=[rv1 -rv2]; k=hv2-hv1;
l=A\k;
mk=hv1+l(1)*rv1; % Kreismittelpunkt
r=norm (u1-mk);  % Radius
 
                 % Transformation von Punkt P2 nach KR
xk=u1-mk; xk=xk/norm(xk);
yk=cross(zk,xk);
KR=[xk yk zk mk; 0 0 0 1]; % Kreis-Koordinatensystem
P2_KR=inv(KR)*P2;
phi=atan2(P2_KR(2,4), P2_KR(1,4)); % y-,x-Koordinate in KR
     if phi<0 phi=2*pi+phi; end  % 0<phi<2*pi erforderlich
s1=phi*rad;      % Bogenlänge



% Listing 5.6  Berechnung des Geschwindigkeitsprofils

function  [tb tk tv]=geschw_lin_plan(s, vm, am)
% Eingangsparameter:
% s:            Weglänge
% vm:           maximale Geschwindigkeit
% am:           Beschleunigung,
% 
% Ausgangsparameter:
% tb:           Zeit Beschleunigung, 
% tk:           Zeit konstante Geschwindigkeit, 
% tv:           Zeit Verzögerung, 

s_abs=abs(s);   % Die Berechnung der Zeit bezieht sich auf 
                % den Betrag der Weg-/Winkeldifferenz
skrit=vm^2/am;  
if s_abs-skrit>eps      % Trapezprofil
    tb=vm/am; tv=tb; tk=s_abs/vm - vm/am;
else                % Dreieckprofil
    tb=sqrt(s_abs/am); tv=tb; tk=0;
end



% Listing 5.7  Skript der Testanwendung

ww_1=[0  -100 10 0 0 0]*pi/180;
ww_2=[90 -100 10 0 0 0]*pi/180;
robot_init(ww_1);
global rob_para
 
P1=vtraf(ww_1,rob_para);
P2=vtraf(ww_2,rob_para);


% Listing 5.8  Visualisierung der Bewegungsbahn

function  robot_bahn_vis(q_l, modus, geschw_l)
global rob_para
ks_linie=0.1;
[ns sp]=size(q_l);
if nargin ==3      % falls Geschwindigkeitsliste übergeben
    max_geschw=max(geschw_l);
    rel_geschw=geschw_l/max_geschw;
end
figure; hold on; view(48,20);

for dd=1:ns      % Schleife über alle Interpolationspunkte
    koor=koortraf(q_l(dd,:), rob_para);

    if modus==1
        if dd==1
            zeichne_kin_kette(koor, ks_linie);
            pp_v=koor{8}(1:3,4);
        end
        pp=koor{8}(1:3,4);
        plot3([pp_v(1),pp(1)],[pp_v(2),pp(2)],
             [pp_v(3),pp(3)],'r');
        pp_v=pp;
    end

    if modus==2
        if dd==1
            zeichne_kin_kette(koor, 0);
        end
        if nargin==3
            zeichne_ks(koor{8},ks_linie*rel_geschw(dd));
        else zeichne_ks(koor{8},ks_linie);
        end
    end

    if modus==3
        zeichne_kin_kette(koor, ks_linie);
        zeichne_ks(koor{8},ks_linie);   % Effektor
    end
end %Schleife
hold off;


 
% Listing 5.9  Numerische Auswertung des Bewegungsverlaufs und Darstellung als Diagramm

function  geschw_l =robot_bahn_ausw(bew_l, teil_bew_index)

global ipo_takt
figure;
teil_bew=bew_l(:,teil_bew_index);
subplot(3,1,1); plot(teil_bew); xlabel(’Teilbewegung’)
geschw_l=gradient(teil_bew, ipo_takt);
subplot(3,1,2); plot(geschw_l); xlabel(’Geschwindigkeit’)
beschl_l=gradient(geschw_l, ipo_takt); 
subplot(3,1,3); plot(beschl_l); xlabel(’Beschleunigung’)












