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

Posted on 2012.05.14 in

５回目：サーボモーター単体を動かし、周囲の距離を測定する

◆前回のおさらい

前回、RCB-4HVに入力したアナログデータをPCのプログラムで取り込む方法を説明しました。また、アナログ出力のPSDセンサー(距離センサー)を取り付けて、データを取得し、距離データに変換しました。

さて、前回用いたPSDセンサーは、前方１点のみしか取得することができません。自律で移動するロボットは周辺の障害物を検知するため、広範囲の距離データを取得する必要があります。

そこで今回は、PSDセンサーをサーボモーターに取り付け、左右にセンサーを振り、周辺の距離データを取得する仕組みを紹介します。

◆前記事

１回目：導入

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

３回目：RCB4ライブラリの紹介と使い方

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

◆サーボモーターにPSDセンサーを取り付ける

PSDセンサーを左右に振るために、サーボモーターにPSDセンサーを取り付けます。

まず、サーボモーターのニュートラルを設定します。軸のマークが目盛りに対し一直線上になるようにまわしてください。

PSDセンサーとサーボホーンは両面テープで固定します。ロボット用のサーボホーンでは凹凸がありつけにくいので、ラジコン用のサーボホーン(スティアホーン)を用います。

サーボモーターの軸にPSDセンサー付きのサーボホーンを取り付けます。

◆サーボモーターの設定と接続

サーボモーターを以下のように設定します。KRS-2552HVを設定するにはICS3.0対応シリアルマネージャーVer1.51を用います。

	設定値
通信速度	115200bps
ID	0

サーボモーターはRCB-4HVのSIO4ポートに接続します。RCB-4HV基板の内側に信号線がくるように挿します。

◆HeartToHeart4を用いてサーボモーターの割り当て

HeartToHeart4（以降HTH4）プロジェクトを使ってRCB-4HVにサーボモーターを登録します。HTH4を立ち上げ、KMR-M6_Sample(V2.0)プロジェクトを読み込みます。次に、プロジェクト設定画面のサーボモーター設定タブを開きます。

左側のSIO1,2,3,4のリストの中に、SIO1_00という名前があります。SIO1_00をサーボモーターの設定画面にドラッグアンドドロップをすると、モーションのプロジェクトにSIO1_00のサーボモーターが追加されます。

設定したデータを「ROMにすべて保存」をクリックしてROMに書き込みます。プロジェクトファイルの保存もしてください。

◆Windowsのプログラムを作成する

Windows上でサーボモーターを動かすプログラムを作成します。サーボモーターを動かしたあとにPSDセンサーとコラボレーションして周囲の距離を測定するプログラムも作成しますので、先に必要なコントロールをすべてFormに貼り付けます。PSDセンサーも使いますので、前回作成したFormを流用し、コントロールを貼り付けます。

サーボモーターを動かすためには、TrackBarを使い、周囲を測定した距離データをグラフィックで表現するためにPictureBoxを使います。

それぞれのプロパティは以下のとおりに設定します。

プロパティの設定値
コントロール名	プロパティ名	プロパティ値	内容
textBox1	設定特になし		PSDセンサ値の表示
button1	Text	距離センサ	PSDセンサのデータ取得ボタン
trackBar1	Maximum	11500	サーボモーターの値の設定
	Minimum	3500
	Value	7500
textBox2	設定特になし		ポジションの表示
pictureBox1	Size	200,100	周囲距離取得したときの表示
button2	Text	周囲距離	周囲距離取得開始ボタン

サーボモーターを動かすための関数を作成します。以下は、RCB-4HVに接続しているサーボモーターのIDを指定し、フレーム数とポジション(指令値)をRCB-4HVに送ります。関数名は「OneServoMove」としました。OneServoMove関数の引数は以下の通りです。

第1引数:SIO番号

RCB-4HVに接続しているSIOポートの番号を以下のように指定します。

SIO1,2,3,4 =>1

SIO5,6,7,8 =>2

第2引数:サーボID

サーボモーターに設定されているID番号を指定します。

第3引数:フレームサイズ

フレームサイズを1～255で指定します。値が小さいほど速く回ります。

第4引数:サーボポジション

サーボモーターのポジション(指令値)を設定します。

最小:3500　　（-135°）

ニュートラル: 7500 (0°)

最大:11500 (+135°)

OneServoMove関数内で使っているRCB4.Command.RunSingleServoはサーボモーターを動かすためのコマンドを作成する関数です。

