トップ > 製品情報 > 研究用ロボット > RTコンポーネント学習キット Beauto Rovert RTC-BT ver.2（ビュートローバー・アールティーシー） Step5
RTコンポーネント学習キット Beauto Rovert RTC-BT ver.2（ビュートローバー・アールティーシー）

STEP 5-1サンプル：ライントレースを行なうRTコンポーネントプログラムの作成

それでは、ここまでの学習内容のまとめとして、RTコンポーネントからロボットにライントレースをさせてみましょう。基本的な設計は、センサ出力やモータ入力などCPUボードを制御するための入出力ポートを備えたRTコンポーネント、及び、それと接続してロボットにライントレースの動作をさせるRTコンポーネントの二つのプログラムを作成します。

↑ページトップへ

STEP 5-2CPUボード入出力側のコンポーネント設計

まず、CPUボードの基本制御用のRTコンポーネントを作成します。こちらは他のRTコンポーネントなどからも扱いやすいように、CPUボードの生の情報を入出力するだけの汎用的な仕様としたいと思います。

それでは、どのようなポートを備えると良いでしょうか。まず、ロボットにライントレースさせるためには、最低限「センサ情報の出力」及び「モータ制御の入力（ゲイン及びスピード）」が必要です。では、その通りに入出力ポートを追加しましょう。

Input側のポート設定

Output側のポート設定

プロジェクトの設定が完了したら、いつも通りVisual Studioからビルドできる状態まで進めてください。

↑ページトップへ

STEP 5-3CPUボード入出力側のコンポーネント設計（Bluetooth利用時）

Bluetooth通信モジュール「VS-BT003」を利用してロボットを無線操縦している場合は、ここでシリアルポート番号をコンフィギュレーション変数で設定してみましょう。（VS-BT003を利用していない方は次に進んでください）。

Bluetoothのシリアルポート番号を表す変数として「BT_port」を宣言しています。ここでは、プログラムの開発時の環境に従って、デフォルト値を44にしていますが、各自の環境に合わせてこの数値を適時変更してください。

なお、次に説明するonExecute関数でも、これにあわせて記述を変更する必要があります。

↑ページトップへ

STEP 5-4CPUボード入出力側のonExecute関数

続いてCPUボード側のonExecute関数を設定したいと思います。今回の関数では、入出力ポートの内容に変化が発生した場合のみCPUボードと通信するようにしたいと思います。

RTC::ReturnCode_t VSWRC003LV_CTRL::onExecute(RTC::UniqueId ec_id)
{
  //ロボットとの通信状態を示すフラグ
  static BOOL init_flg=TRUE;
  {
    //最初の実行、及び接続が途切れた直後の実行であればロボットとの接続を行う
    if(init_flg){
      CWRC_Connect();
      init_flg=FALSE;
    }

    //ロボットの動作モード
    if(m_ModeIn.isNew()){
      m_ModeIn.read();
      CWRC_WriteMemMap(4,(BYTE) m_Mode.data);
      CWRC_WriteMemMap(5,(BYTE) (m_Mode.data>>8));
      printf("Mode=%d \n",m_Mode.data);
    }

    //ロボットのモータゲイン
    if(m_Mt1GainIn.isNew()){
      m_Mt1GainIn.read();
      CWRC_WriteMemMap(8,(BYTE) m_Mt1Gain.data);
      printf("Gain1=%d \n",m_Mt1Gain.data);
    }
    if(m_Mt2GainIn.isNew()){
      m_Mt2GainIn.read();
      CWRC_WriteMemMap(9,(BYTE) m_Mt2Gain.data);
      printf("Gain2=%d \n",m_Mt2Gain.data);
    }

    //ロボットのモータスピード
    if(m_Mt1SpdIn.isNew()){
      m_Mt1SpdIn.read();
      CWRC_WriteMemMap(14,(BYTE) (m_Mt1Spd.data>>8));
      CWRC_WriteMemMap(15,(BYTE) m_Mt1Spd.data);
      printf("Spd1=%d \n",m_Mt1Spd.data);
    }
    if(m_Mt2SpdIn.isNew()){
      m_Mt2SpdIn.read();
      CWRC_WriteMemMap(16,(BYTE) (m_Mt2Spd.data>>8));
      CWRC_WriteMemMap(17,(BYTE) m_Mt2Spd.data);
      printf("Spd2=%d \n",m_Mt2Spd.data);
    }

    //データの送信。エラーが出たら次回の再接続のため、フラグにFALSEを代入
    if(!CWRC_WriteExecute(FALSE)){
      CWRC_Disconnect();
      init_flg=TRUE;
    }


    //ロボットから赤外線センサの情報を取得
    CWRC_ReadMemMap(0x1a,4);

    //データの受信。エラーが出たら次回の再接続のため、フラグにFALSEを代入
    if(CWRC_ReadExecute()){

      //受信したデータを画面に表示し、ポートから出力
      m_AN1.data = wrc_mem_uw(0x1a);
      m_AN2.data = wrc_mem_uw(0x1c);
      m_AN1Out.write();
      m_AN2Out.write();

      printf("Ain1=%04x , Ain2=%04x\n",wrc_mem_uw(0x1c),wrc_mem_uw(0x1a));
    }
    else{
      CWRC_Disconnect();
      init_flg=TRUE;
    }

  }
  return RTC::RTC_OK;
}

