%% Funktion: Umrechnung der Steinkoordinaten in Ansicht-Koord. des Roboters 
% Praktikum Produktionstechnik
% WS 2021/2022
% Gruppe: Haris Adrovic, Maik Spinnrath, Franziska Troja
% Datum: 13.01.2022
% Letzte Änderung: 13.01.2022

function koord = umrechnung_koord(pos)
    % Drehmatrix von Noppen- in Ansichtkoordinatensystem
    drehmatrix = [0    -1     0;
                  1     0     0;
                  0     0     1];
              
    % Berechnung Koordinaten in Ansicht-Koordinatensystem
    vektor = [pos(1); pos(2); pos(3)];
    
    koord = drehmatrix * vektor;
    koord = transpose(koord);
end