
clear all
clc

% Link lengths ------------------------------------------------------------
 l1= 146; l1_1 = 81; l1_2 = 65; l2= 140; l3 = 47; % Link lenghts
 L = [l1 l1_1 l1_2 l2 l3];                                   % Vector L
% Global variables (only animation purposes!) -----------------------------
 global Hh P_trace
 Hh = (l1+l2+l3)*0.923; 
% Joint angular&speed limits ----------------------------------------------
 q_lim = [  -pi  -pi  -pi  -pi;...     % Joint physical limits
             pi   pi   pi  pi];        % Joint physical limits
 dq_max = [3 3 3 3 3 3]';              % Maximum joint angular velocities 

Gain(1)	= (30/22);
Gain(2)	= (32/8);
Gain(3)	= 1;
Gain(4)	= (30/22);
Gain(5)	= (32/8);
Gain(6)	= (32/8);
 
% qi(1) = -(qi(1)-15);
% qi(2) = qi(2)+15;
% qi(3) = qi(3)+0;
% qi(4) = qi(4)-40;
% qi(5) = -(qi(5)+0);
% qi(6) = qi(6)+0;
% 
% qi(1) = 0;
% qi(2) = 0;
% qi(3) = 0;
% qi(4) = 0;
% qi(5) = 0;
% qi(6) = 0;
% qi = qi*pi/180;
% P = DirKin_6dof(qi,L);
% P1 = P(:,4)'

XYZ_i = [0 0 -25]       % 9.86 -15 0 29.53 -15.67 0
[qi,IsVal] = InvKin_6dof(XYZ_i,L,q_lim);

P = DirKin_6dof(qi,L);
P1 = P(:,4)'

% converter e regular
qi = qi*180/pi;
for i=1:6
    qi(i) = qi(i)*Gain(i);
end
qi(1) = -qi(1)+15;
qi(2) = qi(2)-15;
qi(3) = qi(3)+0;
qi(4) = qi(4)+40;
qi(5) = -qi(5)+0;
qi(6) = qi(6)+0;
q1 = qi'

XYZ_i = [115 0 0]       % -25 -15 0 24.67 72.37 0
[qi,IsVal] = InvKin_6dof(XYZ_i,L,q_lim);
qi = qi*180/pi;
for i=1:6
    qi(i) = qi(i)*Gain(i);
end
qi(1) = -qi(1)+15;
qi(2) = qi(2)-15;
qi(3) = qi(3)+0;
qi(4) = qi(4)+40;
qi(5) = -qi(5)+0;
qi(6) = qi(6)+0;
q2 = qi'

XYZ_i = [-115 0 0]      % 40 -15 0 24.67 -118.28 0
[qi,IsVal] = InvKin_6dof(XYZ_i,L,q_lim);
qi = qi*180/pi;
for i=1:6
    qi(i) = qi(i)*Gain(i);
end
qi(1) = -qi(1)+15;
qi(2) = qi(2)-15;
qi(3) = qi(3)+0;
qi(4) = qi(4)+40;
qi(5) = -qi(5)+0;
qi(6) = qi(6)+0;
q3 = qi'

XYZ_i = [30 0 0]        % -25.44 -15 0 -24.38 -70.23 0
[qi,IsVal] = InvKin_6dof(XYZ_i,L,q_lim);
qi = qi*180/pi;
for i=1:6
    qi(i) = qi(i)*Gain(i);
end
qi(1) = -qi(1)+15;
qi(2) = qi(2)-15;
qi(3) = qi(3)+0;
qi(4) = qi(4)+40;
qi(5) = -qi(5)+0;
qi(6) = qi(6)+0;
q4 = qi'