アームロボット研修 ブログ

ロボットアーム研修(まえけん編) その3 ~ソフトウェアの構成~

アームロボット研修

こんにちは
まえけんです。

今週はCRANE-X7の開発をすすめる上でどのようなソフトウェア構成にしようかというのを考えてみます。
以前の二足歩行ロボット研修ではうまくソフトウェアを分けることが出来ずに読みにくいコードになってしまいました。
今回は最後まで開発しやすくしたいので、最初の段階で少し時間を取ろうと思います。

お試しでDynamixelSDKのサンプルプログラムであるbulk_read_write.cppを改造していきます。

ソフトウェアの構成は以下の図のようにしました。

Dynamixelとの通信機能を担うDxl_robotクラスを作る。
それを継承したCraneX7クラスを作るという構成にしようと思います。
CraneX7クラスには軸数やリンク長など実際のロボットの物理的なパラメータや通信ポートなどの設定が記述されます。
この構成にした理由として、アーム研修の後に二足研修が控えているというのもあります。
出来れば通信系の部分は流用出来るようにしたいなと思っています。

また、キー入力を取得などの機能は別ファイルで管理しておくでmain文を見やすくしておきました。

ファイルを分けたことによって、Makefileも修正が必要になります。
最初はbulk_read_write.cppしかコンパイルしないようになっているので、Makefileの51行目を次のように修正します。

修正前


48    #---------------------------------------------------------------------
49    # Files
50    #---------------------------------------------------------------------
51    SOURCES = bulk_read_write.cpp \

修正後↓


48    #---------------------------------------------------------------------
49    # Files
50    #---------------------------------------------------------------------
51    SOURCES = bulk_read_write.cpp func.cpp crane_x7.cpp dxl_robot.cpp\

最終的に以下のようなコードになりました。
修正後のbule_read_writeの一部抜粋


int main(){
	//CRANE-X7クラスを作成
	CR7 cr(DEVICENAME,2.0);
	//トルクディセーブル
	cr.truque_disable();
	//位置制御モードに変更
	cr.init_position_mode();
	//トルクON
	cr.truque_enable();
	
	while(1)
	{
		//ゴールポジション(角度)をサーボに書き込める形に変形
		for (int cnt = 0; cnt < JOINT_NUM; cnt++){
			cr.set_joint_goal[cnt] = cr.deg2value(goal_pose[cnt]);
		}
		//ゴールポジションの書き込み
		cr.bulkwrite(ID, ARRAY_LENGTH(ID), ADDR_GOAL_POSITION, LEN_GOAL_POSITION, cr.set_joint_goal);
		//キー入力があるまで待つ
		printf("Press any key to continue! (or press ESC to quit!)\n");
		if (getch() == ESC_ASCII_VALUE)
			break;
		//サーボからパラメータの呼び出し
		cr.bulkread(ID, ARRAY_LENGTH(ID), ADDR_GOAL_POSITION, LEN_GOAL_POSITION,cr.get_joint_goal);
		cr.bulkread(ID, ARRAY_LENGTH(ID), ADDR_PRESENT_POSITION, LEN_PRESENT_POSITION,cr.joint_pos);
		//現在地と目標値の表示
		for (int cnt = 0; cnt < JOINT_NUM; cnt++)
		{
			printf("ID[%d] pos:%d\n", ID[cnt], cr.joint_pos[cnt]);
			printf("ID[%d] joint_goal:%ld\n", ID[cnt], cr.get_joint_goal[cnt]);
		}
	}
	printf("\nend\n");
	return 0;
}

さて、開発に必要な環境は整ったので次回からはさっそく教科書に沿ってプログラムをしていきたいと思います!
まずは順運動学を解くところからになります。
来週もよろしくおねがいします!

タイトルとURLをコピーしました