差分

この文書の現在のバージョンと選択したバージョンの差分を表示します。

--- ja:manual:additional_function [2014/08/08 18:49]
sugiura
+++ ja:manual:additional_function [2018/04/24 14:25] (現在)
doripoyo [トルク指令の制御プログラム(DLL)]
@@ ライン 62: / ライン 62: @@
 \\
-サーボモータをトルク指令にするために、サーボモータ設定エディタで図13-6のように#17と#18をトルク指令に変更してください。
+サーボモータをトルク指令にするために、サーボモータ設定エディタで図13-6のように#17と#18をトルク指令に変更してください。\\
+{{:ja:manual:add_trq3.png?500|}}\\
 図 13-6　サーボモータをトルク指令に変更\\
+\\
+==== トルク指令の制御プログラム(DLL)====
+トルク制御のモデルを作成でたので、制御プログラムの作り方について解説します。本サンプルではDLL(C++)でプログラムする方法を示します。
+DLLのサンプルプロジェクト一式を掲示板にアップしました（http://techno-road.com/board/view-postlist/forum-9-go-simulation-/topic-141-cdll.html）。ダウンロードしてください。
+main_11TestDll4_basic.cppの、Dll4_Run関数の中を以下のように書き換えてください（testTorqueRef(sim)が実行されるようにする）。
+<code cpp>
+void __stdcall Dll4_Run( DLL_SIM_ROBOT4 *sim) {
+	//Time count up
+	int idt = (int)(sim->dt/0.001);
+	gImsec+=idt;
+	//select a test function
+	//	testAnalogJoypad(sim);
+	//	testMotionDSensor(sim);
+	//	testCamera(sim);
+	testTorqueRef(sim);
+	return;
+}
+</code>
+testTorqueRef()は次のようになっています。\\
+右肩（１７番関節）と左肩（１８番関節）にそれぞれPD制御を施しています。\\
+PF_SetRefMotorTorque(int id, double torque);を使用し、idに関節番号、torqueに指令トルク（力）を設定しています。\\
+<code cpp>
+void testTorqueRef(DLL_SIM_ROBOT4 *sim){
+	if(sim->Axis<=18){
+		return;
+	}
+	int axis;
+	//Sholder Pitch Right Joint(ShoP_R) is set as "Hinge Joint" and "Torque Control Motor"
+	axis = 17; //ShoP_R
+	if(sim->ServoMode[axis]==3)
+	{
+		//トルク制御モードなら
+		static double bx=0;
+		static double ref=0;
+		//angle ref 0 to 120[deg]
+		ref+=0.1;
+		if(ref>120){
+			ref=120;
+		}
+		double x = sim->ServoNowDeg[axis];//angle
+		double v = (x-bx)/sim->dt;//vel
+		//PD Control
+		double kp = 0.04;
+		double kd = 0.001;
+		double f = kp*(ref-x) - kd*v;
+		//output torque
+		SetRefMotorTorque(axis,f);
+		//message
+		wchar_t s[128];
+		swprintf(s,L"ang ref=%lf x=%lf\n",ref,x);
+		sim->PrintCommandWindow(s);
+		bx = x;//back up
+	}
+	//Sholder Pitch Left Joint(ShoP_L) is set as "Slider Joint" and "Force Control Motor"
+	axis = 18; //ShoP_L
+	if(sim->ServoMode[axis]==3)
+	{
+		static double bx=0;
+		static double ref=0;
+		//position ref 0 to 0.3[m]
+		ref+=0.001;
+		if(ref>0.3){
+			ref=0.3;
+		}
+		//double f = 0.1;
+		double x = sim->ServoNowDeg[axis];//position
+		double v = (x-bx)/sim->dt;//vel
+		//PD Control
+		double kp = 0.8;
+		double kd = 0.5;
+		double f = kp*(ref-x) - kd*v;
+		//output torque
+		SetRefMotorTorque(axis,f);
+		//message
+		wchar_t s[128];
+		swprintf(s,L"pos ref=%lf x=%lf\n",ref,x);
+		sim->PrintCommandWindow(s);
+		bx = x;//back up
+	}
+}
+</code>
+それではこのサンプルの動かし方を解説します。
+ビルドして、Releaseフォルダ内に出来た、DllCont4.dllをコピーし、先程つくったロボットプロジェクトのPROGRAMフォルダ下に入れてください。
+そして、コマンドウィンドウから
+<code>
+dll_run(DllCont4)
+</code>
+と入力すると先ほどのDLLが実行されトルク制御が行われるようになります。
+この例では最初に説明したように、\\
+「右肩＝ヒンジ＝トルク指令」\\
+「左肩＝スライダ＝力指令」\\
+になっているので、下図のような結果になり、サンプルでは右肩120[deg]上方、左肩0.3[m]前方 の位置に制御しています。
+{{:ja:manual:add_trq4.png?400|}}
 \\
@@ ライン 77: / ライン 201: @@
 	border:solid #ccc 1px;">
 株式会社テクノロード<br />
-川崎市幸区新川崎7-7　KBIC 222<br />
 <a href="http://techno-road.com/">http:// techno-road.com/</a><br />
 <img src="http://go-simulation.com/wiki/_media/ja/mail.png" />
 </div>
 </html>

Go Simulation! Wiki

ユーザ用ツール

サイト用ツール

差分

ページ用ツール