KMR-M6をリモートブレインで動かす(6回目)

Posted on 2012.05.17 in

６回目：モーションを再生させ、ロボットを移動させる

◆前回のおさらい

前回、サーボモーター単体を動かし、PSDセンサーとコラボレーションさせ周囲の距離測定を行いました。ですが、お掃除ロボットの真似をするには、ロボット自身が動き回りながら距離を測る必要があります。

今回は、PCからロボットに保存されているモーションを再生させる仕組みについて説明します。

◆前記事

1回目:導入

2回目:お掃除ロボットの動きを再現させるための下準備

3回目:RCB4ライブラリの紹介と使い方

4回目:アナログデータを取り込み距離データを取得する

5回目:サーボ単体を動かし、周囲の距離を測定する

◆モーションの作成と登録

ロボットを移動させるには、サーボモータを複数同時に制御する必要がありますが、それぞれの動きをプログラムで作成するには少々困難です。弊社では、一連の動作を１つのパッケージ(モーション）として保存しておき、再生させる方法を用いています。そのモーションの作成や管理を行うソフト「HeartToHeart4(以降HTH4)」を公開しています。また、KMR-M6のサンプルモーションも公開しておりますので、このサンプルモーションを流用しながら、ロボットをリモートブレインで動かします。

HTH4の詳しい使い方については、取扱説明書をご覧ください。

まずHTH4を起動し、前回サーボを追加したプロジェクトを書き込みます。あとで保存されているモーションを再生するのにモーション番号が必要ですので、モーション番号とモーション内容を控えておきます。プロジェクトを書き込むには、プロジェクト設定画面の「ROMにすべて保存」をクリックします。

サンプルモーション以外にも、オリジナルモーションもPCのプログラムから再生させることができます。今回、ゆっくりと安定した前進モーションを使いますので、自作したモーション(M6-13_前進（ゆっくり・カウンタ2歩）)を13番のモーションデータに書き込みました。このモーションデータをZIPファイルで公開しますので、必ずモーション番号13番に書き込んでご利用ください。

ゆっくり前進モーションデータ:M6-13_前進（ゆっくり・カウンタ2歩）

◆プログラムによるモーションの再生方法

*Windowsプログラムの作成

さて、簡単にモーションを再生させてみます。まず、VisualStudioで「Windows フォームアプリケーション」プロジェクトを新規で作成したら、第３回目の通信設定のプログラムを実装してください。Formには下記図のようにモーションの再生を開始するボタンを一つ貼り付け、Clickイベントを作成します。

また、プログラムを書きやすくするために4回目で説明いたしました「using ~ 」を追加します。

using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;

using Extensions.Collections;
using Rcb4;

using System;

using System.Collections.Generic;

using System.ComponentModel;

using System.Data;

using System.Drawing;

using System.Linq;

using System.Text;

using System.Windows.Forms;

using Extensions.Collections;

using Rcb4;

次に、Clickイベント内は以下のようにプログラムを実装します。MotionPlay関数については後々に説明し実装しますので、空の関数を用意しておきます。例としてボタンが押されると、MotionPlay関数にモーション番号１とモーション再生完了時間１秒(1000ms)が送られます。

private void button1_Click(object sender, EventArgs e)
{
    MotionPlay(1, 1000);
}

//関数を実装しないとエラーが出るので、空のMotionPlay関数を実装する
private void MotionPlay(int motionNum,int waitTime)
{

}

private void button1_Click(object sender, EventArgs e)

{

MotionPlay(1, 1000);

}

//関数を実装しないとエラーが出るので、空のMotionPlay関数を実装する

private void MotionPlay(int motionNum,int waitTime)

{

}

*モーション再生の手順とモーション再生関数の作成

モーションを再生するには４つの手順が必要です。

①再生しているモーションを中断(サスペンド)する

②プログラムカウンタのリセット

③モーションのアドレスをセット

④モーションをリスタートする

以下、それぞれについて説明します。

①再生しているモーションを中断(サスペンド)する

RCB-4HVはROMの中でコマンドを待ち続けながらモーションをたえず再生しています。再生中、次に動かすモーションを指定するには、RCB-4HVのSYSTEMレジスタを使っていったんモーションを中断する必要があります。SYSTEMレジスタについてはファームウェアリファレンスの４ページをご覧ください。SYSTEMレジスタを扱うためには、プログラム上でRCB4.Configクラスを使います。GetConfigData関数は、SYSTEMレジスタRCB4-HVから読み出す関数です。SYSTEMデータは他でも使いますので、Formのメンバ変数(インスタンス変数)として宣言しておきます。

//全体で使うので、メンバ変数として宣言
private Config config = new Config();

///コンフィグデータを読み込む関数
private void GetConfigData()
{
    ByteList cmd = new ByteList();
    byte[] rx = new byte[5];
   
    //コマンドの生成
    cmd.Bytes = Command.MoveRamToCom(Rcb4.Rcb4.ConfigRamAddress, 2);
    if (Command.Synchronize(serialPort1, cmd.Bytes, ref rx) == true)
    {
        //読み込んだデータをConfigクラスに保存する
        config[0] = rx[2];
        config[1] = rx[3];
    }
}

//全体で使うので、メンバ変数として宣言

private Config config = new Config();

///コンフィグデータを読み込む関数

private void GetConfigData()

{

ByteList cmd = new ByteList();

byte[] rx = new byte[5];

//コマンドの生成

cmd.Bytes = Command.MoveRamToCom(Rcb4.Rcb4.ConfigRamAddress, 2);

if (Command.Synchronize(serialPort1, cmd.Bytes, ref rx) == true)

{

//読み込んだデータをConfigクラスに保存する

config[0] = rx[2];

config[1] = rx[3];

}

SYSTEMレジスタはむやみに変えると動作がおかしくなってしまうので、通信が確立した時点でSYSTEMデータを呼び出します。FormのLoadイベント内の通信の設定後GetConfigData関数を呼びます。

private void Form1_Load(object sender, EventArgs e)
{
    try
    {
        //COMポートを開く
        serialPort1.Open();             
    }
    catch
    {
        //失敗したらエラーを出して終わる
        MessageBox.Show("COMが開けません！！");
        serialPort1.Close();
        this.Close();
    }

    //COMが開くことができたらconfigを呼ぶ
    GetConfigData();
}

private void Form1_Load(object sender, EventArgs e)

{

try

{

//COMポートを開く

serialPort1.Open();

}

catch

{

//失敗したらエラーを出して終わる

MessageBox.Show("COMが開けません！！");

serialPort1.Close();

this.Close();

}

//COMが開くことができたらconfigを呼ぶ

GetConfigData();

}

RCB4.Suspend関数は、RCB-4HVから読み出したSYSTEMレジスタ(Config)を変更してモーションをサスペンドするコマンドを作ります。MotionSuspend関数では、一時停止設定されたConfigデータをRCB-4HVに送信し、実際にモーションを一時停止させます。

private void MotionSuspend()
{
    ByteList cmd =new ByteList();
    Byte[] rx = new  Byte[4];       //読み取るために必要だが特に使わない

    //コマンドの生成
    cmd.Bytes = Rcb4.Rcb4.Suspend(config);

    //コマンドの送信
    Command.Synchronize(serialPort1,cmd.Bytes,ref rx);
}

private void MotionSuspend()

{

ByteList cmd =new ByteList();

Byte[] rx = new Byte[4]; //読み取るために必要だが特に使わない

//コマンドの生成

cmd.Bytes = Rcb4.Rcb4.Suspend(config);

//コマンドの送信

Command.Synchronize(serialPort1,cmd.Bytes,ref rx);

}

②プログラムカウンタのリセット

プログラムカウンタは、プログラムがどこまで実行されているかを示すアドレス(場所)を保存しています。COMポートから命令を送ってモーションを再生させる前に、カウンタをリセットする必要があります。以下のPgCounterReset関数は、RCB-4HVのプログラムカウンタをリセットする関数です。

private void PgCounterReset()
{
    ByteList cmd = new ByteList();
    Byte[] rx = new Byte[4];       //読み取るために必要だが特に使わない
           
    //コマンドの生成
    cmd.Bytes = Rcb4.Rcb4.ResetProgramCounter(Rcb4.Rcb4.MainLoopCmd);
            
    //コマンドの送信
    Command.Synchronize(serialPort1, cmd.Bytes, ref rx);
}

private void PgCounterReset()

{

ByteList cmd = new ByteList();

Byte[] rx = new Byte[4]; //読み取るために必要だが特に使わない

//コマンドの生成

cmd.Bytes = Rcb4.Rcb4.ResetProgramCounter(Rcb4.Rcb4.MainLoopCmd);

//コマンドの送信

Command.Synchronize(serialPort1, cmd.Bytes, ref rx);

}

③モーションのアドレスをセット

モーションを再生させるためには、モーションを保存しているアドレス(場所)を指定する必要があります。以下のMotionAddressSet関数は、次に再生したいモーション番号からモーションアドレス計算し、RCB-4HVにセットする関数です。モーション番号からモーションの先頭アドレスを計算する式は、

モーションアドレス = (モーション番号 -1)×エリア１個分のデータ数

　　　　　　　　　　　+ モーションの書き込みエリアの先頭アドレス

モーション番号 :motionNum

エリア１個分のデータ数 : Rcb4.Rcb4.MotionSingleDataCount (予約済)

モーション書き込みエリアの先頭アドレス : Rcb4.Rcb4.MotionRomAddress (予約済)

となります。

private void MotionAddressSet(int motionNum)
{
    ByteList cmd = new ByteList();
    Byte[] rx = new Byte[4];       //読み取るために必要だが、特に使わない

    //モーションのアドレスは＝モーションの先頭アドレス＋モーション番号＊１モーション１つあたりの容量
    int address = (motionNum - 1) * Rcb4.Rcb4.MotionSingleDataCount + Rcb4.Rcb4.MotionRomAddress;

    //コマンドの生成
    cmd.Bytes = Rcb4.Rcb4.Play(address);

    //コマンドの送信
    Command.Synchronize(serialPort1, cmd.Bytes, ref rx);
}

private void MotionAddressSet(int motionNum)

{

ByteList cmd = new ByteList();

Byte[] rx = new Byte[4]; //読み取るために必要だが、特に使わない

//モーションのアドレスは＝モーションの先頭アドレス＋モーション番号＊１モーション１つあたりの容量

int address = (motionNum - 1) * Rcb4.Rcb4.MotionSingleDataCount + Rcb4.Rcb4.MotionRomAddress;

//コマンドの生成

cmd.Bytes = Rcb4.Rcb4.Play(address);

//コマンドの送信

Command.Synchronize(serialPort1, cmd.Bytes, ref rx);

}

④モーションをリスタートする

最後にセットしたモーションをスタートさせます。以下のMotionRestart関数は、RCB-4HVにモーションをリスタートさせるコマンドを送る関数です。

private void MotionRestart()
{
    ByteList cmd = new ByteList();
    Byte[] rx = new Byte[4];       //読み取るために必要だが、特に使わない

    //コマンドの生成
    cmd.Bytes = Rcb4.Rcb4.Resume(config);

    //コマンドの送信
    Command.Synchronize(serialPort1, cmd.Bytes, ref rx);
}

private void MotionRestart()

{

ByteList cmd = new ByteList();

Byte[] rx = new Byte[4]; //読み取るために必要だが、特に使わない

//コマンドの生成

cmd.Bytes = Rcb4.Rcb4.Resume(config);

//コマンドの送信

Command.Synchronize(serialPort1, cmd.Bytes, ref rx);

}

以上の手順を順番に行うとプログラムで指定したモーションが再生されます。この手順は常に同じなので、簡単に実行できるようにMotionPlay関数を作成します。最初に空のMotionPlay関数を作成しましたので、下記プログラムを追加してください。コマンドの送信が終わったら、モーションの再生が終わるまでMySleep関数で待ちます。

private void MotionPlay(int motionNum,int waitTime)
{
    //モーションを一時中断する（サスペンド）
    MotionSuspend();
    //プログラムカウンタとＥＥＰＲＯＭ更新フラグリセット(プログラムカウンタをリセット）
    PgCounterReset();
    //コール命令で移動先アドレスをセット
    MotionAddressSet(motionNum);
    //モーションをリスタート
    MotionRestart();

    //モーションが終わるまで待つ
    MySleep(waitTime);    
 }

private void MotionPlay(int motionNum,int waitTime)

{

//モーションを一時中断する（サスペンド）

MotionSuspend();

//プログラムカウンタとＥＥＰＲＯＭ更新フラグリセット(プログラムカウンタをリセット）

PgCounterReset();

//コール命令で移動先アドレスをセット

MotionAddressSet(motionNum);

//モーションをリスタート

MotionRestart();

//モーションが終わるまで待つ

MySleep(waitTime);

}

何もせず単にプログラムを停止させるにはSleep関数を用いますが、実行中は他のボタンを押すなどのイベント処理関数が発生しません。そこで、イベント処理の発生を検知するMySleep関数を用意しました。引数は時間指定でミリ秒(ms)で指定します。

private void MySleep(int ms)
{
    System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();

    sw.Start();
    
    //待っている最中はイベントがあるかどうか見に行く
    for (; ; )
    {
        if (sw.ElapsedMilliseconds &gt; ms)
        {
            break;
        }
        Application.DoEvents();
     }
     sw.Stop();
     sw.Reset();
}

private void MySleep(int ms)

{

System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();

sw.Start();

//待っている最中はイベントがあるかどうか見に行く

for (; ; )

{

if (sw.ElapsedMilliseconds > ms)

{

break;

}

Application.DoEvents();

}

sw.Stop();

sw.Reset();

}

◆モーションの連続再生

モーションを連続で再生させるには、何回もそのモーションを呼びだします。clickイベント内を以下に変更することで、連続再生できます。例として、1番のモーションを再生し、13番のモーションを３回再生し、最後1番のモーションを再生しています。

private void button1_Click(object sender, EventArgs e)
{
    MotionPlay(1, 1000);
      
    for (int j = 0; j &lt; 3; j++)
    {
        MotionPlay(13, 4000);
    }
            
    MotionPlay(1, 1000);
}

private void button1_Click(object sender, EventArgs e)

{

MotionPlay(1, 1000);

for (int j = 0; j < 3; j++)

{

MotionPlay(13, 4000);

}

MotionPlay(1, 1000);

}

もし、MotionPlay関数で再生が終わる時間を適切に設定しないと、前のモーションが終わる前に次のモーションを再生させようとするため正常にモーションが再生できません。再生時間は適切な時間を入れておきます。時間が適切でないと以下の動画のような動きをします。

◆移動モーションの移動量測定

KMR-M6がお掃除ロボットの真似をするには、地図を作成し、ロボット自身がどの位置にいるかを把握する必要があります。ロボット自身の位置を求める方法は、ロボットの方向と進んだ距離をもとに最初の位置から足し合わせていきます。この方法については次回詳しく説明いたします。今回はその準備としてKMR-M6の前進モーションと左右の旋回モーションの１回あたりの移動量を計測します。

＊前進の移動距離測定

前進モーションは13番のモーション(M6-13_前進(ゆっくり・カウンタ2歩))を用いました。下記図のようにKMR-M6の下に定規を置き、モーションを再生させ、モーション完了時の距離を測定します。

１回のモーションでは、細かい数値まで計測することができないので、10回連続でモーションを再生させ１回あたりの移動距離を計測します。測定結果は以下のようになりました。

前進モーション移動距離
	移動距離(cm)
連続10回モーションを３回行った平均距離	66.0
１モーションの距離	6.6

結果から、前進モーション１回あたりの移動量は6.6cmとなりました。

＊旋回角度の測定

次に、KMR-M6の旋回角度の計測を行います。前進モーションと同じように、１回のモーションでは細かい数値が計測できないので、10回連続でモーションを再生させ旋回角度を計測します。右旋回モーションは、サンプルモーションに含まれている「M6-05_旋回（右・無線）」(5番のモーション)、左旋回は「M6-04_旋回（左・無線）」(4番のモーション)を用います。計測は以下図のように初期地点に印をつけ、旋回させ、終点に印をつけてその角度を分度器で計測しました

測定結果は以下のようになりました。

旋回モーション角度測定
	左(4番)モーション(度)	右(5番)モーション（度）
10回連続モーションを３回行ったの平均角度	230	230
１モーションの旋回角度	23	23

旋回モーション角度は左右両方とも23度となりました。

◆まとめ

Windowsのプログラムからモーションの再生コマンドを発行し、ロボットを動かすことができるようになりました。今回、前進と旋回モーションの再生しか行いませんでしたが、他のモーションの再生も可能です。モーションを組み合わせて、KMR-M6にダンスをさせるなどの応用をしてみてはいかがでしょうか？

◆サンプルプログラム

今回作成したプログラムをサンプルとして配布します。ZIPでまとめてありますので、解凍してお使いください。

【ダウンロード】KMR-M6_MotionPlay.zip

◆次回予告

7回目:お掃除ロボットのように距離データをもとに考えて行動する

3回目～6回目にわたり、プログラムで各機能を動かすことを行ってきました。

次回はいよいよ最終回。今までのプログラムを組み合わせ、１回目で紹介したお掃除ロボットの動きをするプログラムの作成を行います。

KHR-3HV Ver.3 リフェバッテリー付きセットの詳細をみる KMR-M6 Ver.3 リフェバッテリー付きセットの詳細をみる KHR-3HV Ver.2 リフェバッテリー付きセットの詳細をみる KHR-3HV Ver.2の詳細をみる KHR-3HV Ver.2 セレクトパックの詳細をみる KMR-M6 Ver.2 リフェバッテリー付きセットの詳細をみるカメ型ロボット02　Ver.1.5の詳細をみる KMR-P4 Ver.1.5 リフェバッテリー付きセットの詳細をみる BluetoothモジュールKBT-1の詳細をみる

キーワード検索

KMR-M6をリモートブレインで動かす(6回目)

６回目：モーションを再生させ、ロボットを移動させる

キーワード検索