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

Posted on 2012.05.10 in

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

◆前回のおさらい

前回は、RCB-4HVの仕組みとRCB4ライブラリの紹介をしました。また、VisualStudioで作成したプログラムでRCB-4HVと接続をしました。

今回は、VisualStudioで作成したプログラムからRCB-4HVに接続されているアナログセンサーのデータを取得します。また、PSDセンサー(距離センサー)を接続し、取得したアナログデータを距離データに変更します。

◆前記事

１回目：導入

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

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

◆RCB-4HVに入力されるアナログデータの処理方法

RCB-4HVはアナログデータを10個入力することができます。入力されたアナログデータ(0～5Ｖ)を1024分割(10bit)に区切って取得しています。また、RCB-4HVは１周期ごとにアナログポート(ADポート)からデータを読み取りRAMの特定の場所に保存しています。したがって、アナログデータが必要な時は、RAMに保存してあるデータを読めばよいわけです。

◆VisualStudioでRCB-4HVのアナログデータを読みに行く方法

RCB-4HVのコマンドはByte型の配列を扱う必要があり、直接コマンドを作ったり保存するのがやや面倒です。RCB4ライブラリでは、コマンドを保存するByte型の配列を簡単に扱えるようにしたByteListクラスを利用します。

まず、今回のテーマである「アナログデータの読み取り」を例に基本的なクラスの使い方を説明します。

アナログデータをRCB-4HVから取得するには、RCB4クラスのRCB4.AdReadという関数を下記のように用います。

    Extensions.Collections.ByteList cmd = new ByteList();      //コマンド用にリストを確保
    cmd.Bytes = Rcb4.Rcb4.AdRead(1);    //AD1につながっているデータを取りに行くコマンド生成

1 2	Extensions.Collections.ByteList cmd = new ByteList(); //コマンド用にリストを確保 cmd.Bytes = Rcb4.Rcb4.AdRead(1); //AD1につながっているデータを取りに行くコマンド生成

RCB4.ADRead関数は、指定したADポートからアナログデータを取得するコマンド(Byte型の配列)を返します。そのByte型の配列を保存するにはByteListクラスを用います。

１行目では、コマンドを保存するByteLiseクラスを定義（インスタンス化）し、２行目でこのクラスにコマンドを保存しています。

Extensions.Collections.ByteListはよく使うので、そのたびにExtensions.Collections.ByteListを書くのは面倒です。そこで、プログラム上部に以下の強調された部分を追加をします。

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;

using Extensions.Collections;を記述したので、プログラムのExtensions.Collections部分を省略することができます。usingを使うと下のように記述が少なくてすみます。RCB4クラスも同様にさまざまなところで使うので記述をしておきます。

    ByteList cmd = new ByteList();      //コマンド用にリストを確保
    cmd.Bytes = Rcb4.Rcb4.AdRead(1);    //AD1につながっているデータを取りに行くコマンド生成

1 2	ByteList cmd = new ByteList(); //コマンド用にリストを確保 cmd.Bytes = Rcb4.Rcb4.AdRead(1); //AD1につながっているデータを取りに行くコマンド生成

これでコマンドが生成できました。次にコマンドを送って、アナログデータを受け取る関数を記述します。新規作成されたプログラムは以下のようになっており、特に記載がない場合、「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の中括弧に

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

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

}

先ほどの中括弧で囲まれた部分にアナログデータを受け取る以下のプログラム(関数)を書きます。関数名はGetADDataにしました。

private int GetADData()
{
    //コマンド用にリストを確保        
    ByteList cmd = new ByteList();     

    //ＡＤの受け取りは5byte
    Byte[] rx = new Byte[5];
            
    //AD1につながっているデータを取りに行くコマンド生成
    cmd.Bytes = Rcb4.Rcb4.AdRead(1);    
    
    //コマンドを送って受信する
    if (Command.Synchronize(serialPort1, cmd.Bytes, ref rx) == false)   
    {
        //失敗した場合は、
        MessageBox.Show("読み取りに失敗しました　");
        //textBox1.Text = " ";
        return -1;
    }
    else
    {
        //成功した場合
        //AD値を一度byte配列に代入
        byte[] ADData = { rx[2], rx[3] };
        //int型に変換（正確には2byteなのでshout型）   
        int data = BitConverter.ToInt16(ADData, 0);
　　　　return data;    
    }
    
}

private int GetADData()

