%% UR5e_data.m

%% Simulation and Eviroment Parameters
Sim.time = 15;                              %[s] time of simulation
Sim.dt = 1e-5;                              %[s] time step
Sim.g = 9.80665;                            %[m/s^2] gravity acceleration
Sim.T = 25;                                 %[°C] enviroment temperature
Ground.l = 1;                               %[m] length
Ground.w = 1;                               %[m] width
Ground.h = 0.1;                             %[m] height

%% Links Parameters
%Link 1
L1.Mass = 3.7610;                        %[kg] base mass
L1.CM = [0, -0.02561, 0.00193];          %[m] base center of mass
L1.InertiaM = [0.0067, 0.0064, 0.0067];  %[kg*m^2] base moment of inertia
L1.InertiaP = [0, 0, 0];                 %[kg*m^2] base product of inertia

%Link 2
L2.Mass = 8.0580;                        %[kg] lower arm mass
L2.CM = [0.2125, 0, 0.11336];            %[m] lower arm center of mass
L2.InertiaM = [0.0149, 0.3564, 0.3553];  %[kg*m^2] lower arm moment of inertia
L2.InertiaP = [0, 0, 0];                 %[kg*m^2] lower arm product of inertia

%Link 3
L3.Mass = 2.8460;                        %[kg] upper arm mass
L3.CM = [0.15, 0.0, 0.0265];             %[m] upper arm center of mass
L3.InertiaM = [0.0025, 0.0551, 0.0546];  %[kg*m^2] upper arm moment of inertia
L3.InertiaP = [0, 0.0034, 0];            %[kg*m^2] upper arm product of inertia

%Link 4
L4.Mass = 1.3700;                        %[kg] wrist 2 mass
L4.CM = [0, -0.0018, 0.01634];           %[m] wrist 2 center of mass
L4.InertiaM = [0.0012, 0.0012, 0.0009];  %[kg*m^2] wrist 2 moment of inertia
L4.InertiaP = [0, 0, 0];                 %[kg*m^2] wrist 2 product of inertia

%Link 5
L5.Mass = 1.3000;                        %[kg] wrist 3 mass
L5.CM = [0, 0.0018,0.01634];             %[m] wrist 3 center of mass
L5.InertiaM = [0.0012, 0.0012, 0.0009];  %[kg*m^2] wrist 3 moment of inertia
L5.InertiaP = [0, 0, 0];                 %[kg*m^2] wrist 3 product of inertia

%Link 6
L6.Mass = 0.356;                        %[kg] tool flange mass
L6.CM = [0, 0, -0.001159];               %[m] tool flange center of mass
L6.InertiaM = [0.0001, 0.0001, 0.0001];  %[kg*m^2] tool flange moment of inertia
L6.InertiaP = [0, 0, 0];                 %[kg*m^2] tool flange product of inertia

%% DH Table, DH Matrices and Geometric Errors
%DH Table
DH.table = UR_DHtable_e('UR5');

%DH Matrices
DH.theta = DH.table(2:7,1)';
DH.d = DH.table(2:7,2)';
DH.a = DH.table(1:6,3)';
DH.alfa = DH.table(1:6,4)';
A_all = DH_mat(DH.theta,DH.d,DH.a,DH.alfa);

% Geometric errors
E.E01 = eye(4);
E.E12 = eye(4);
E.E23 = eye(4);
E.E34 = eye(4);
E.E45 = eye(4);
E.E56 = eye(4);
A_all(:,:,1) = A_all(:,:,1)*E.E01;
A_all(:,:,2) = A_all(:,:,2)*E.E12;
A_all(:,:,3) = A_all(:,:,3)*E.E23;
A_all(:,:,4) = A_all(:,:,4)*E.E34;
A_all(:,:,5) = A_all(:,:,5)*E.E45;
A_all(:,:,6) = A_all(:,:,6)*E.E56;

% DH Rotation Matrices and DH Translation Vector
A01r = A_all(1:3,1:3,1);
A01t = A_all(1:3,4,1);
A12r = A_all(1:3,1:3,2);
A12t = A_all(1:3,4,2);
A23r = A_all(1:3,1:3,3);
A23t = A_all(1:3,4,3);
A34r = A_all(1:3,1:3,4);
A34t = A_all(1:3,4,4);
A45r = A_all(1:3,1:3,5);
A45t = A_all(1:3,4,5);
A56r = A_all(1:3,1:3,6);
A56t = A_all(1:3,4,6);

%% Gearbox Parameters
Motor.eta = 1;                              %[/] gear box efficiency
Motor.tau = 101;                            %[/] gear ratio

%% Joints Parameters
%Joint 1
J1.al = 360;                             %[deg] angular position limits
J1.B = 0.0525/Motor.tau^2;               %[N*m*s/rad] coulomb friction
J1.Cp = 0.07685/Motor.tau;               %[N*m] positive viscous friction
J1.Cn = -0.07655/Motor.tau;              %[N*m] negative viscous friction

%Joint 2
J2.al = 360;                             %[deg] angular position limits
J2.B = 0.0611/Motor.tau^2;               %[N*m*s/rad] coulomb friction
J2.Cp = 0.08392/Motor.tau;               %[N*m] positive viscous friction
J2.Cn = -0.08209/Motor.tau;              %[N*m] negative viscous friction

