clc
clear all
close all
%采樣時間
ts=20
%PID的控制變量 1 0.005 0.3
kp=1;
ki=0.05;
kd=0.3;
%原系統離散化
sysc=tf([1],[60 1],'inputdelay',80);
sysd=c2d(sysc,ts,'zoh');
%得到需要的離散系統參數
[num,den]=tfdata(sysd,'v');
%PID仿真參數設定
u1=0;u2=0;u3=0;u4=0;u5=0;
y1=0;y2=0;y3=0;
e1=0;e2=0;
es=0; 
%開始PID仿真
for k=1:500
    t(k)=k*ts;
    y(k)=-den(2)*y1+num(2)*u5;
    r(k)=1;
    e(k)=r(k)-y(k);
    es=es+e(k)*ts;
    %PID Control
    u(k)=kp*e(k)+kd*(e(k)-e1)/ts+ki*es;
    %Control V Lim
    if u(k)>5
        u(k)=5;
    elseif u(k)<-5
        u(k)=-5;
    end
    u5=u4;
    u4=u3;
    u3=u2;
    u2=u1;
    u1=u(k); 
    
    e2=e1;
    e1=e(k);
end
%DMC仿真
%建立離散的系統
gmpc=poly2tfd([1],[60 1],0,80);
%系統階躍響應的時間
tend=20000;
mymodel=tfd2step(tend,ts,1,gmpc);
%MPC參數設定
P=5;
M=3;
ywt=[];
uwt=[1];
rmpc=[1];
%系統的仿真時間設定
tend=10000;
%建立MPC的系數矩陣
mpcM=mpccon(mymodel,ywt,uwt,M,P);
%MPC仿真
[ympc,umpc,ymmpc]=mpcsim(mymodel,mymodel,mpcM,tend,rmpc);
%繪圖
%PID控制繪圖
figure(1)
subplot(2,1,1);
title('PID System Output');
plot(t,r,'b',t,y,'r');
grid on
xlabel('Time(s)');
ylabel('System Input(V) & System Output');
subplot(2,1,2);
title('PID Control Output');
plot(t,u);
grid on
xlabel('Time(s)');
ylabel('Control Output')
%MPC控制繪圖
figure(2)
subplot(2,1,1);
title('MPC System Output');
plot(t,r,'b',t,ympc(1:500),'r');
grid on
xlabel('Time(s)');
ylabel('System Input(V) & System Output');
subplot(2,1,2);
title('MPC Control Output');
plot(t,umpc(1:500));
grid on
xlabel('Time(s)');
ylabel('Control Output')
%PID控制與MPC控制比較
figure(3)
subplot(2,1,1);
title('PID System Output(blue) Vs MPC System Output(red)');
plot(t,y,'b',t,ympc(1:500),'r');
grid on;
xlabel('Time(s)');
ylabel('System Output');
subplot(2,1,2);
title('PID Control Output(blue) Vs MPC Control Output(red)');
plot(t,u,'b',t,umpc(1:500),'r');
grid on;
xlabel('Time(s)');
ylabel('Control Output');