%% Funktion: Roboter bewegt sich mit Uebergabe von Translation- und Rotationswerten
% Positionskoordinaten muss in mm angegeben werden
% t ist Socket Connection Zugriff

function Pos_new = moverobot(t,Ziel_Pos,Orientation)
    % Ueberpruefung, wie viele Parameter uebergeben werden
    if nargin == 1
        error('error; nicht ausreichende Anzahl an Eingangsparametern')
    elseif nargin == 2
        P = Ziel_Pos;
    elseif nargin == 3
        P = [Ziel_Pos,Orientation];
    end
    
    % Umrechnung von mm in m
    P(1:3) = P(1:3) * 0.001; 
    % Speicherung in String 
    P_char = ['(',num2str(P(1)),',',...
        num2str(P(2)),',',...
        num2str(P(3)),',',...
        num2str(P(4)),',',...
        num2str(P(5)),',',...
        num2str(P(6)),...
        ')'];
    % temporaere Variable
    success = '0';
    while strcmp(success,'0')
        fprintf(t,'(1)'); % task = 1: moving task
        pause(0.01); % Pause, muss ggf. aber noch angepasst werden von der Laenge
        % Senden der neuen Position an Roboter 
        fprintf(t,P_char);
        while t.BytesAvailable == 0
            %t.BytesAvailable
        end
        success  = readrobotMsg(t);
    end
    % Ueberpruefung, ob Fehler beim Senden der Position an Roboter 
    if ~strcmp(success,'1')
        error('error sending robot pose')
    end
    pause(0.5);
    % neue Roboterposition 
    Pos_new = readrobotpose(t);
end