%pid.m
%PID Design


clc

next_figure=0;

pend_par

equi=input('Choose equilibrium point: 1- \theta=0, 2- \theta=pi ')

%Plant
if equi==1
    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];
elseif equi==2
    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];
else
    display('Wrong selection. Exiting ...')
    exit;
end
    
zG=roots(numG)
pG=roots(denG)
sysG=tf(numG,denG)

next_figure=next_figure+1;
figure(next_figure)
pzmap(sysG)

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

next_figure=next_figure+1;
figure(next_figure)
step(sysG)
axis([0 25 0 0.2])

next_figure=next_figure+1;
figure(next_figure)
impulse(sysG)
axis([0 25 -0.3 0.3])

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

%P:50,I:50,D:10
while 1
    Kp=input('\nEnter proportional gain (0 to exit): ');
    if Kp==0
        break;
    end
    Ki=input('\nEnter integral gain: ');
    Kd=input('\nEnter derivative gain: ');
    P_gain=Kp;
    I_gain=Ki;  
    D_gain=Kd;
    numK=[Kd Kp Ki];
	denK=[1 0];
	sysK=tf(numK,denK)

	%Series connection
	sys_series=series(sysK,sysG);
	
	%Feeback connection
	sys_feedback=feedback(sys_series,1)

	[Z_feedback,P_feedback,K_feedback]=zpkdata(sys_feedback,'v')
	next_figure=next_figure+1;
    figure(next_figure)
	pzmap(sys_feedback)

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

	tfinal=input('\nEnter simulation time: ');
	
    axis_type=input('\nEnter axis type (fixed=1, variable=0): ')
    
    [ystep,tstep]=step(sys_feedback,tfinal);
	next_figure=next_figure+1;
    figure(next_figure)
    plot(tstep,ystep,'r-',tstep,ones(size(tstep)),'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
    xlabel('Time (sec)')
    ylabel('Amplitude')

    [yimp,timp]=impulse(sys_feedback,tfinal);
	next_figure=next_figure+1;
    figure(next_figure)
    plot(timp,yimp,'r-',timp,ones(size(timp)),'b:')
    title(['IMPULSE RESPONSE - PID CONTROLLER: K_{p}=',num2str(Kp),' K_{I}=',num2str(Ki),' K_{D}=',num2str(Kd)])
    if axis_type==1
        axis([0 tfinal -2 12])
    else
        axis([0 tfinal -Inf Inf])
    end
    xlabel('Time (sec)')
    ylabel('Amplitude')    
end



%echo off

