/* "mckpd.mm" Simulation of the following system: M x^(2) + C x^(1) + K x = u + d u = -Kp x - Kd x^(1), d = 0.5 Kp = 4.0; Kd = 4.0; */ Func void main() { Real t0,t1; Real h,dtsav; Matrix x0,TC,XC,UC; void diff_eqs(),link_eqs(); t0 = 0.0; //シミュレーションの開始時刻を t=0 にする t1 = 20.0; //シミュレーションの終了時刻を t=20 にする x0 = [1.0,0.0]'; //初期状態を x(0)=[1.0,0.0]' にする h = 1.0e-3; // シミュレーションの刻を1msecにする dtsav = 1.0e-2;// データは10msecおきに保存する print "Now simulating\n"; // Ode()によってシミュレーションを行う {TC,XC,UC} = Ode(t0,t1,x0,diff_eqs,link_eqs,h,dtsav); // diff_eqs() は微分方程式を記述する関数 // link_eqs() は複数の微分方程式の関係を記述する関数 // TC: 時間の時系列 // XC: 状態x(t)の時系列 // UC: 入出力ベクトルの時系列(diff_eqs,link_eqsのUY) // 数値解を表示する.横軸 t, 縦軸 x(t) mgplot(1,TC,XC(1,*)); // XC(1,*)はXCの1行目を表す // 数値解を表示する.横軸 t, 縦軸 u(t) mgplot(2,TC,UC); // ファイルに保存 print TC >> "TC.mat"; print XC >> "XC.mat"; print UC >> "UC.mat"; } |
// diff_eqs() は微分方程式を記述する関数 Func void diff_eqs(DX,t,X,UY) Real t; Matrix X,DX,UY; { Real K,C,M; Matrix xp,up,dxp; Matrix A,B; Matrix d; // 物理パラメータの設定 K = 1.0; C = 0.4; M = 1.0; // A,Bの設定 A = [[0,1][-K/M,-C/M]]; B = [0,1/M]'; xp = X; // X はxp(t)を表している. // link_eqs()で計算したUYを代入する. up = UY; // 外乱dの設定 d = [0.5]; // 微分方程式 dxp = A * xp + B * (up + d); // 最後に速度をDXに渡す DX = [dxp]; } // link_eqs() は複数の微分方程式の関係を記述する関数 Func void link_eqs(UY,t,X) Real t; Matrix UY,X; { Real Kp,Kd; Matrix xp,up; // X はxp(t)を表している. xp = X; // Kp,Kdを設定 Kp = 4.0; Kd = 4.0; // 最後にUYにupの時刻tの値を渡す. up = [-Kp,-Kd] * xp; // UYにupを代入し,diff_eqsに渡す UY = [up]; } |
前節との違いは,link_eqs()
で制御入力(up
)を計算している点で
す.計算されたup
はUY
を通してdiff_eqs()
に渡されてい
ます.