マインドストームNXTでのSPPを通したコマンドの送受信について書きたいと思います。
マインドストームのコマンド形式はLEGO Mindstorm Bluetooth Developer Kitにある資料に書かれています。コマンドはバイナリ形式で最大64バイト長の命令になっています。
たとえばモータの回転などの出力では0x00 0x04 0x00 0x14 0x01 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 などと１３バイトで記述します。ただし、コマンドをBluetoothで送信する場合にはコマンドの先頭にメッセージの長さを表す２バイトがつき、全体では１５バイトとなります。数値を表すワード長データはリトルエンディアン（Lowバイト Highバイトの順に並ぶ）ので、前述の例では0x0d 0x00が先頭につくことになります。なお、メッセージのバイト長とはバイト長を表す２バイトのデータを除いたものであることに注意が必要です。
NXTからの応答メッセージのバイト数はコマンドにより決まっているので必要なバイト数だけを受信できるようにプログラムを作成します。なお、通信プログラムではどのタイミングでデータエラーとなっても回復が可能であるように、タイムアウト処理やエラー処理をしっかり記述しなければなりません。
プログラムを次に示します。
#define BAUD_RATE 9600
#define BYTE_SIZE 8
#define PARITY NOPARITY
#define STOP_BIT TRUE
#define F_PARITY ONESTOPBIT
bool init(){
hCommT = ::CreateFile(
_T("COM3"),
GENERIC_READ | GENERIC_WRITE,
0,
NULL,
OPEN_EXISTING,
FILE_ATTRIBUTE_NORMAL,
NULL
);
if(hCommT == INVALID_HANDLE_VALUE){
hCommT = ::CreateFile(
_T("COM5"),
GENERIC_READ | GENERIC_WRITE,
0,
NULL,
OPEN_EXISTING,
FILE_ATTRIBUTE_NORMAL,
NULL
);
if(hCommT == INVALID_HANDLE_VALUE){
return false;
}
}
// 通信属性を設定する
DCB dcb;
GetCommState(hCommT, &dcb); /* DCB を取得 */
dcb.BaudRate = BAUD_RATE;
dcb.ByteSize = BYTE_SIZE;
dcb.Parity = PARITY;
dcb.fParity = STOP_BIT;
dcb.StopBits = F_PARITY;
SetCommState(hCommT, &dcb); /* DCB を設定 */
//タイムアウトを設定する。
_COMMTIMEOUTS CommTimeouts;
CommTimeouts.ReadIntervalTimeout=0;
CommTimeouts.ReadTotalTimeoutConstant=500;
CommTimeouts.ReadTotalTimeoutMultiplier=10;
CommTimeouts.WriteTotalTimeoutConstant=500;
CommTimeouts.WriteTotalTimeoutMultiplier=10;
SetCommTimeouts(hCommT,&CommTimeouts);
return true;
};
unsigned long readCom2(unsigned char* sBuf,int cn){
DWORD dwErrors; /* エラー情報 */
COMSTAT ComStat; /* デバイスの状態 */
DWORD dwCount; /* 受信データのバイト数 */
DWORD dwRead; /* ポートから読み出したバイト数 */
ClearCommError(hCommT, &dwErrors, &ComStat);
dwCount = ComStat.cbInQue;
ReadFile(hCommT, sBuf, cn, &dwRead, NULL);
return dwRead;
};
void writeCom(unsigned char* sBuf, int cn){
DWORD nn;
WriteFile(hCommT,sBuf,(DWORD)cn,&nn,NULL);
};
void drive_m(int portnum,int motorpower){
unsigned char tele[64];
unsigned char resu[64];
unsigned long cn;
tele[0]=13; // Length of LSB
tele[1]=0x00; // Length of MSB
tele[2]=0x00; // Direct Command,with response
tele[3]=0x04; // Command
tele[4]=portnum; // Output Port
tele[5]=(char)motorpower; // Power set point
tele[6]=0x01; // Mode byte:MotorON=0x01,Break=0x02,Reguratied=0x04
tele[7]=0x00; // Regulation Mode: IDLE=0x00,Speed=0x01,Sync=0x02
tele[8]=100; // Turn Ratio -100-100
tele[9]=0x20; // Run Stat //Iedle=0x00,RampUp=0x10,Running=0x20,RampDown=0x40
tele[10]=0x00;// Tacho Limit 0:run forever
tele[11]=0x00;// Tacho Limit 0:run forever
tele[12]=0x00;// Tacho Limit 0:run forever
tele[13]=0x00;// Tacho Limit 0:run forever
tele[14]=0x00;// Tacho Limit 0:run forever
writeCom(tele,15);
cn = readCom2(resu,5);
};
// メイン関数の一部
if(init()){
drive_m(1,50); drive_m(2,50);
}
上記の例ではCOM3を開いてみて開けなければCOM5を開いていますが、これは使用している環境に合わせて調整する必要があります。コマンドを送信後、応答メッセージを受信するまでには仕様上３０msec以上は待機することになりますから、受信のタイムアウト時間は十分にとっておく必要があります。
追記10/29 ベタ書きが変なところに挿入されていたため読みにくくなりご迷惑をおかけしておりました。
