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

コントロール	Name	プロパティ名	プロパティ値	機能
Form	AutoMoveNoSave	(Name)	AutoMoveNoSave	作成したwindowの表示
		Text	自律ロボット制御プログラム
		Size	580,500
PictureBox	pictureBox1	Size	400,400	地図の表示
Button	button1	Text	障害物ストップ	障害物があると止まる
	button2		回避1方向	一定方向回避を開始する
	button3		停止	自律移動を停止させる
	button6		５点スキャンマッピング	前方5点の距離測定をして移動させる
	button7		１１点スキャンマッピング	前方11点の距離測定をして移動させる
	button8		行動計画つき	距離データをもとに行動パターンを何種類も変える
Label	roboPosition	(Name)	roboPosition	ロボットの位置座標の表示
Label	roboAngleLabel	(Name)	roboAngleLabel	ロボットの向きを表示

プログラムを書きやすくするために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;

次に、４回目～６回目で実装した下記の関数をコピーしFormクラス内に貼り付けます。

コピーして実装する関数
関数名	紹介箇所	機能説明
GetRangeADData	4回目	PSDセンサーのデータを取得
GetADData		アナログデータを取得
RangeADToConvert		PSDセンサーのAD値を距離データに変換
OneServoMove	5回目	サーボモーターを動かす
servoAngleConvert	5回目	角度をポジションに変換する
MotionPlay	6回目	モーションを再生する
MotionSuspend		モーションをサスペンドする
PgCounterReset		プログラムカウンタのリセット
MotionAddressSet		モーションのアドレスをセットする
MotionRestart		モーションをリスタートする
MySleep		自作Sleep関数
GetConfigData		RCB-4HVのSYSTEMレジスタ値を読み出す

下記プログラムのように、public partial class Form1 : Formの後の中括弧で囲まれたところに貼り付けます。メンバ変数も同様です。

namespace WindowsFormsApplication1
{
    public partial class Form1 : Form
    {
        public Form1()
        {
            InitializeComponent();       
    }
	
    //*************************************************//
    //
    //	Formクラスの関数やメンバ変数などは
    //	public partial class Form1 : Formの中括弧に
    //	囲まれているここの部分に書く
    //
    //*************************************************//
    
    } 
}

namespace WindowsFormsApplication1

