%%%
%%% Robotik mit UR3, mit Fernsteuerung des UR3
%%% Testprogramm


close all
clear all
clc


%% Roboter auswaehlen
mdl_ur3;

%% Roboter-Einstellungen fuer die Bewegung
% Beschleunigung in rad/s^2
accel_radss = 3;
% Geschwindigkeit in rad/s 
vel_deg = 70;
vel_rads = deg2rad(vel_deg);
% Wird nicht benoetigt!!!
time_robot = 0;
blend_radius = 0;


%% Verbindung herstellen

% Virtueller UR3
Robot_IP = '10.0.2.15';

Port_NR = 30003;
Socket_conn = tcpip(Robot_IP,Port_NR);
fclose(Socket_conn);                % ggf. bestehende Verbindung schliessen
pause(2);                           % kurz warten
try
    fprintf(1, 'Aufbau der Verbindung mit dem UR3....');
    fopen(Socket_conn);             % Verbindung herstellen
catch
    error('Verbindung zum  UR3 nicht herstellbar');
end

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

%% Startposition des Roboters
q_start = [0, -pi/2, 0, -pi/2, -pi/2, 0];

%% Roboter in Startposition fahren
q = q_start;
Befehl = command2string(q,accel_radss,vel_rads,time_robot,blend_radius);
fprintf(Socket_conn, Befehl);

fprintf(1, 'Verfahre den UR3 an die Startposition.\n');
pause(5);


%% Gelenkkoordinaten = Gelenkwinkel waehlen
q1 = deg2rad(0);
q2 = deg2rad(-35);
q3 = deg2rad(60);
q4 = deg2rad(-115);
q5 = deg2rad(-90);
q6 = deg2rad(0);



%% Darstellung
figure(1) % Figure von Matlab oeffnen
ur3.plot([q1 q2 q3 q4 q5 q6]);


%% Roboter verfahren
% q in rad umrechnen
q = [q1, q2, q3, q4, q5, q6] * 180/pi;
% Befehl zusammensetzen
Befehl = command2string(q,accel_radss,vel_rads,time_robot,blend_radius);
% Befehl an Roboter schicken
fprintf(1, 'Verfahre den UR3.\n');
fprintf(Socket_conn, Befehl);

%% zweite Position mit Roboter anfahren

% Umrechnung Gradmaß in Bogenmaß
start = [0 -35 60 -115 -90 0] * pi/180;


fprintf(Socket_conn, command2string(start, accel_radss, vel_rads, time_robot, blend_radius));
pause(2);
startPosition = ur3.fkine(start).t
fprintf(1, 'Roboter befindet sich in Startposition.\n');


function [commandString] = command2string(q,a,v,t,r)
    
    commandString = ['movej([' num2str(q(1)) ', ' num2str(q(2)) ', ' num2str(q(3)) ', ' num2str(q(4))...
                    ', ' num2str(q(5)) ', ' num2str(q(6)) '], ' num2str(a) ', ' num2str(v) ', ' num2str(t)...
                    ', ' num2str(r) ')'];

end