%pid.m
%PID Design


clc

pend_par

%Plant
fprintf('\nPLANT: G(s)=1/(m*l^2*(s^2+b/m/l^2*s-g/l))\n')
numG=[1/m/l^2];
denG=[1 b/m/l^2 -g/l];
zG=roots(numG)
pG=roots(denG)
sysG=tf(numG,denG)
figure(1)
pzmap(sysG)
title('Pole-Zero Map: Open-loop Transfer Function')

fprintf('\nPlotting Step Response in OPEN loop ... \n')

figure(2)
%step(g*m*l*sysG,10)
step(sysG,10)
axis([0 10 0 2])
title('Step Response: Open-loop Transfer Function')

fprintf('\nPress a key to continue ... \n')
pause

%P:50,I:50,D:10
while 1
    Kp=input('\nEnter proportional gain (0 to end): ');
    if Kp==0
        break;
    end
    Ki=input('\nEnter integral gain: ');
    Kd=input('\nEnter derivative gain: ');
    fprintf('\nCONTROLLER: K(s)=(Kd*s^2+Kp*s+Ki)/(s)\n')
    numK=[Kd Kp Ki];
    %Ideal Differentiator
    denK1=[1 0];
    %Real Differentiator
    denK2=conv([1 0],[1/1000 1]);
    denK=denK1;
    zK=roots(numK)
    pK=roots(denK)
	sysK=tf(numK,denK)

	%Series connection
	sys_series=series(sysK,sysG);
	figure(3)
    pzmap(sysK)
    title('Pole-Zero Map: Controller Transfer Function')

    figure(4)
    rlocus(sys_series)
    title('Root Locus: Loop Gain Transfer Function')
    
	%Feeback connection
	sys_feedback=feedback(sys_series,1)

	[Z_feedback,P_feedback,K_feedback]=zpkdata(sys_feedback,'v')
	figure(5)
	pzmap(sys_feedback)
    title('Pole-Zero Map: Closed-loop Transfer Function')
    
    numCL=[1 Kp/Kd Ki/Kd]*Kd/m/l^2;
    denCL=[1 (b+Kd)/m/l^2 (-g/l+Kp/m/l^2) Ki/m/l^2];
    zCL=roots(numCL)
    pCL=roots(denCL)
    KCL=Kd/m/l^2

	fprintf('\nPress a key to continue ... \n')
	pause

	tfinal=input('\nEnter simulation time: ');
	[y,t]=step(sys_feedback,tfinal);

	axis_type=input('\nEnter axis type (fixed=1, variable=0): ')
	figure(6)
    plot(t,y,'r-',t,ones(size(t)),'b:')
    title(['STEP RESPONSE - PID CONTROLLER: K_{p}=',num2str(Kp),' K_{I}=',num2str(Ki),' K_{D}=',num2str(Kd)])
    if axis_type==1
        axis([0 tfinal 0 1.8])
    else
        axis([0 tfinal -Inf Inf])
	end
end

%echo off

