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

%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

%P controller transfer function - position loop
Kp = 2 %Much greater values than 2 will make the torque reach its max value 

%PI controller transfer function - velocity loop
Tv = Tm %made equal to Tm, then zero of controller cancels pole of motor
Kv = 1 %to be adjusted

%Encoder transfer function
KTP = 1
%Tachometer transfer function
KTV = 1

%Open loop transfer function
P = tf(Km*Kp*Kv*[Tv 1],[Tm 1 0 0])
H = KTP*tf([KTV/(Kp*KTP) 1],1)
L = series(P,H)

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

%Adjust Kp value by multiplying by the factor found by rlocfind
Kv = K*Kv
P = tf(Km*Kp*Kv*[Tv 1],[Tm 1 0 0])
H = KTP*tf([KTV/(Kp*KTP) 1],1) 
W = feedback(P,H)
[zeros,pols,k] = zpkdata(W,'v')