clear all

tic

t = 0:0.01:1;
theta_d_1 = zeros(1,length(t));
theta_dot_d_1 = zeros(1,length(t));
theta_dot_dot_d_1 = zeros(1,length(t));
theta_d_2 = zeros(1,length(t));
theta_dot_d_2 = zeros(1,length(t));
theta_dot_dot_d_2 = zeros(1,length(t));

global u_1 u_2 counter

counter = 1;
u_1 = zeros(1,length(t));
u_2 = zeros(1,length(t));

i = 1;

for t = 0:0.01:1

     theta_d_1(i) = 0.01454441*t^3 - 0.0036361*t^4 + 0.00024241*t^5;
     theta_dot_d_1(i) = 3*0.01454441*t^2 - 4*0.0036361*t^3 + 5*0.00024241*t^4;
     theta_dot_dot_d_1(i) = 6*0.01454441*t - 12*0.0036361*t^2 + 20*0.00024241*t^3;
     %theta_d_1(i) = 0.5*sin(0.3*pi*t);
     %theta_dot_d_1(i) = 0.5*0.3*pi*cos(0.3*pi*t);

     theta_d_2(i) = 0.01454441*t^3 - 0.0036361*t^4 + 0.00024241*t^5;
     theta_dot_d_2(i) = 3*0.01454441*t^2 - 4*0.0036361*t^3 + 5*0.00024241*t^4;
     theta_dot_dot_d_2(i) = 6*0.01454441*t - 12*0.0036361*t^2 + 20*0.00024241*t^3;
     %theta_d_2(i) = 0.5*cos(0.3*pi*t);
     %theta_dot_d_2(i) = -0.5*0.3*pi*sin(0.3*pi*t);

    i = i + 1;

end

% Constant Controller Parameters
global l_1 l_2 k1 k2 p_i p_1_0 p_2_0 gamma_1 gamma_2 epsilon_1 epsilon_2 K step_size_angle step_size_velocity
global q_prev_theta_1 q_prev_theta_2 q_prev_theta_3 q_prev_theta_4

% Manipulator's Parameters
% I_i --> Moment of Inertia of Link-i
% m_i --> Mass of Link-i
% li  --> Length of Link-i
global I_1 I_2 m_1 m_2 l1 l2 g
I_1 = 0.96;
I_2 = 0.81;
m_1 = 1.5;
m_2 = 1;
l1 = 0.25;
l2 = 0.2;
g = 9.81;

global k_1 k_2 % --> Friction Coefficients of Joints 1 & 2 respectively
k_1 = 1;
k_2 = 1;

l_1 = 1.9;
l_2 = 1.9;
k1 = 2;
k2 = 2;
p_i = 0.01;
p_1_0 = 2.8;
p_2_0 = 2.8;
gamma_1 = 0.05;
gamma_2 = 0.05;
epsilon_1 = 0.1;
epsilon_2 = 0.1;
% K = 100000; % --> With K = 100000 the simulation runs
K = 100000;
step_size_angle = 0.002;
step_size_velocity = 0.001;

% Initial Conditions of Joint Variables
% The system initially rests --> Initial Velocities are equal to 0
theta = [pi/16 ; 0 ; pi/14 ; 0];
q_prev_theta_1 = theta(1);
q_prev_theta_2 = theta(2);
q_prev_theta_3 = theta(3);
q_prev_theta_4 = theta(4);

tspan = 0:0.01:1;
% The definitely executable one --> odeset('RelTol',1e-7,'AbsTol',1e-10)
S = [0 1 0 0 ; 1 1 1 1 ; 0 0 0 1 ; 1 1 1 1];
options = odeset('RelTol',1e-7,'AbsTol',1e-10,'JPattern',S,'Refine',4);
[t, theta] = ode15s(@ode_simulation_kuka, tspan, theta, options);

first_joint_angle = theta(:,1);
first_joint_angular_velocity = theta(:,2);
second_joint_angle = theta(:,3);
second_joint_angular_velocity = theta(:,4);

p_1 = p_1_0*exp(-l_1*t) + k1*p_i;
p_2 = p_2_0*exp(-l_2*t) + k2*p_i;

delta_1 = k1*step_size_velocity+k1*step_size_angle;
delta_2 = k2*step_size_velocity+k2*step_size_angle;

first_joint_angle_error = first_joint_angle - theta_d_1';
first_joint_velocity_error = first_joint_angular_velocity - theta_dot_d_1';
second_joint_angle_error = second_joint_angle - theta_d_2';
second_joint_velocity_error = second_joint_angular_velocity - theta_dot_d_2';