% ------ pid2.m ------
% 小車位置PID控制 
% 輸入倒立擺傳遞函數 G1(s)=num1/den1,G2(s)=num2/den2
M = 0.5;
m = 0.2;
b = 0.1;
I = 0.006;
g = 9.8;
l = 0.3;
q = (M+m)*(I+m*l^2) -(m*l)^2;  
num1 = [m*l/q  0 0];
den1 = [1  b*(I+m*l^2)/q  -(M+m)*m*g*l/q  -b*m*g*l/q 0];
num2 = [-(I+m*l^2)/q  0  m*g*l/q];
den2 = den1;
% 輸入控制器PID數學模型 Gc(s)=numPID/denPID
Kp = 1;
Ki = 1; 
Kd = 1;
numPID = [Kd Kp Ki];
denPID = [1 0];
% 計算閉環系統傳遞函數G(s)=num/den
% 多項式相乘
num = conv(num2,denPID);
% 多項式相加
den = polyadd(conv(denPID,den2),conv(numPID,num1 ));
% 求閉環系統極點
[r,p,k] = residue(num,den);
% 顯示閉環系統極點
s = p
% 求取多項式傳函的脈沖響應
t=0:0.005:5;
impulse(num,den,t) 
% 顯示范圍：橫坐標0-5，縱坐標0-10，此條語句參數可根據仿真輸出曲線調整
axis([0 5 0 10])
grid
% ------ end ------