%% Hauptprogramm zur Ansteuerung des UR3-Roboters mit Server-Client-Verbindung 
% Praktikum Produktionstechnik
% WS 2021/2022
% Gruppe: Haris Adrovic, Maik Spinnrath, Franziska Troja
% Datum: 13.01.2022
% Letzte Änderung: 20.01.2022


clear all
clc

% HSHL-Roboter
% Robot_IP = '192.168.0.75';


% Virtuelle Maschine
Robot_IP = '10.0.2.15';

%% Server-Verbindung
Socket_conn = tcpip(Robot_IP,33,'NetworkRole','server');

fclose(Socket_conn);                    % ggf. bestehende Verbindung schliessen

disp('Druecke Start im Polyscope des Roboters...')
fopen(Socket_conn);
disp('Server-Verbidnung konnte hergestellt werden!');

%% Client-Verbindung fuer Realtime-Schnittstelle zum Auslesen der Roboterdaten
Port_NR2 = 30003;
Socket_conn2 = tcpip(Robot_IP, Port_NR2);

fclose(Socket_conn2);                   % ggf. bestehende Verbindung schliessen
pause(2);                               % kurz warten

    fprintf(1, 'Aufbau der Verbindung mit dem UR3 ');
    fopen(Socket_conn2);                 % Verbindung herstellen

    error('-> Verbindung zum  UR3 nicht herstellbar');

fprintf(1, '-> Verbindung mit UR3 hergestellt.\n\n');
pause(2);                               % warten

%% Einlesen der Koordinatenpunkte der Klemmbausteine

[steine_matrix, Anzahl] = steinkoordinaten();

realtime = struktur_realtime(Robot_IP, Port_NR2);

%% Festlegung von Koordinatenpunkten der Magazine
%Sicherheitsposition ueber den Magazinentnahmestellen

magazin_1 = [-336.25 -33.28 -128.26 1.18 2.919 0];
magazin_2 = [-315.31 11.79 -143.84 1.126 2.858 0];
magazin_3 = [-289.23 38.91 -146.48 1.181 2.853 0];
magazin_4 = [-262.11 71.41 -148.05 1.173 2.829 0];



%% Kalibrierung des Roboters 
% Klemmbaustein muss oben links auf der Ecke mit 6 Noppen auf der Noppenplatte vertikal/horizontal liegen 
 
Robot_Pose = readrobotpose(Socket_conn)
Translation = Robot_Pose(1:3);  % in mm; 1.Stelle x-Koord, 2. y-Koord., 3. z-Koord. 
Orientation = Robot_Pose(4:6);  % Rx, Ry, Rz

% Ueberpruefung, das Roboter steht
Roboter_steht(Robot_IP, Port_NR2);
% Greifer oeffnen, damit Kalibrierungsstein abgelegt werden kann
fprintf(Socket_conn,'(4)'); %% Greifer AUF
pause(0.5);
% Position ueber Kalibrierungsstein anfahren
Translation(3) = Translation(3) + 10 ; % 20mm in y-Achse
moverobot(Socket_conn, Translation, Orientation); 
pause(0.5);

%% Hauptprogramm



for i = 1:Anzahl
    %% Auslesen der Magazinzuordnung des jeweiligen Steins
    magazin_stein = steine_matrix(i,5);
    stein = i                                   % Ausgabe der Steinnummer im Command Window
    
    % Zuordnung der Koordinaten abhängig der Entnahmestelle des jeweiligen Steins
    switch magazin_stein
       case 1
           magazin_trans = magazin_1(1:3);
           magazin_orien = magazin_1(4:6);
       case 2
           magazin_trans = magazin_2(1:3);
           magazin_orien = magazin_2(4:6);
       case 3
           magazin_trans = magazin_3(1:3);
           magazin_orien = magazin_3(1:3);
       case 4
           magazin_trans = magazin_4(1:3);
           magazin_orien = magazin_4(4:6);
    end

    %% Koordinaten der Positon fuer Klemmbaustein
    stein_koord = steine_matrix(i,1:3);
    stein_pos_trans = umrechnung_koord(stein_koord);
    
    % Ueberpruefung, das der Roboter steht
    Roboter_steht(Robot_IP, Port_NR2);
    
    %% Sicherheitsposition ueber Magazin
    sicher_magazin_Z = magazin_trans(3) + 30;
    sicher_magazin = [magazin_trans(1) magazin_trans(2) sicher_magazin_z];
    % Sicherheitsposition ueber Magazin anfahren
    moverobot(Socket_conn, sicher_magazin, magazin_orien);
    
    % Ueberpruefung, das der Roboter steht
    Roboter_steht(Robot_IP, Port_NR2);
    
    %% Magazinentnahmestelle anfahren und Stein greifen
    % Position der Entnahmestelle anfahren
    moverobot(Socket_conn, magazin_trans, magazin_orien);
    
    % Ueberpruefung, das der Roboter steht
    Roboter_steht(Robot_IP, Port_NR2);
    
    % Greifer schliessen, um Stein zu greifen
    fprintf(Socket_conn,'(3)'); % Greifer ZU
    pause(0.5); 
    
    %% erneutes Anfahren der Sicherheitsposition ueber Magazin 
    moverobot(Socket_conn, sicher_magazin, magazin_orien);
    
    % Ueberpruefung, das der Roboter steht
    Roboter_steht(Robot_IP, Port_NR2);
 
    
    %% Sicherheitsposition ueber Ablagestelle anfahren    
    sicher_pos_z = stein_pos_trans(3) + 30;
    sicher_pos_trans = [stein_pos_trans(3) stein_pos_trans(3) sicher_pos_z];
    moverobot(Socket_conn, sicher_pos_trans, Translation);
    
    % Ueberpruefung, das der Roboter steht
    Roboter_steht(Robot_IP, Port_NR2);
    
    %% Ablageposition anfahren und Stein ablegen
    moverobot(Socket_conn, stein_pos_trans, Translation);
    
    % Ueberpruefung, das der Roboter steht
    Roboter_steht(Robot_IP, Port_NR2);
    
    % Stein absetzen
    fprintf(Socket_conn,'(4)'); %% Greifer AUF
    pause(0.5); 
    
    %% erneutes Anfahren der Sicherheitsposition ueber Ablagestelle
    moverobot(Socket_conn, sicher_pos_trans, Translation);
    
    % Ueberpruefung, das der Roboter steht
    Roboter_steht(Robot_IP, Port_NR2);
end

%% Verbindung zu Roboter schliessen
fclose(Socket_conn);
fclose(Socket_conn2);