{

//コマンド用にリストを確保

ByteList cmd = new ByteList();

//ＡＤの受け取りは5byte

Byte[] rx = new Byte[5];

//AD1につながっているデータを取りに行くコマンド生成

cmd.Bytes = Rcb4.Rcb4.AdRead(1);

//コマンドを送って受信する

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

{

//失敗した場合は、

MessageBox.Show("読み取りに失敗しました　");

//textBox1.Text = " ";

return -1;

}

else

{

//成功した場合

//AD値を一度byte配列に代入

byte[] ADData = { rx[2], rx[3] };

//int型に変換（正確には2byteなのでshout型）

int data = BitConverter.ToInt16(ADData, 0);

　　　　return data;

}

コマンドを送るときは、Rcb4.Command.Synchronize(serialPort1, cmd.Bytes, ref rx)をもちいます。

第1引数：通信速度などが設定済みのSerialPort。今回は、前回のFormに張り付けたSerialPort1クラスをそのまま指定します

第2引数：送信コマンドが入っているByte型の配列

第3引数：送った後に受信されるByte型の配列(参照型)

戻り値：通信の合否で、通信に失敗した場合Falseになります。

Command.Synchronizeが正しく実行されると、rxの配列には受信データが入りますので、Byte型の配列を5byte分用意しておきます。

ADポート読み取りコマンド受信データ構成
1	2	3	4	5
Size	CMD	DAT_1	DAT_2	Sum

受信したデータの2番目と3番目の配列にAD変換された10bit(0～1023)が8bitずつ分かれて入っています。このデータをBitConverterクラスのToInt16関数でint型に変換しています。

これで、この関数を呼ぶとAD1ポートのデータが返ってきます。

◆PSDセンサーについて

さて、アナログ出力のPSDセンサーについて説明します。PSDセンサーは距離に応じて出力端子電圧が変化します。弊社で販売しているPSDセンサーはSHARPのGP2Y0A21YK0Fモジュールを使用しています。

このセンサーの有効範囲は、10cm～80cmで、距離と出力電圧は下図のような関係になります。この出力電圧値を読み取り、距離データに変換します。今回は、先にPSDセンサーを接続しているRCB-4HVのADポートの値と距離を測定し、変換式を作ります。

PSDセンサーの電圧-距離グラフ

◆PSDセンサーとRCB-4HVの接続方法

AD1ポートにPSDセンサーを取り付けます。白い配線(信号線)をRCB-4HVの内側に向けます。

◆プログラムによるアナログデータの取得

先ほどのGetADData関数を用いれば、アナログデータを取得することができますが、この距離センサーは少しノイズが多いので複数回データを取得し平均をとります。下記プログラムはAD1ポートを10回読み取り平均値を返す関数です。関数名は、「GetRangeADData」にしました。

private int GetRangeADData()
{
    //平均とる回数
    const int getSensorNum = 10;
    //データの総和を保存する変数
    long dataSum = 0;

    //getSensorNum回分データを取得し、足し合わせる
    for (int i = 0; i &lt; getSensorNum; i++)
    {
        dataSum += GetADData();
    }

    //平均値を返す
    return (int)(dataSum / getSensorNum);
}

private int GetRangeADData()

{

//平均とる回数

const int getSensorNum = 10;

//データの総和を保存する変数

long dataSum = 0;

//getSensorNum回分データを取得し、足し合わせる

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

{

dataSum += GetADData();

}

//平均値を返す

return (int)(dataSum / getSensorNum);

}

◆PSDセンサーデータ表示プログラムの作成

次に、読み取ったデータをフォームに表示できるようにします。前回作成したフォームにデータを表示させるテキストボックスとコマンドを送るボタンを追加します。ツールボックスからButtonとTextboxをフォームにドラッグアンドドロップします。Buttonの名前はbutton1、Textboxの名前はtextBox1とします。

button1の表示名を変更します。button1のプロパティにTextがありますので、その右欄を「データ取得」と入力します。

次に、ボタンを押したときにコマンドを送るために、Clickイベント関数を作成します。前回と同様にボタンのプロパティを表示し、雷マークをクリックしてイベント一覧を表示します。アクション欄から「Click」を選び、その右の空欄をダブルクリックしてClickイベント関数を作成します。

作成されたClickイベント関数は下記のように記述します。

private void button1_Click(object sender, EventArgs e)
{
    int rangeData = GetRangeADData();       //アナログデータを取得する
    textBox1.Text = rangeData.ToString();   //アナログデータを文字データに変換してテキストボックスに表示する
}

private void button1_Click(object sender, EventArgs e)

{

int rangeData = GetRangeADData(); //アナログデータを取得する

textBox1.Text = rangeData.ToString(); //アナログデータを文字データに変換してテキストボックスに表示する

} 　　

textBox1.Textに文字を入れるとその文字がtextBox1に表示されます。センサーデータを表示するので、センサーデータを文字列データ(string型)に変換してtextBox1のTextプロパティに代入しています。

プログラムを書き終えたら実行してみましょう。

データ取得ボタンを押すと、アナログデータがテキストボックスに表示されます。距離センサーの前に障害物になるような物をかざして、値が変化することを確認します。

◆取得データと距離の関係について

＊実測によるデータの取得

PSDセンサーのアナログデータを取れるようになりましたが、この時点ではまだ距離のデータではありません。そこで、RCB-4HVで読み取ったデータと距離を実測して関係性を調べます。

距離とRCB-4HVのデータを以下図のように計測します。

実際、計測を行った結果以下のようになりました。

PSDセンサ値と距離の比較
Ａ/Ｄ値	距離(cm)	距離の逆数（1/x (cm)）	Ａ/Ｄ値	距離(cm)	距離の逆数（1/x (cm)）
496	10	0.1000	143	40	0.0250
421	12	0.0830	136	42	0.0238
365	14	0.0714	132	44	0.0227
321	16	0.0625	126	46	0.0217
288	18	0.0555	123	48	0.0208
267	20	0.0500	119	50	0.0200
244	22	0.0454	114	52	0.0192
228	24	0.0416	111	54	0.0185
209	26	0.0384	107	56	0.0178
197	28	0.0357	103	58	0.0172
190	30	0.0333	100	60	0.0166
178	32	0.0312	96	65	0.0153
164	34	0.0294	88	70	0.0142
151	36	0.0277	85	75	0.0133
147	38	0.0263	83	80	0.0125

＊変換式の算出

Microsoft社のエクセルを用いて計測したアナログ変換値から距離に換算する式を作成します。先ほどのPSDセンサーの電圧-距離グラフありましたが、距離をそのまま用いるときれいな近似直線ができません。そこで、距離の逆数とPSDセンサ―値の関係のグラフを参考にします(下記図）。縦軸はPSDセンサの出力電圧で、横軸は距離を逆数にした値です。距離の逆数とPSDセンサ―値のグラフを見ると、10cm～80cm間ではほぼ直線になりそうです。

PSDセンサーの電圧と距離の逆数したものとの関係

エクセルにPSDセンサ値と距離測定をしたデータを入力します。入力したデータを縦軸が距離の逆数、横軸がPSDセンサーのデータになるようにグラフ(散布図)を作成します。作成されたグラフの表示部分を右クリックすると、「近似曲線の追加」があります。近似曲線を追加すると、その近似曲線の式を出力します。

上記の図は近似曲線(１次の近似曲線)を追加したグラフです。作成されたグラフを見ると、ほぼ近似直線上にデータがあるので、この近似式(y=0.0002x-0.0048)を採用します。

PSDセンサーのデータを距離(cm)に変換する関数は、以下のようになります。関数名は、RangeADToConvertです。

private Double RangeADToConvert(int ADData)
{
    //エクセルで算出した式を用いる
    double distanceData = 0.0002 * (double)ADData - 0.0048;     
    //距離は逆数で計算されるので、元に戻す
    distanceData = 1 / distanceData;

    //範囲外にデータがあるときは-1を設定する
    if (distanceData &lt; 10 || 80 &lt; distanceData)
    {
        distanceData = -1;
    }
    return distanceData;    
}

private Double RangeADToConvert(int ADData)

{

//エクセルで算出した式を用いる

double distanceData = 0.0002 * (double)ADData - 0.0048;

//距離は逆数で計算されるので、元に戻す

distanceData = 1 / distanceData;

//範囲外にデータがあるときは-1を設定する

if (distanceData < 10 || 80 < distanceData)

{

distanceData = -1;

}

return distanceData;

}

RangeADToConvertの引数に先ほどのRCB-4HVから取り込んだデータ(GetRangeADData関数の戻り値)を渡せば距離をcm単位で取得することができます。計算値が10cm~80cmでない場合はエラーとして-1を返すようにしました。

さて、PSDセンサーのデータを距離データに変換するプログラムをフォームに組み込んでみましょう。アナログデータを読み取るプログラムで作成したClickイベント関数を以下のように変更し実行します。

private void button1_Click(object sender, EventArgs e)
{
    //アナログデータを取得する
    int rangeData = GetRangeADData();      

    //PSDのデータを距離データに変換する
    double rangeDataD = RangeADToConvert(rangeData);

    //距離データをテキストボックスに入れる
    textBox1.Text = rangeDataD.ToString();   
}

private void button1_Click(object sender, EventArgs e)

{

//アナログデータを取得する

int rangeData = GetRangeADData();

//PSDのデータを距離データに変換する

double rangeDataD = RangeADToConvert(rangeData);

//距離データをテキストボックスに入れる

textBox1.Text = rangeDataD.ToString();

}

センサーの前に障害物がない場合は-1が表示され、80cm以内に障害物があるとそこまでの距離が表示されます。

◆次回予告

５回目：広範囲の距離データを取得する

PCで作成したプログラムでロボットに搭載した距離データを取得できるようになりました。ただし、PSDセンサーは前方の狭い範囲しかとることができません。そこで次回は、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をリモートブレインで動かす(4回目)

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

キーワード検索