data-1
がdata-2
が
// "manip2.mm" // Simulation of the tow link manipulator 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 にする // 初期状態を x(0)=[PI*0.75,0.0,0.0,0.0]' にする x0 = [PI*0.75,0.0,0.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, 縦軸 th1(t),th2(t) mgplot(1,TC,XC(1:2,*)); // XC(1:2,*)はXCの1,2行目を表す // 数値解を表示する.横軸 t, 縦軸 tau1(t),tau2(t) // mgplot(2,TC,UC); pause; // ファイルに保存 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 th1,th2,th1d,th2d,J1,J2,m1,m2,l1,l2,D1,D2,g; Matrix xp,up,dxp; Matrix q,qd; Matrix d; Matrix J,C,D,G; // X = [[q][qd]]; q = X(1:2,1); qd = X(3:4,1); // link_eqs()で計算したUYを代入する. up = UY; |
// q = [th1,th2]'; th1 = q(1,1); th2 = q(2,1); // qd = [th1d,th2d]'; th1d = qd(1,1); th2d = qd(2,1); // 物理パラメータ m1 = m2 = 1.0; l1 = l2 = 1.0; J1 = m1*l1^2/3.0+(m1+4*m2)*l1^2; J2 = m2*l2^2/3.0+m2*l2^2; D1 = 5.0; D2 = 5.0; g = 9.805; // J(q)の計算 J = [[J1+J2+2*2*m2*l1*l2*cos(th2), J2+2*m2*l1*l2*cos(th2)] [J2+2*m2*l1*l2*cos(th2),J2]]; // C(q,qd)の計算 C = [[-2*m2*l1*l2*(2*th1d*th2d+th2d^2)*sin(th2)] [2*m2*l1*l2*th1d^2*sin(th2)]]; // Dの計算 D = [[D1,0] [0,D2]]; // Gの計算 G=-[[(m1+2*m2)*l1*g*sin(th1)+m2*l2*g*sin(th1+th2)] [m2*l2*g*sin(th1+th2)]]; // 外乱dの設定 d = Z(2,1); // 微分方程式 dxp=[[qd][-J~*(C+D*qd+G)]]+[[Z(2,2)][J~]]*(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; Matrix q,qd; // X = [[q][qd]]; q = X(1:2,1); qd = X(3:4,1); // 最後にUYにupの時刻tの値を渡す. up = Z(2,1); // UYにupを代入し,diff_eqsに渡す UY = [up]; } |
後の都合により,このプログラムを関数とグローバル変数を用いて 書き換えておきましょう.
// "manip2new.mm" // Simulation of the tow link manipulator Real J1,J2,m1,m2,l1,l2,D1,D2,g;// グローバル変数 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 にする // 初期状態を x(0)=[PI*0.75,0.0,0.0,0.0]' にする x0 = [PI*0.75,0.0,0.0,0.0]'; h = 1.0e-3; // シミュレーションの刻を1msecにする dtsav = 1.0e-2;// データは10msecおきに保存する // 物理パラメータ m1 = m2 = 1.0; l1 = l2 = 1.0; J1 = m1*l1^2/3.0+(m1+4*m2)*l1^2; J2 = m2*l2^2/3.0+m2*l2^2; D1 = 5.0; D2 = 5.0; g = 9.805; 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, 縦軸 th1(t),th2(t) mgplot(1,TC,XC(1:2,*)); // XC(1:2,*)はXCの1,2行目を表す // 数値解を表示する.横軸 t, 縦軸 tau1(t),tau2(t) // mgplot(2,TC,UC); pause; // ファイルに保存 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; { Matrix xp,up,dxp; Matrix q,qd; Matrix d; Matrix J,C,D,G; Matrix calJ(),calC(),calD(),calG(); // X = [[q][qd]]; q = X(1:2,1); qd = X(3:4,1); up = UY; J = calJ(q); // J(q)の計算 C = calC(q,qd); // C(q,qd)の計算 |
D = [[D1,0][0,D2]]; // Dの計算 G = calG(q); // Gの計算 d = Z(2,1); // 外乱dの設定 // 微分方程式 dxp=[[qd][-J~*(C+D*qd+G)]]+[[Z(2,2)][J~]]*(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; Matrix q,qd; // X = [[q][qd]]; q = X(1:2,1); qd = X(3:4,1); // 最後にUYにupの時刻tの値を渡す. up = Z(2,1); // UYにupを代入し,diff_eqsに渡す UY = [up]; } // J(q)の計算を行う関数 Func Matrix calJ(q) Matrix q; { Real th1,th2; th1 = q(1,1); th2 = q(2,1); return [[J1+J2+2*2*m2*l1*l2*cos(th2), J2+2*m2*l1*l2*cos(th2)] [J2+2*m2*l1*l2*cos(th2),J2]]; } // C(q,qd)の計算を行う関数 Func Matrix calC(q,qd) Matrix q,qd; { Real th1,th2,th1d,th2d; th1 = q(1,1); th2 = q(2,1); th1d = qd(1,1); th2d = qd(2,1); return [[-2*m2*l1*l2*(2*th1d*th2d+th2d^2)*sin(th2)] [2*m2*l1*l2*th1d^2*sin(th2)]]; } // Gの計算を行う関数 Func Matrix calG(q) Matrix q; { Real th1,th2; th1 = q(1,1); th2 = q(2,1); return -[[(m1+2*m2)*l1*g*sin(th1)+ m2*l2*g*sin(th1+th2)] [m2*l2*g*sin(th1+th2)]]; } |