%Joint 3
J3.al = 360;                             %[deg] angular position limits
J3.B = 0.0603/Motor.tau^2;               %[N*m*s/rad] coulomb friction
J3.Cp = 0.07835/Motor.tau;               %[N*m] positive viscous friction
J3.Cn = -0.07774/Motor.tau;              %[N*m] negative viscous friction

%Joint 4
J4.al = 360;                             %[deg] angular position limits
J4.B = 0.0081/Motor.tau^2;               %[N*m*s/rad] coulomb friction
J4.Cp = 0.01430/Motor.tau;               %[N*m] positive viscous friction
J4.Cn = -0.01474/Motor.tau;              %[N*m] negative viscous friction

%Joint 5
J5.al = 360;                             %[deg] angular position limits
J5.B = 0.0179/Motor.tau^2;               %[N*m*s/rad] coulomb friction
J5.Cp = 0.02085/Motor.tau;               %[N*m] positive viscous friction
J5.Cn = -0.01900/Motor.tau;              %[N*m] negative viscous friction

%Joint 6
J6.al = 360;                             %[deg] angular position limits
J6.B = 0.0124/Motor.tau^2;               %[N*m*s/rad] coulomb friction
J6.Cp = 0.02097/Motor.tau;               %[N*m] positive viscous friction
J6.Cn = -0.02111/Motor.tau;              %[N*m] negative viscous friction

%% Motor and Driver Parameters
U_s = 48;                                   %[V] DC monophase equivalent supply voltage

%Simple DC Motor
%Motor 1-2-3
Motor.Im_123 = 187.740e-06;                 %[kg*m^2] first three motors rotor inertia
Motor.R_123 = 0.3;                          %[Ohm] DC motor resistance
Motor.L_123 = 0.83e-3;                      %[H] DC motor inductance
Motor.kc_1 = 0.1350;                        %[Nm/A] DC motor torque constant
Motor.kc_2 = 0.1361;                        %[Nm/A] DC motor torque constant
Motor.kc_3 = 0.1355;                        %[Nm/A] DC motor torque constant
Motor.kv_1 = Motor.kc_1;                    %[V*s/rad] DC motor electrical constant
Motor.kv_2 = Motor.kc_2;                    %[V*s/rad] DC motor electrical constant
Motor.kv_3 = Motor.kc_3;                    %[V*s/rad] DC motor electrical constant
Motor.Tmax123 = 28/Motor.tau;               %[N*m] Max motor torque
Motor.imax1 = Motor.Tmax123/Motor.kc_1;     %[A] Max current
Motor.imax2 = Motor.Tmax123/Motor.kc_2;     %[A] Max current
Motor.imax3 = Motor.Tmax123/Motor.kc_3;     %[A] Max current

%Motor 4-5-6
Motor.Im_456 = 20.767e-06;                  %[kg*m^2] last three motors rotor inertia 
Motor.R_456 = 1.65;                         %[Ohm] DC motor resistance
Motor.L_456 = 2.5e-3;                       %[H] DC motor inductance
Motor.kc_4 = 0.0957;                        %[Nm/A] DC motor torque constant
Motor.kc_5 = 0.0865;                        %[Nm/A] DC motor torque constant
Motor.kc_6 = 0.0893;                        %[Nm/A] DC motor torque constant
Motor.kv_4 = Motor.kc_4;                    %[V*s/rad] DC motor electrical constant
Motor.kv_5 = Motor.kc_5;                    %[V*s/rad] DC motor electrical constant
Motor.kv_6 = Motor.kc_6;                    %[V*s/rad] DC motor electrical constant
Motor.Tmax456 = 150/Motor.tau;              %[N*m] Max motor torque
Motor.imax4 = Motor.Tmax456/Motor.kc_4;     %[A] Max current
Motor.imax5 = Motor.Tmax456/Motor.kc_5;     %[A] Max current
Motor.imax6 = Motor.Tmax456/Motor.kc_6;     %[A] Max current

% PI position loop control
% Driver 1
Driver1.P = 1;
Driver1.I = 0;
Driver1.G = 1e3;

% Driver 2
Driver2.P = 1;
Driver2.I = 0;
Driver2.G = 1e3;

% Driver 3
Driver3.P = 1;
Driver3.I = 0;
Driver3.G = 1e3;

% Driver 4
Driver4.P = 5;
Driver4.I = 0;
Driver4.G = 1e3;

% Driver 5
Driver5.P = 5;
Driver5.I = 0;
Driver5.G = 1e3;

% Driver 6
Driver6.P = 5;
Driver6.I = 0;
Driver6.G = 1e3;

% PWM Control
T_switch =  1e-4;                           %[s] PWM carrier period

% PI current loop control
kp_i = 50;                                  %[V/A]
ki_i = 10;                                  %[V*s/A]
imax = 15;
% PI motor velocity loop control
kp_v = 10;                                  %[A*s/m]
ki_v = 0;                                  %[A*s^2/m]
% P position loop control
kp_p = 1e3;

%% Sensors Parameters
%Current Sensor
tau_i = 1/(2000*2*pi);                      %[s] Current sensor time constant
Hi = 1;                                     %[V/A] Current sensor static gain

% Initial angular position
%q0 = [0.0968,2.0164,-2.1052,1.6605,1.5680,0.0948];

%qset = testlaw(15,'pick&place_test');