private void OneServoMove(int sioNum, int servoID,int frameSize, int servoPos)
{
    ByteList cmd = new ByteList();
    Byte[] rx = new Byte[4];
　　
　　//ICS番号を作成する
    byte sendID = (byte)(servoID * 2 + (sioNum - 1));

    //コマンドを生成する
    cmd.Bytes = Command.RunSingleServo(sendID, (byte)frameSize, servoPos);

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

private void OneServoMove(int sioNum, int servoID,int frameSize, int servoPos)

{

ByteList cmd = new ByteList();

Byte[] rx = new Byte[4];

　　//ICS番号を作成する

byte sendID = (byte)(servoID * 2 + (sioNum - 1));

//コマンドを生成する

cmd.Bytes = Command.RunSingleServo(sendID, (byte)frameSize, servoPos);

//コマンドを送る

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

}

ICS対応のサーボモーターは、7500を指定すると0°、3500は-135°、11500は+135°に対応しています。角度指定してサーボモーター動かすために、角度をポジションに変換する関数を用意しました。下記関数は、引数に±135度の角度(double型)を渡すと、ポジション(int型)が返ってきます。

private int servoAngleConvert(double angle)
{
    //7500のとき0deg            
    int neutral = 7500;       
    //±135deg =&gt;±4000 
    int servoAngle = neutral + (int)((angle / 135.0) * 4000);
    
    //サーボモーター最大角、最小角の設定
    //3500より小さいときは3500にする
    if (servoAngle &lt; 3500) servoAngle = 3500;      
    //11500より大きいときは11500にする
    if (servoAngle &gt; 11500) servoAngle = 11500;     
    return servoAngle; 
}

private int servoAngleConvert(double angle)

{

//7500のとき0deg

int neutral = 7500;

//±135deg =>±4000

int servoAngle = neutral + (int)((angle / 135.0) * 4000);

//サーボモーター最大角、最小角の設定

//3500より小さいときは3500にする

if (servoAngle < 3500) servoAngle = 3500;

//11500より大きいときは11500にする

if (servoAngle > 11500) servoAngle = 11500;

return servoAngle;

}

サーボモーターへポジションを送るタイミングは、トラックバーを動かしたとき発生するScrollイベントを用います。Scrollイベント関数内は以下のように記述します。Scrollイベントが発生すると、OneServoMove関数を呼び、トラックバーの値をサーボモーターに送信します。

private void trackBar1_Scroll(object sender, EventArgs e)
{
    //SIO1の０番のサーボモーターを動かす
    OneServoMove(1, 0,1,trackBar1.Value);
}

private void trackBar1_Scroll(object sender, EventArgs e)

{

//SIO1の０番のサーボモーターを動かす

OneServoMove(1, 0,1,trackBar1.Value);

}

この状態でプログラムを実行すると、トラックバーと連動してサーボモーターが動くのが確認できます。

◆サーボモーターとPSDセンサーのコラボレーション

サーボモーターの角度制御ができるようになったので、サーボモーターに取り付けたPSDセンサーとコラボレーションさせて周囲の距離を取得します。サーボモーターのニュートラルを中心に首を振るようにします。

周囲の距離を取得する手順は以下のようにしました。

サーボモーターの動かす範囲とデータの取得数、１回の動く角度を設定する。
サーボモーターの角度を変更する。
サーボモーターが動き終わるまで待つ。
PSDセンサーの値を取得する。
PSDセンサーの値を保存し、画面上に表示する。
２番目に戻りデータの取得回数繰り返す
最後にサーボモーターをニュートラルに戻す

ボタンがクリックされたときに上記の動作を実行します。button2のClickイベントを実装し、Clickイベント関数を以下のように記述します。

private void button2_Click(object sender, EventArgs e)
{
    Graphics g = pictureBox1.CreateGraphics();
    //ピクチャーボックス内を白く塗りつぶす
    g.FillRectangle(Brushes.White, 0, 0, pictureBox1.Width, pictureBox1.Height);    

    const int rangeNum = 51;                 //取得する距離データの数
    const double angleWidth =90.0;      //サーボモーターの回転角度の設定
    double oneAngleData =angleWidth/( rangeNum-1);      //１回あたりのサーボの移動角度

    //距離データを保存しておく配列を確保   
    double[] rangeData = new double[rangeNum];      

    //サーボモータをoneAngleDataステップでrangeNum回くりかえす
    for (int i = 0; i &lt; rangeNum; i++)
    {
        //測定するサーボモーターの角度指定
        float angle =(float)( -(angleWidth / 2) + (i * oneAngleData)); 
        int servoAngel = servoAngleConvert(angle);
        
        //サーボモーターを回す
        OneServoMove(1, 0, 1, servoAngel);

        //動き終わるまで待つ
        System.Threading.Thread.Sleep(200);
        
        //PSDセンサー値の取得
        int rangeDataI = GetRangeADData();
        rangeData[i] = RangeADToConvert(rangeDataI);
                
        //ピクチャーボックスに表示する
        PictDrawLine(new PointF(0,0),angle,(float)rangeData[i]);
    }
    
    //最後は原点にもどる        
    OneServoMove(1, 0, 1, servoAngleConvert(0));       
}

private void button2_Click(object sender, EventArgs e)

{

Graphics g = pictureBox1.CreateGraphics();

//ピクチャーボックス内を白く塗りつぶす

g.FillRectangle(Brushes.White, 0, 0, pictureBox1.Width, pictureBox1.Height);

const int rangeNum = 51; //取得する距離データの数

const double angleWidth =90.0; //サーボモーターの回転角度の設定

double oneAngleData =angleWidth/( rangeNum-1); //１回あたりのサーボの移動角度

//距離データを保存しておく配列を確保

double[] rangeData = new double[rangeNum];

//サーボモータをoneAngleDataステップでrangeNum回くりかえす

for (int i = 0; i < rangeNum; i++)

{

//測定するサーボモーターの角度指定

float angle =(float)( -(angleWidth / 2) + (i * oneAngleData));

int servoAngel = servoAngleConvert(angle);

//サーボモーターを回す

OneServoMove(1, 0, 1, servoAngel);

//動き終わるまで待つ

System.Threading.Thread.Sleep(200);

//PSDセンサー値の取得

int rangeDataI = GetRangeADData();

rangeData[i] = RangeADToConvert(rangeDataI);

//ピクチャーボックスに表示する

PictDrawLine(new PointF(0,0),angle,(float)rangeData[i]);

}

//最後は原点にもどる

OneServoMove(1, 0, 1, servoAngleConvert(0));

}

PictDrawLine関数は、PicturBox1に指定した原点から角度と長さを指定して線を引く関数です。以下の関数をコピーしてFormのソースコードに貼り付けてください。

private void PictDrawLine(PointF startPoint, float angle, float range)
{
    Graphics g = pictureBox1.CreateGraphics();      //ピクチャーボックス内に線が引けるようにする
            
    int pointSize = 2;

    PointF endPoint = new PointF();

    PointF centerPoint = new PointF(pictureBox1.Width/2,pictureBox1.Height / 2);

    startPoint.X += centerPoint.X;      //０点（中心）に合わせる
    startPoint.Y += centerPoint.Y;

    int defaultAngle = 90;
    //degをradに変換
    float angleRad = (angle - defaultAngle) / 180 * (float)Math.PI;

    //終点座標の計算
    endPoint.X = range * (float)Math.Cos(angleRad) + startPoint.X;
    endPoint.Y = range * (float)Math.Sin(angleRad) + startPoint.Y;

    //線を引く
    g.DrawLine(new Pen(Color.WhiteSmoke,2), startPoint, endPoint);
    //距離の点を表示
    g.FillEllipse(Brushes.Brown, endPoint.X - (pointSize / 2), endPoint.Y - (pointSize / 2)
                  , pointSize, pointSize);
    //中心座標を表示
    g.FillEllipse(Brushes.ForestGreen, startPoint.X - (pointSize / 2), startPoint.Y - (pointSize / 2)
                  , pointSize, pointSize);

}

private void PictDrawLine(PointF startPoint, float angle, float range)

{

Graphics g = pictureBox1.CreateGraphics(); //ピクチャーボックス内に線が引けるようにする

int pointSize = 2;

PointF endPoint = new PointF();

PointF centerPoint = new PointF(pictureBox1.Width/2,pictureBox1.Height / 2);

startPoint.X += centerPoint.X; //０点（中心）に合わせる

startPoint.Y += centerPoint.Y;

int defaultAngle = 90;

//degをradに変換

float angleRad = (angle - defaultAngle) / 180 * (float)Math.PI;

//終点座標の計算

endPoint.X = range * (float)Math.Cos(angleRad) + startPoint.X;

endPoint.Y = range * (float)Math.Sin(angleRad) + startPoint.Y;

//線を引く

g.DrawLine(new Pen(Color.WhiteSmoke,2), startPoint, endPoint);

//距離の点を表示

g.FillEllipse(Brushes.Brown, endPoint.X - (pointSize / 2), endPoint.Y - (pointSize / 2)

, pointSize, pointSize);

//中心座標を表示

g.FillEllipse(Brushes.ForestGreen, startPoint.X - (pointSize / 2), startPoint.Y - (pointSize / 2)

, pointSize, pointSize);

}

プログラムを実行させ、周囲距離ボタンを押すとPSDセンサーが回りながら距離を測定し、データが描画されます。

下図は、30cm先に壁があるとき、周囲測定を行った実行画面です。計測範囲は±45°（全体90°）で中心を含め角度を51分割し距離計測しました。左側は障害物なし、右側は手前に空き缶を置いたときの図です。測定結果から、空き缶とまではわかりませんが障害物があることを検知できることがわかります。


壁のみ周囲計測	壁＋空き缶の周囲計測

◆サンプルプログラム

参考にしていただくために、今まで３回から５回分のプログラムをまとめたものをzipファイルに圧縮して配布します。

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

◆次回予告

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

サーボモーターとPSDセンサーの組み合わせにより周囲の距離を計測することができるようになりました。それにより周囲の距離や障害物を検知できるようにもなりました。

次回はロボットのモーションを再生させて、ロボット本体が移動するプログラムを作成します。

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をリモートブレインで動かす(5回目)

５回目：サーボモーター単体を動かし、周囲の距離を測定する

キーワード検索