応答が返ってきた…

NXT2.0

とある事情でノブを上げ下げする動作を自動化したくなったので、何か使えるものはないかってことで試しにLEGOのNXT2.0ってセットを買ってみたまでは良かったんだけど、プログラムから制御しようとしたらとたんに大ハマリしてしまった…orz


まずUSB経由でfantomってドライバ経由で制御しようとしたんだけど、なぜか応答がall 0に見えてしまう。１つだけ応答に何か入ってくるコマンドもあったけど基本all 0でいまいちな感じ。

何が悪いのか分からないので、BT-Micro3E2XってBluetoothドングルを買ってきてXPマシンにぶっ刺したらコントロールパネルに「Bluetooth デバイス」ってのが出現したので、そこからデバイスの追加とかパスキーの入力とかやってNXT2.0を認識させたらCOM8が「発信」、COM9が「着信」で仮想COMポートが出現。

ここもまぁ大丈夫だったんだけど、次がヤバかった。
ちょっとググってみて、シリアルの入出力が普通にファイルI/Oで出来ることを発見して「楽勝だぜー」とか思ってごめんなさい。
最終的にはこんなコードをbcc32に食わせたらバッテリの残量が返ってきたんだけど、かなり時間を使ってしまった。

void a(int port_number)
{
	HANDLE	handle;
	char	name[16];

	sprintf(name, "\\\\.\\COM%d", port_number);

	handle = ::CreateFile(
		name,
		GENERIC_READ | GENERIC_WRITE,
		0,
		NULL,
		OPEN_EXISTING,
		0,
		NULL
	);
	DWORD	api_result;
	api_result = ::GetLastError();
	if(api_result != NO_ERROR){
		// getApiErrorMessage() は自作のエラー文字列化関数(今回は省略)
		printf("CreateFile(%s), %08x, %s\n", name, api_result, getApiErrorMessage(api_result).c_str());
		return;
	}

	unsigned char	command[16] = {
		0x02, 0x00,	/* command size(0x0002) */
		0x00,		/* Direct Command(with response) */
		0x0b		/* GetBatteryLevel */
	};
	DWORD			size;

	::WriteFile(
		handle,
		command,
		4,
		&size,
		NULL
	);

	::FlushFileBuffers(handle);


	unsigned char	response[32] = {0};

	::ReadFile(
		handle,
		response,
		7,//sizeof(response), じゃダメ！
		&size,
		NULL
	);

	printf("%02x%02x%02x%02x",   response[0], response[1], response[2], response[3]);
	printf("%02x%02x%02x%02x\n", response[4], response[5], response[6], response[7]);

	::CloseHandle(handle);
}

まず、ファイル名の「\\.\COMなんとか」ってのではまった。普通に「COM%d」にしちゃうとCOMの番号が１ケタのときは問題なく動くけど、２ケタになったとたんにエラーになっちゃう…
それで「CreateFile COM10」でググったらhttp://support.microsoft.com/kb/115831/jaに「２ケタ以上の場合は「\\.\」をつけろ。てか、１ケタでもつけろ」って書いてあったり。そういうことはAPIのヘルプに書いてほしいよ…最初に試したマシンだと２ケタポートに割り当たって大変だった。

さらにハマったのがReadFile()のサイズ指定。最初はバッファを多めに取ってsizeof(バッファ)にしてたんだけど、これだと一向に応答が返らなくてReadFile()呼び出しから戻ってこなかった。これはいろいろググって最終的に「\\.\COM1 ReadFile」でググったら、http://members.jcom.home.ne.jp/0434383301/vc10.htmに

このモードで設定されている場合、ReadFile（）を呼び出すと指定したサイズの受信が完了するまで戻りません。

って書いてあるみたいで、今回もどうやらバッファサイズ分受信するまで戻ってきてないっぽくて応答メッセージ分のサイズに指定し直したらあっさり返事が返ってきた。
これも同じくAPIのヘルプに明記してほしい。っと思ったけど、よくよくヘルプを見たら

[日本語]
ReadFile 関数は、次のいずれかが成立すると制御を返します。パイプの書き込み側で書き込みが完了するか、指定されたバイト数の読み取りが終わるか、エラーが発生した場合です。
[英語]
The ReadFile function returns when one of the following conditions is met: a write operation completes on the write end of the pipe, the number of bytes requested has been read, or an error occurs.

これの「読み取りが終わる(the number of bytes requested has been read)」って部分は"そ〜ゆ〜意味"なのか！？もしかしなくても。
だあぁ。無駄に疲れてしまった。
将来的にはいろいろコマンド発行したいから毎回ReadFile()の引数にきっちりした値を入れるのは面倒だなぁ。何か回避方法がないか探さないとダメかな。

ちなみにモータを動かすにはcommand[]の中身を

		0x0c, 0x00,
		0x00,	/* Direct Command(with response) */
		0x04,	/* 0:SetOutputState */
		0x00,	/* 1:Port A */
		100,	/* 2:Full Power */
		0x01,	/* 3:MotorOn */
		0x00,	/* 4:REGULATION_MODE_IDLE */
		0,		/* 5:Turn Ratio(straight) */
		0x20,	/* 6:MOTOR_RUN_STATE_RUNNING */
		0x00,	/* 7〜 TachoLimit(run forever) */
		0x00,
		0x00,
		0x00

に変更してWriteFile()のサイズ指定4を14に、ReadFile()のサイズ指定を7から5に変更するとモータが全力で回ってちゃんと応答も返ってきた。
モータを止めるときは「2:Full Power」のところを100から0に、「3:MotorOn」を0x01から0x02(Brake)に、「6:MOTOR_RUN_STATE_RUNNING」を0x20から0x00(MOTOR_RUN_STATE_IDLE)にすると止まった。
このあたりは「NXT SetOutputState」とかでググってたどり着いたこのサイトを参考にした。
http://bearmini.net/blog/View.aspx?bid=1&aid=133


コマンドラインから実行するとこんな感じで。とりあえず「発信」側のポート(今回はCOM8)で良いみたい。むしろ着信のCOM9だと通信確立しなかった。(NXT側のアイコンが変化しない)

C:\Lego>Battery.exe 8
0500020b00c01d

C:\Lego>MotorOn.exe 8
0300020400

C:\Lego>MotorOff.exe 8
0300020400

バッテリーの0xc01dはたぶんリトルエンディアンで0x1DC0(7616)=7.616Vってことかな。eneloop６本で１本当たり1.3Vくらいでだいたいあってそうな感触だし。でも、そもそも応答１バイト目の0x05とか0x03って何だろう。Direct Commandsの仕様書に書いてたっけ？


あと、気になったのはBluetoothの通信確立？にちょっと時間がかかるってことかな。つなぎっぱならもしかしたら問題ないかもしれないけど、今接続−通信−切断ってやると体感で2secくらいかかる気がする。
モータを緊急停止させたい時はNXTを直接電源Offした方が早い(^^;