ロボット：ビーチフラッグに挑戦

ロボコンマガジン2009年7月号の記事をみて、近藤科学のサイトからビーチフラッグのサンプルプログラムを入手しました。（ココからダウンロード）

サンプルは受信センサが２個装着されている事が前提となっていましたが作成したフォークリフトには１個しかついていません。そこでプログラムを変更して１個で旋回しながら送信センサ位置を探すようにしました。具体的には、旋回しながら受信センサを読み取り、現在の測定値と直前の測定値を利用して送信センサの位置を探すように変更しました。

【最初のバージョン】

とりあえず送信センサには近づくようになりました。２ｍ以内なら送信センサを検出できる様子です。ただ、探査時は常に右旋回で、サンプルにあった左右のセンサ値（今回は現在の測定値と直前の測定値）によって旋回方向を切り替える処理には入らないように見えました。そこで、その部分は削除して直前の測定値が範囲内で現在の測定値がLOST値の時に逆旋回するようにしました。また、受信センサが右端にあると送信センサの正面で止まれないため写真のように真ん中に移動しました。

【次のバージョン】

受信センサを真ん中にしたので正面で停止するようになりましたが、相変わらず探査時に逆旋回しませんでした。片方だけが範囲外になる確率が低いのだと思います。そこで、探査終了時の直進が始まる前に次の旋回は逆になるよう旋回方向を指定する変数を追加する事にしました。

【本日の最終版】

フォークリフトは後輪が１つしかないので直進性が悪いです。右旋回後だと右へ、左旋回後だと左へ流れて進む事が多いです。ただ、送信センサをロストして再探査する際は必ず逆旋回して探査するようになったので無駄な旋回が減り、短時間で送信センサに近づくようになりました。本日はここまでとします。

＜本日のサンプルプログラム＞

/***********************************************************************/
/*                                                                     */
/*  FILE        :forklift.c                                            */
/*  DATE        :Thu, Jun 11, 2009                                     */
/*  DESCRIPTION :main program file.                                    */
/*  CPU GROUP   :27                                                    */
/*                                                                     */
/*  This file is generated by Renesas Project Generator (Ver.4.15).    */
/*                                                                     */
/***********************************************************************/
//
// ビーチフラッグの自律プログラムです。
// 　・近藤科学からダウンロードした「KCB2_Lesson2.c」のサンプルプログラム
// 　　を参考にしています。
// 　・受信センサが１つのため、その点を改良しています。
// 車体はフォークリフト（タミヤ工作キット）です。
//
// 動作概要は次の通りです。
// 　・まず旋回しながら送信センサ位置を探査します。
// 　・送信センサが見つかったら前進します。
// 　・送信センサをロストすると旋回探査に入ります。
// 　・旋回探査から抜ける時に次の旋回探査は逆旋回するようにしています。
// 　・送信センサに近づく（測定値420以上になる）と停止します。
//
// 端子の接続
//  MOTER1 - 本体移動用モーター(左)
//  MOTER2 - 本体移動用モーター(右)
//  AD4    - 超音波センサー(USRX-1)
//

#include <pwm.h>
#include <led.h>
#include <ad.h>

#define LOST 200
void main(void)
{
 _Bool t_sw = 0;   // ターン方向の変数：初期値は右旋回
 int adv1, adv2, adv_dif; // センサ値（現在と直前）とその差の変数
 
 // 初期化
 cpu_init();   // CPU
 pwm_init();   // PWM
 ad_init();   // AD

 ledred_on();

 adv2 = ad_read(4);  // 直前のセンサ値変数を現在の測定値で初期化

 while(1) {

  adv1 = ad_read(4); // 現在のセンサ値を読み取り

  // 目標が正面にいる場合  
  if((adv1 < 420) && (adv1 >= LOST) && (adv2 < 420) && (adv2 >= LOST)){
   adv_dif = adv1 - adv2;
   // 直前のセンサ値との誤差が10前後のときは前進
   if((adv_dif <= 25)&&(adv_dif >=-25)){
    pwm1 ( 70,FWD);  // Duty 50%の正転動作を設定します
    pwm2 ( 70,FWD);
   }
  }
  
  // 直前のセンサ値だけが範囲内なら逆旋回
  else if((adv2 < 420) && (adv2 >= LOST)){
   if(t_sw == 1){   // 旋回方向判断
    pwm1 ( 70,BAK);  // 左旋回
    pwm2 ( 70,FWD);
   }
   else{
    pwm1 ( 70,FWD);  // 右旋回
    pwm2 ( 70,BAK);
   }
  }

  // 目標を見失ったと判断した場合
  else if((adv1 < LOST) || (adv2 < LOST)){
   // 直前と現在のセンサ値がLOSTを超えるまで旋回し続ける
   while(1){
    if(t_sw == 1){   // 旋回方向判断
     pwm1 ( 70,BAK);  // 左旋回
     pwm2 ( 70,FWD);
    }
    else{
     pwm1 ( 70,FWD);  // 右旋回
     pwm2 ( 70,BAK);
    }
    wait( 30000 );
    pwm1 ( 0,BRK);  // 一時停止
    pwm2 ( 0,BRK);
    wait( 100000 );
    adv1 = ad_read(4);
    if ((adv1 > LOST) && (adv2 > LOST)){ // LOST値以上になったかを判断
     if(t_sw == 1){  // 次の旋回方向を指定
      t_sw = 0; // 次は右旋回
     }
     else{
      t_sw = 1; // 次は左旋回
     }
     break;
    }
    adv2 = adv1;  // 直前のセンサ値変数を更新
   }
  }
  
  // もし目標が近くにいた場合
  else{
   pwm1 ( 0,BRK);  // 閾値420を超えたら停止
   pwm2 ( 0,BRK);
  }
  adv2 = adv1;   // 直前のセンサ値変数を更新
 }
}

TAMIYA 【工作キット】 3chフォークリフト工作キット

• ショップ: ホビープラザ　ビッグマン
• 価格: 2,835 円

KCB-2（シリアルUSBアダプター付セット）

• ショップ: グラスロードカンパニー
• 価格: 6,300 円

ロボコンマガジン 2009年 07月号 [雑誌]

• ショップ: 楽天ブックス
• 価格: 980 円

タグ：KCB-2 ビーチフラッグ

コメント 1

　自分は4足ﾛﾎﾞｯﾄを趣味で作りながらツイッタ上で同ネームにより近況を書き綴っている者です。
　KHR・Jrの車両型によるビーチフラッグ競技はあるようですが、多脚ﾛﾎﾞｯﾄにはまだないようですね。
　自分の技術もまだまだ未熟で、PICﾏｲｺﾝのｱｾﾝﾌﾞﾗによるﾌﾟﾛｸﾞﾗﾑで赤外線ﾘﾓｺﾝ操作・SHARPの赤外線距離ｾﾝｻと赤外線焦電ｾﾝｻによる自立歩行に昨年成功したばかりです。
　通信をXbeeに、ﾓｰﾀｰをもう少しマシなものしてカメラ搭載の新型機を構想中で、これにより何かの大会に参戦しようと思っています。
　もし、多足歩行ロボの競技に関する情報がをご存知ならば教えてください。
by クロゴマ (2012-07-07 20:34)