なお、VS-BT003で通信を行うために先ほどコンフィギュレーション変数を設定した場合は、以下のようにソースを一部書き換えてください（太字部分が書き換え箇所）

    //最初の実行、及び接続が途切れた直後の実行であればロボットとの接続を行う
    if(init_flg){
      CWRC_Connect_BT(m_BT_port);
       init_flg=FALSE;
    }

↑ページトップへ

STEP 5-5ライントレース制御側のコンポーネント設計

続いて、ロボットにライントレースの動作をさせるRTコンポーネントを作成します。

入出力ポートは、CPUボード制御側のプログラムに合わせて設計します。

Input側の設定

Output側の設定

また、RT System Editor上での動作時にロボットのパラメータを柔軟に変更できるように、コンフィギュレーション変数をいくつか設定したいと思います。ライントレースプログラムで、動作環境に応じて頻繁に変更する数値としては「モータゲイン（モータ速度を制御）」「センサ閾値（ライン検出）」「Mode（ロボットの駆動・停止制御）」が挙げられます。

コンフィギュレーション変数の設定は「コンフィギュレーション」のタブで行います。今回は下図のように合計6つの変数を追加します。

「MotorGainL」「MotorGainR」は、左右のモータのゲインを調整する変数です。この数値が大きいほどモータのスピードが大きくなります。左右のモータで速度差がある場合などに、調整用に利用できます。「CPUMode」はCPUボードの動作モードを設定します。1にすればモータが動作し、0にすればモータが停止します。「SensorThL」「SensorThR」は、ロボットの左右赤外線センサがラインに反応する閾値です。ちなみに、今回のプログラムでは、ラインは黒系統の色が前提です。「Speed」はモータの回転速度です。左右のモータの誤差はゲインで調整するので、Speedは両方のモータの速度をまとめて設定する役割になります。

全ての変数の型は「int」に設定し（直接テキストを入力します）、デフォルトの数値は上図と同じ値に設定してください。

設定が完了したらVisual Studioでビルドできるところまで進めてください。

続いてonExecute関数を下記のように変更します。


