clear
%Motor Transfer function (voltage to velocity): M(s) = Kmm / (s*Im + Bm)
Im = 581e-6 % Total inertia seen by motor of joint 2 as computed by:
            %   >> mdl_puma560
            %   >> p560_inertia_evaluation
            %   >> Jm = p560.links(2).Jm + meanB22 / p560.links(2).G^2
Bm = 817e-6 % Joint 2 viscous friction: p560.links(2).B
Kmm = 0.228 % Motor 2 torque constant from Table 9.1 page 205 Corke book

%Motor Transfer function (voltage to velocity): M(s) = Km / (s*Tm + 1)        
Tm = Im/Bm
Km = Kmm/Bm
Gpp = tf([Kmm],[Im Bm 0])

%Other motor constants:
G2 = 107.815 %Joint 2 Gear ratio: p560.links(2).G
tau_max = 0.9 %Maximum torke of motor 2 from Table 9.1 page 205 Corke book

%PI controller transfer function
Tp = 10  %chosen 10 times greater than Tm
Kp = 1 %to be adjusted
Cp = tf([Kp*Tp Kp],[1 0])

%Encoder transfer function
KTP = 1

%Open loop transfer function
L = KTP*series(Cp,Gpp)

%Root locus
%rlocus(L)
h=rlocusplot(L); 
p = getoptions(h);
p.YLim={[-2.5 2.5]};
setoptions(h,p);

%Choose the poles near -1/Tp
K = rlocfind(L)

%Adjust Kp value by multiplying by the factor found by rlocfind
Kp = K*Kp
Cp = tf([Kp*Tp Kp],[1 0])
L = KTP*series(Cp,Gpp) 
W = feedback(L,1)
[zeros,pols,k] = zpkdata(W,'v')