{

public partial class Form1 : Form

{

public Form1()

{

InitializeComponent();

}

//*************************************************//

// Formクラスの関数やメンバ変数などは

// public partial class Form1 : Formの中括弧に

// 囲まれているここの部分に書く

//*************************************************//

}

その他描画等に必要な関数をサンプルプログラムからコピーしてFormに貼り付けます。

サンプルプログラムからコピーする関数
関数名	機能説明
RangeDataScan	サーボモーターとPSDセンサーで周囲の距離を取得する
RangeDataScan2	RangeDataScanに距離データを返す機能を付加
PictDrawPoint	pictureBox1の指定した座標に点を打つ
PictDrawLine	pictureBox1の始点の座標と大きさと角度を指定し線を引く
MoveForward	前進モーションを再生し、ロボット座標を変更し、地図に表示も行う
TurnRight	右旋回を行う
TurnLeft	左旋回を行う
RangePointDraw	地図(pictureBox1)に距離データの点を打つ

Form全体で使う変数や規定値などをメンバ変数として宣言するため、以下をコピーし、Formのクラス内に貼り付けます。

#region 各種パラメータ

//前進１モーションの距離
private const double forwardOneSize = 6.6;      
//回転１モーションの角度
private const double turnOneSize = 23.0;        

//ロボット上の距離センサの位置座標（Ｙ軸のみ、Ｘ軸は中心なので０）
private const double rangeXYPoint = 9.0;          

//MAPについてのパラメータ設定
private PointF centerPoint;                     
//点や線を表示させるためのグラフィック変数
private Graphics g;                            

//ピクチャボックスの横が０度なので、縦（９０度）に変更分の値
private const int defaultAngle = 90;            

//ロボットのワールド（絶対）座標系
private PointF robotXY;                        
//ロボットの絶対角度
private double robotAngle;                      

//頭についているサーボのＩＤ
private const int servoHeadSIO = 1;             
private const int servoHeadID = 0;

//SYSTEMデータが必要なので、宣言しておく
private Config config = new Config();

//ストップを知らせるためのフラグ
private bool stopFlag = false; 

#endregion

#region 各種パラメータ

//前進１モーションの距離

private const double forwardOneSize = 6.6;

//回転１モーションの角度

private const double turnOneSize = 23.0;

//ロボット上の距離センサの位置座標（Ｙ軸のみ、Ｘ軸は中心なので０）

private const double rangeXYPoint = 9.0;

//MAPについてのパラメータ設定

private PointF centerPoint;

//点や線を表示させるためのグラフィック変数

private Graphics g;

//ピクチャボックスの横が０度なので、縦（９０度）に変更分の値

private const int defaultAngle = 90;

//ロボットのワールド（絶対）座標系

private PointF robotXY;

//ロボットの絶対角度

private double robotAngle;

//頭についているサーボのＩＤ

private const int servoHeadSIO = 1;

private const int servoHeadID = 0;

//SYSTEMデータが必要なので、宣言しておく

private Config config = new Config();

//ストップを知らせるためのフラグ

private bool stopFlag = false;

#endregion

次に通信部分のプログラムを作成します。本記事3回目を参考にしてserialPort1とFormのLoadイベント関数、FormClosingイベント関数を実装します。serialPort1のプロパティ値は本記事3回目をご覧ください。今回もSYSTEMレジスタ値が必要になりますので、本記事6回目を参考にして、Loadイベント関数内で通信設定が終了した後、GetConfigData関数でSYSTEMレジスタ値を読みます。

最後に、ロボットを停止させるためのボタンを実装します。自律移動で繰り返し動作を行っていますが、止める場合はstopFlag変数を見てtrueの時に止まるようにしました。そのstopFlag変数を変えるタイミングは停止ボタンを押したときです。停止ボタン(button3)のClickイベント関数を実装し、下記プログラムを記述します。

private void button3_Click(object sender, EventArgs e)
{
     stopFlag = true;
}

private void button3_Click(object sender, EventArgs e)

{

stopFlag = true;

}

◆センサデータをもとにモーションを変更する

＊一定距離になると止まる

始めに一定距離になったら止まるプログラムを作成します。距離を計測して、一定距離より離れている場合は前進、一定距離以内になると停止させます。

障害物ストップボタン(button1)のClickイベント関数に以下のプログラムを記述します。このプログラムでは、測定した距離が30cm以内になるとモーション番号1(ホームポジション)を再生し前進をやめます。

private void button1_Click(object sender, EventArgs e)
{           
    MotionPlay(1, 1000);
    //ずっとループを続ける可能性があるので、１０００回くらいでとめる
    for (int i= 0;i&lt;1000 ;i++ )         
    {
        int rangeDataI =  GetRangeADData();
        double rangeDataF =  RangeADToConvert(rangeDataI);     //距離データを変換

        if (0 &lt; rangeDataF &amp;&amp; rangeDataF &lt;= 30) //距離が３０cm以内の時は終わる
        {
            //ホームポジションを再生し、終わる
            MotionPlay(1, 1000);
            break;
        }
        else    //そうでないときは前進
        {
            MotionPlay(13, 4000);
        }
    } 
}

private void button1_Click(object sender, EventArgs e)

{

MotionPlay(1, 1000);

//ずっとループを続ける可能性があるので、１０００回くらいでとめる

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

{

int rangeDataI = GetRangeADData();

double rangeDataF = RangeADToConvert(rangeDataI); //距離データを変換

if (0 < rangeDataF && rangeDataF <= 30) //距離が３０cm以内の時は終わる

{

//ホームポジションを再生し、終わる

MotionPlay(1, 1000);

break;

}

else //そうでないときは前進

{

MotionPlay(13, 4000);

}

実際に動かすと、以下の動画の通りになります。

＊一方向に回避する

距離データをもとにKMR-M6の動作を変化させることができました。次に、もう少し複雑な動きを実装してみましょう。次のプログラムは、前方に障害物があると左に旋回し、回避するプログラムです。障害物がない場合は前進します。回避１方向ボタン(button2)のclickイベント関数に以下のプログラムを実装します。

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

    //ずっとループを続ける可能性があるので、１０００回くらいでとめる
    for (int i = 0; i &lt; 1000; i++)        
    {
        //ストップボタンが押されたときはstopFlagがtrueになる。
        if (stopFlag == true)      
        {            
            stopFlag = false;    //stopFlagの初期化
            MotionPlay(1, 500);  //ホームポジションに戻す
            break;               //ループを抜ける
        }

        int rangeDataI = GetRangeADData();
        //距離データを変換
        double rangeDataF = RangeADToConvert(rangeDataI);

        //距離が３０cm以内の時
        if (0 &lt; rangeDataF &amp;&amp; rangeDataF &lt;= 30) 
        {
            //左旋回を２回する
            MotionPlay(4, 500);
            MotionPlay(4, 500);
        }
        else //そうでないときは前進
        {
            MotionPlay(13, 4000);
        }
    }
}

private void button2_Click(object sender, EventArgs e)

{

MotionPlay(1, 1000);

//ずっとループを続ける可能性があるので、１０００回くらいでとめる

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

{

//ストップボタンが押されたときはstopFlagがtrueになる。

if (stopFlag == true)

{

stopFlag = false; //stopFlagの初期化

MotionPlay(1, 500); //ホームポジションに戻す

break; //ループを抜ける

}

int rangeDataI = GetRangeADData();

//距離データを変換

double rangeDataF = RangeADToConvert(rangeDataI);

//距離が３０cm以内の時

if (0 < rangeDataF && rangeDataF <= 30)

{

//左旋回を２回する

MotionPlay(4, 500);

}

else //そうでないときは前進

{

MotionPlay(13, 4000);

}

実際に動かすと、以下の動画の通りになります。

◆自分の位置を把握する方法

次に、地図を作成するプログラムを作成します。地図を作成するためには、動作中のロボット位置情報が必要です。ロボットの進む前の位置に進んだ距離や方向を足し合わせてフィールド上の位置を求めます。

ここで、ロボットを中心(原点)とした座標をロボット座標、ロボットが移動する平面(フィールド)をワールド座標と呼びます。ロボットを中心とした移動量はロボット座標であり、ロボットの移動量を元にフィールドのどこにいるのか位置を求めるにはロボット座標からワールド座標に変換し、計算する必要があります。回転(旋回)方向は１軸なので、ロボットの向きに関してはそのまま足し合わせることができます。

ロボット座標にあるロボットの移動量をワールド座標に変換するには、下記図を参考にロボットが進んだ距離rとロボットの向きθ_rから求めることができます。

x_r=r * cos θ_r
y_r=r * sin θ_r

進んだ距離をワールド座標に変換することができましたので、移動前の座標(X,Y)から移動後の座標(X',Y')を求める方法は以下のようになります。

X' = X + x_r
Y' = Y + y_r

これらの計算式をプログラムに直すと以下のようになります。C#で使う三角関数は、「度(deg)」ではなく「ラジアン(rad)」を用いるのプログラム中では角度にπ/180°をかけて変換します。

PointF oneSizePoint = new PointF();         //１回あたりのすすむ距離をX,Yまとめて扱う変数
double angleRad = robotAngle / 180 * Math.PI;  //degをradに変換
oneSizePoint.X = (float)(forwardOneSize * Math.Cos(angleRad));  //X軸移動量の計算
oneSizePoint.Y = (float)(forwardOneSize * Math.Sin(angleRad));  //Y軸移動量の計算
//前の移動距離に移動した分を足す
robotXY.X += oneSizePoint.X;
robotXY.Y += oneSizePoint.Y;

PointF oneSizePoint = new PointF(); //１回あたりのすすむ距離をX,Yまとめて扱う変数

double angleRad = robotAngle / 180 * Math.PI; //degをradに変換

oneSizePoint.X = (float)(forwardOneSize * Math.Cos(angleRad)); //X軸移動量の計算

oneSizePoint.Y = (float)(forwardOneSize * Math.Sin(angleRad)); //Y軸移動量の計算

//前の移動距離に移動した分を足す

robotXY.X += oneSizePoint.X;

robotXY.Y += oneSizePoint.Y;

このプログラムを組み込み、初期位置からモーション再生毎に進んだ距離を足し合わせば、ロボット自身の位置を算出する事ができます。サンプルプログラムの前進モーションを行う関数(MoveForward関数)内でそれらの計算を行っています。

◆ロボットの中心とPSDセンサーの位置補正

これまでは、ロボット座標の中心(原点)をロボットの中央にして計算を行ってきましたが、PSDセンサーはロボットの中心にはありません。ロボット前方約9cmの位置に配置されます(下記図)ので、PSDセンサーの座標位置を考慮して計算します。

ロボット座標にあるPSDセンサーの位置座標(X_s,Y_s)をワールド座標に変換するには、ロボットの座標(X,Y)とロボットの向き(θ_r)を用いて以下のように計算します。

X_s = X + 9.0 * cos θ_r
Y_s = Y + 9.0 * sin θ_r

次に、測定した距離データをワールド座標に変換します。

取得した距離データ(d)を元に障害物の位置をワールド座標(X_d,Y_d)に変換するには、PSDセンサーの位置座標(X_s,Y_s)とセンサーを振った角度(θ_d)を用いて以下の式になります。

X_d = X_s + d * cos(θ_r+θ_d)
Y_d = Y_s + d * sin(θ_r+θ_d)

これらの式を使うと、ロボットで取得した距離データを地図に反映することができます。先ほどの距離データから障害物の位置をワールド座標に変換する式をプログラムにすると以下のようになります。このプログラムは地図に障害物をプロットするRangePointDraw関数内に実装しています。

//robotXY     &lt;=ロボットの位置座標(PointF型、メンバ変数として定義)
//robotAngle　&lt;=ロボットの向き(double型、メンバ変数として定義)
//rangeXYPoint&lt;=距離センサの取り付け位置(double型、Y座標のみ)
//rangeAngle  &lt;= 距離センサを回転させた角度(double型)
//rangeData   &lt;= 距離データ(double型)

//距離センサの位置を定義
PointF rangeSensorWorldPoint = new PointF();
double rangeAngleRad = robotAngle / 180 * Math.PI;
//距離センサの位置をワールド座標に変換
rangeSensorWorldPoint.X = (float)(rangeXYPoint * Math.Cos(rangeAngleRad)) + robotXY.X;      
rangeSensorWorldPoint.Y = (float)(rangeXYPoint * Math.Sin(rangeAngleRad)) + robotXY.Y;

//障害物(距離データで取得された)位置の定義
PointF rangeDataWorldPoint = new PointF();
double rangeWorldAngleRad = (robotAngle + rangeAngle) / 180 * Math.PI;        //距離センサの向きをradに変換

rangeDataWorldPoint.X = (float)(rangeData * Math.Cos(rangeWorldAngleRad)) + rangeSensorWorldPoint.X;
rangeDataWorldPoint.Y = (float)(rangeData * Math.Sin(rangeWorldAngleRad)) + rangeSensorWorldPoint.Y;

//robotXY <=ロボットの位置座標(PointF型、メンバ変数として定義)

//robotAngle　<=ロボットの向き(double型、メンバ変数として定義)

//rangeXYPoint<=距離センサの取り付け位置(double型、Y座標のみ)

//rangeAngle <= 距離センサを回転させた角度(double型)

//rangeData <= 距離データ(double型)

//距離センサの位置を定義

PointF rangeSensorWorldPoint = new PointF();

double rangeAngleRad = robotAngle / 180 * Math.PI;

//距離センサの位置をワールド座標に変換

rangeSensorWorldPoint.X = (float)(rangeXYPoint * Math.Cos(rangeAngleRad)) + robotXY.X;

rangeSensorWorldPoint.Y = (float)(rangeXYPoint * Math.Sin(rangeAngleRad)) + robotXY.Y;

//障害物(距離データで取得された)位置の定義

PointF rangeDataWorldPoint = new PointF();

double rangeWorldAngleRad = (robotAngle + rangeAngle) / 180 * Math.PI; //距離センサの向きをradに変換

rangeDataWorldPoint.X = (float)(rangeData * Math.Cos(rangeWorldAngleRad)) + rangeSensorWorldPoint.X;

rangeDataWorldPoint.Y = (float)(rangeData * Math.Sin(rangeWorldAngleRad)) + rangeSensorWorldPoint.Y;

◆地図作成プログラムと複数点の距離を取得したときの違い

PSDセンサーを回転させ、距離を測定すると距離データは地図上にプロットされます。また、ロボットを動作させたあと自動的に現在位置もプロットしていますので、ロボットを動かせば自動的に地図が作成されます。このプログラムを左下図のフィールドで実行したとき作成された地図は右下図のようになりました。


計測フィールド	前方スキャンのみの地図

フィールドは、90cm×180cmです。地図の青い点がロボットの経路、黒い点が距離データをプロットした点です。地図画像の元データは1pxあたり1cmで表示しています。前方の１点しか見ていないので、壁がほとんど見えておらず、地図と呼べるほどではありません。そこで、PSDセンサーがついている頭のサーボモーターを動かし、周囲の5点および11点の距離データを取得するようなプログラムに変更します(下記図)。本記事５回目で計測した方法と同じように中心から角度と計測数を指定して距離を計測します。RangeDataScan関数およびRangeDataScan2関数は全体角度と計測数を渡すと、PSDセンサーついたサーボモータを回転させ周囲の距離を取得します。

5点スキャンマッピングボタン(button6)および11点スキャンマッピングボタン(button7)に次のようにClick関数を実装します。違いは、RangeDataScan関数の計測数のみです。5点スキャンマッピングボタンのときRangeDataScan関数の第１引数は5、11点スキャンマッピングボタンのときRangeDataScan関数の第１引数は11にします。下記はbutton6のClickイベント関数を記述しています。

private void button6_Click(object sender, EventArgs e)
{
    robotXY = new PointF(0, 0);
    robotAngle = 0;             //初期は０にしておく
    MotionPlay(1, 1000);

    for (int i = 0; i &lt; 1000; i++)         //ずっとループを続ける可能性があるので、１０００回くらいでとめる
    {
        //ストップボタンが押されたとき
        if (stopFlag == true)     
        {
            stopFlag = false;
            MotionPlay(1, 500);
            break;
        }

        //距離データを取得(5点スキャン)
        RangeDataScan(5, 90);
        int rangeDataI = GetRangeADData();
        double rangeDataF = RangeADToConvert(rangeDataI);     //距離データを変換

        //距離が30cm以内にある場合
        if (0 &lt; rangeDataF &amp;&amp; rangeDataF &lt;= 30)
        {
            TurnLeft();
            TurnLeft();
        }
        else                            //そうでないときは前進
        {
            MoveForward();
        }
    }
}

private void button6_Click(object sender, EventArgs e)

{

robotXY = new PointF(0, 0);

robotAngle = 0; //初期は０にしておく

MotionPlay(1, 1000);

for (int i = 0; i < 1000; i++) //ずっとループを続ける可能性があるので、１０００回くらいでとめる

{

//ストップボタンが押されたとき

if (stopFlag == true)

{

stopFlag = false;

MotionPlay(1, 500);

break;

}

//距離データを取得(5点スキャン)

RangeDataScan(5, 90);

int rangeDataI = GetRangeADData();

double rangeDataF = RangeADToConvert(rangeDataI); //距離データを変換

//距離が30cm以内にある場合

if (0 < rangeDataF && rangeDataF <= 30)

{

TurnLeft();

}

else //そうでないときは前進

{

MoveForward();

}

実装したプログラムを実行し比較します。以下が作成された地図です。


5点計測地図	11点計測地図

11点計測を行い、地図を作成したほうが障害物(壁)が細かくプロットされていることがわかります。最初の１点のみ計測した場合と比較して明らかに壁があるということがわかる地図になりました。この結果から11点の周囲距離の計測を行う事にします。

◆センサ情報をもとに行動計画を生成

今までは障害物を見つけると左旋回しかしないプログラムでしたが、複数点距離を計測することで、行動の幅が広がります。そこで距離のデータをもとに、左旋回や右旋回など複数の行動パターンでロボットを動かし、より詳細な地図を作るプログラムを作成します。障害物との距離やロボットの向きなどを考慮すると、たくさんの動作パターンがありますので、フローチャート(下記図)を参考に説明していきます。フローチャートで出てくる左右の判別ですが、PSDセンサー付きのサーボモータを左から順に回転させ距離データを取得しています。つまり、計測位置の中央を基準とすると最初のほうに取得した距離データは左側、最後のほうに取得した距離データは右側に位置します。左右一定距離以内に障害物がある場合は、左右それぞれの距離データの最小値を計算し、どちら側に障害物が近いか判断しています。

①距離データを取得する

②両足元に障害物がある場合は８回左旋回をして後ろを向く

周囲の距離を11点で取得しているので、左右どちらとも18cm以内に物があれば、前進することができないと判断して、後ろを向くことにしました。

③片方の足元に障害物がある場合は障害物のない方向に旋回する

左右どちらか18cm以内に障害物がある場合は、障害物がない方向に旋回します。

④前方一定距離(30cm)以内に障害物がある場合はランダムで旋回する

18cm以内に物がない場合で前方30cm以内に物がある場合は前進してもぶつかるため、左右どちらかフィールドが開いているほうにランダムな回数だけ旋回します。左右どちらかフィールドが開いているほうの判別は、左右それぞれ取得した距離データの合計を計算し、結果が大きいほうを開いている側とします。

⑤何もなければ前進する

①で測定した距離データを基に②～⑤の行動を選択します。これらの行動を繰り返します。

以下のように行動計画つきボタン(button8)のClickイベント関数に先ほどのロボットの行動を実装します。

private void button8_Click(object sender, EventArgs e)
{
    robotXY = new PointF(0, 0);
    robotAngle = 0;             //初期は０にしておく
    MotionPlay(1, 1000);

    //それぞれのパラメータを作成
    int scanNum = 11;                   //スキャンする数
    double underFootWidth = 18.0;       //足元の距離設定            

    double[] rangeDataList = new double[scanNum];  //距離を保存しておく配列

    for (int i = 0; i &lt; 1000; i++)         //ずっとループを続ける可能性があるので、１０００回くらいでとめる
    {

        if (stopFlag == true)      //ストップボタンが押されたとき
        {
            stopFlag = false;
            MotionPlay(1, 500);
            break;
        }
                
        ///　距離データを取得
        RangeDataScan2(rangeDataList, 90);
        double rangeDataCenter = rangeDataList[(scanNum / 2) - 1];       //中央のデータを入れておく 

        ///  ＬとＲで一番近い数を入れる変数を用意
        double nearLData = 1000;        //とりあえずありえない数を入れておく
        double nearRData = 1000;        
        double rangeLSum = 0;
        double rangeRSum = 0;

        //右左それぞれの距離の合計と最小値を計算
        for (int j = 0; j &lt; (scanNum / 2); j++)
        {
            int r = j + (scanNum / 2) + 1;  //右の距離データの入っている配列の番号

            //左右それぞれの距離データの合計値を計算
            //合計値を入れるが、負の数の場合は遠い場合なので、80cmを足す
            rangeLSum += rangeDataList[j] &gt; 0 ? rangeDataList[j] : 80;
            rangeRSum += rangeDataList[r] &gt; 0 ? rangeDataList[r] : 80;

            /// 小さい数を入れて最小値を求める（ただし-1が返ってくる場合もあるので負の数は除外）
            if (rangeDataList[j] &gt; 0 &amp;&amp; rangeDataList[j] &lt; nearLData)
            {
                nearLData = rangeDataList[j];
            }
            if (rangeDataList[r] &gt; 0 &amp;&amp; rangeDataList[r] &lt; nearRData)
            {
                nearRData = rangeDataList[r];
            }
        }

        if (nearLData &lt; underFootWidth &amp;&amp; nearRData &lt; underFootWidth)//両足元(underFootWidth以内に)に障害物があった場合
        {
           //後ろを向く
           for (int l = 0; l &lt; 8; l++)
           {
                TurnLeft();
           }
        }
        else if (nearLData &lt; underFootWidth)        //左側に障害物があった場合
        {
           TurnRight();　//右を向く
        }

        else if (nearRData &lt; underFootWidth)        //右側に障害物があった場合
        {
            TurnLeft();     //左を向く
        }
        else
        {
            if (0 &lt; rangeDataCenter &amp;&amp; rangeDataCenter &lt;= 30)   //正面に障害物があった場合                                               
            {
                /// 乱数をもとにモーションを繰り返す
                Random rnd = new Random();
                int randomNumber = rnd.Next(3);
                for (int k = 0; k &lt; randomNumber + 1; k++)
                {
                    if (rangeRSum &gt; rangeLSum)
                    {
                        TurnRight();
                    }
                    else
                    {
                        TurnLeft();
                    }
                }
            }
            else   //障害物がないときは前進
            {
                MoveForward();
            }
       }
    }
}

private void button8_Click(object sender, EventArgs e)

{

robotXY = new PointF(0, 0);

robotAngle = 0; //初期は０にしておく

MotionPlay(1, 1000);

//それぞれのパラメータを作成

int scanNum = 11; //スキャンする数

double underFootWidth = 18.0; //足元の距離設定

double[] rangeDataList = new double[scanNum]; //距離を保存しておく配列

for (int i = 0; i < 1000; i++) //ずっとループを続ける可能性があるので、１０００回くらいでとめる

{

if (stopFlag == true) //ストップボタンが押されたとき

{

stopFlag = false;

MotionPlay(1, 500);

break;

}

///　距離データを取得

RangeDataScan2(rangeDataList, 90);

double rangeDataCenter = rangeDataList[(scanNum / 2) - 1]; //中央のデータを入れておく

/// ＬとＲで一番近い数を入れる変数を用意

double nearLData = 1000; //とりあえずありえない数を入れておく

double nearRData = 1000;

double rangeLSum = 0;

double rangeRSum = 0;

//右左それぞれの距離の合計と最小値を計算

for (int j = 0; j < (scanNum / 2); j++)

{

int r = j + (scanNum / 2) + 1; //右の距離データの入っている配列の番号

//左右それぞれの距離データの合計値を計算

//合計値を入れるが、負の数の場合は遠い場合なので、80cmを足す

rangeLSum += rangeDataList[j] > 0 ? rangeDataList[j] : 80;

rangeRSum += rangeDataList[r] > 0 ? rangeDataList[r] : 80;

/// 小さい数を入れて最小値を求める（ただし-1が返ってくる場合もあるので負の数は除外）

if (rangeDataList[j] > 0 && rangeDataList[j] < nearLData)

{

nearLData = rangeDataList[j];

}

if (rangeDataList[r] > 0 && rangeDataList[r] < nearRData)

{

nearRData = rangeDataList[r];

}

if (nearLData < underFootWidth && nearRData < underFootWidth)//両足元(underFootWidth以内に)に障害物があった場合

{

//後ろを向く

for (int l = 0; l < 8; l++)

{

TurnLeft();

}

else if (nearLData < underFootWidth) //左側に障害物があった場合

{

TurnRight();　//右を向く

}

else if (nearRData < underFootWidth) //右側に障害物があった場合

{

TurnLeft(); //左を向く

}

else

{

if (0 < rangeDataCenter && rangeDataCenter <= 30) //正面に障害物があった場合

{

/// 乱数をもとにモーションを繰り返す

Random rnd = new Random();

int randomNumber = rnd.Next(3);

for (int k = 0; k < randomNumber + 1; k++)

{

if (rangeRSum > rangeLSum)

{

TurnRight();

}

else

{

TurnLeft();

}

else //障害物がないときは前進

{

MoveForward();

}

最初の例よりも少々複雑なL型のフィールドで実行を行った結果、以下のようになりました。上の動画で再生した時の地図が下の動画になります。おおよそのフィールドの形が出来上がっていくことがわかります。

◆まとめ

ある程度限定的ではありますが、お掃除ロボットの動きのまねができるようになりました。作成した地図をもとに、まだロボットが行ったことのない方面に積極的に向かうようなプログラムを作成するなど応用できると思います。

◆サンプルプログラム(フルバージョン)

BMP画像で地図を保存する機能をつけたサンプルプログラムをZIPファイルにて配布します。地図を保存したい場合はこちらのサンプルをお使いください。ただし、説明は省略させていただきます。本連載では、このサンプルプログラムで、地図作成経過を記録し、動画作成に使用しています。

【ダウンロード】AutoMove_Full.zip

◆最後に

KMR-M6をリモートブレインで動かし、お掃除ロボットの行動をまねできるようになりました。リモートブレインで動かせるということで、ほかにも様々な可能性があります。

たとえば、天井カメラなどでロボットの位置を取得し、ロボットを指定した場所に移動させることもできます。

KBT-1接続先を増やすこともできますので、複数台のロボットをPC側のひとつのソフトウェア(プログラム)で同時に動かし、協調作業をさせることも可能です。

読者の皆様も、応用を考えてリモートブレインでロボットを動かしてみてはいかがでしょうか？

今まで７回にわたり読んでいただきありがとうございました。

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

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

キーワード検索