RTC::ReturnCode_t LINETRACE_CTRL::onExecute(RTC::UniqueId ec_id)
{
  //モード情報の取得と出力
  if(m_Mode.data!=m_CPUMode){
    m_Mode.data = m_CPUMode;
    m_ModeOut.write();
    printf("Mode=%d\n",m_Mode.data);
  }

  //モータゲイン情報情報の取得と出力
  if(m_Gain1.data!=m_MotorGainL){
    m_Gain1.data = m_MotorGainL;
    m_Gain1Out.write();
    printf("Gain1=%d\t",m_Gain1.data);
  }

  if(m_Gain2.data!=m_MotorGainR){
    m_Gain2.data = m_MotorGainR;
    m_Gain2Out.write();
    printf("Gain2=%d\n",m_Gain2.data);
  }


  //センサ情報の取得
  if(m_Sensor1In.isNew()) m_Sensor1In.read();
  if(m_Sensor2In.isNew()) m_Sensor2In.read();
  printf("Sensor1=%d , Sensor2=%d\n",m_Sensor1.data,m_Sensor2.data);

  //モータ速度の決定
  //「直進」を基準として、センサがラインを見つけたら、見つけた方の車輪を停止
  {
    short pre_spd_l,pre_spd_r;
    m_Motor1.data = m_Speed;
    m_Motor2.data = -m_Speed;

    //左センサがラインを見つけた場合
    if(m_Sensor2.data>m_SensorThL) m_Motor1.data = 0;
    //右センサがラインを見つけた場合
    if(m_Sensor1.data>m_SensorThR) m_Motor2.data = 0;

    printf("Spd1=%d , Spd2=%d\n",m_Motor1.data,m_Motor2.data);
    //モータ速度をCPUボード制御側のRTコンポーネントへ出力
    if(pre_spd_l!=m_Motor1.data){
      m_Motor1Out.write();
      pre_spd_l=m_Motor1.data;
    }
    if(pre_spd_r!=m_Motor2.data){
      m_Motor2Out.write();
      pre_spd_r=m_Motor2.data;
    }
  }
  return RTC::RTC_OK;
}

前半では、動作モード、モータゲイン、センサ情報の各情報をCPUボードに出力しています。

続いてセンサの数値を読み込み、その結果によって左右のモータの駆動・停止を切り替えています。具体的には「センサの数値がSensorGain1（又は2）より大きい=センサがラインを見つけている」という場合に、左右でラインを見つけた側のモータを停止させています。「ラインを見つけたセンサを停止させる」ことで、ロボットがライントレース出来るという原理は、下の図をご参照ください。

↑ページトップへ

STEP 5-6プログラムの実行

では、作成したプログラムを実行してみたいと思います。まずは、これまでと同様にRT System Editorにコンポーネントを追加して、それぞれの入出力ポートを接続してください。ちなみに、接続のラインが重なって見づらい場合は、下図のようにマウスでドラッグすると見やすくなります。

次に「All Acvitate」をクリックしてシステムを実行してください。すると、プログラムが動き出して画面にテキストも表示されますが、ロボットはライントレースのコースに置いてみてもタイヤが動きません。これは、ロボットの動作モードを設定するコンフィギュレーション変数が0に設定されているためです。「LINETRACE_CTRL」で設定されたコンフィギュレーション変数「CPUMode」は、初期値の0が「Mode」のポートから出力され、「WRC003_CTRL」の「Mode」のポートに入力されているので、ロボットに送られる動作モードの数値は0になります。

では、RT System Editor上からコンフィギュレーション変数「CPUMode」を書き換えてロボットのモータを動作させてみたいと思います。まず「LINETRACE_CTRL」のブロックをクリックしてください。クリックするとウィンドウ下側の「Configuration View」に、「LINETRACE_CTRL」に備わっている全てのコンフィギュレーション変数とその現在値が表示されます。それでは、CPUModeの数値「0」をクリックして「1」に書き換え、「適用」のボタンをクリックしてください。クリックして表示される確認ダイアログは「OK」をクリックしてください。

スピードを速くすると、通信速度に制御が追いつかずラインを外れやすくなるのでご注意ください。

ここまで、RTコンポーネントの作成について、非常に簡単な入門レベルですが一通りの開発が出来るようになりました。この他にもRTコンポーネントには様々な機能（データの転送手段やデータタイプなど）が備わっていますが、本教材とこれらの基本サンプルを元に改造していくことで、高度な制御や複雑なシステムも構築できます。

OpenRTM-aistのwebページには、様々なフォーラムや技術情報が逐次公開されているので、こちらも参照しながら、RTコンポーネントの開発を是非習得してください。

↑ページトップへ