% Programm zum berechnen der Denavit&Hartenberg Methode
clear
clc

%*************** D&H-Tabelle definieren ***************%
%     alpha(i-1)   a(i-1)   d(i)   theta(i)            %
% 1                                                    %
% 2                                                    %
% 3                                                    %
% :                                                    %
% i                                                    %
% :                                                    %
% n                                                    %
%******************************************************%
Tabelle=[0,0,0,0;-pi()/2,0,2,0;pi()/2,0,3,0];

%********* Tabelle auswerten zu T verarbeiten *********%
G=size(Tabelle);              
g=G(1,1);               % Anzahl Zeilen der D&H-Tabelle  
T=zeros(1:g,1:4);
T(1:g,1:4)=Tabelle(1:g,1:4);
%****** (end) Tabelle auswerten zu T verarbeiten ******%

%********* Initialisieren von TFM, TF_TOT, O **********%
TFM=zeros(4,4*g);       % Transformationsmatrix pro Zeile 
TF_TOT=zeros(4,4*g);    % Gesamte Transformationsmatrix
O=zeros(4,g+1);
%****** (end) Initialisieren von TFM, TF_TOT, O *******%
    
%********** Transformationsmatrix generieren **********%
i=1;                    %Integrator                     
while(i<=g)
    % Transformationsmatrix von i-1 bis i
    TFM(1:4,(4*i-3):4*i)=DoMatrix(T,i);
    % Transformationsmatrizen von 0 bis i    
    if(i==1)
        TF_TOT(1:4,(4*i-3):4*i)=TFM(1:4,(4*i-3):4*i);
    else
        TF_TOT(1:4,(4*i-3):4*i)=TF_TOT(1:4,(4*(i-1)-3):4*(i-1))*TFM(1:4,(4*i-3):4*i);
    end
    i=i+1;
end  
Transformationsmatrix=TF_TOT(1:4,(4*g)-3:4*g)    
%******* (end) Transformationsmatrix generieren *******%
disp('done!')