(課題2の参考ページ)

プログラム例

2012年後期の課題のプログラム例です。 今回の課題とよく似ているので参考になるかもしれません。 ちなみにこのときのコースは下図のようなものでした。

2012年度後期課題
//////////////////////////////////////////////////////////////////////
//
//   2012年後期の課題のサンプル・プログラム
//   (ライントレース&ピンポン玉運び)
//
//      http://yakushi.shinshu-u.ac.jp/robotics/?2012b%2FMission
//
//    by Seiji Matsumoto
//
//////////////////////////////////////////////////////////////////////

//
//   明るさ判定用の数値定義 (黒線の左側の境界をトレースする)
//
//   白黒を5段階に分けて明るさを判定。ただし
//
//     ONLINE < L1 < L2 < BORDER < L3 < L4 < OFFLINE
//
//   となるように設定する (ONLINEとOFFLINEから他を計算)
//
#define ONLINE 41                   //  黒線上の最も暗い値
#define OFFLINE 59                  //  白い部分のだいたいの明るさ
#define BORDER (ONLINE+OFFLINE)/2   //  白と黒の中間値
#define L1 BORDER-6                 //  L1以下で完全に黒
#define L2 BORDER-2                 //  L2以上L3以下で境界上
#define L3 BORDER+2
#define L4 BORDER+6                 //  L4以上でで完全に白

//
//  センサ、モータの定義
//
#define EYE     SENSOR_2   //  光センサの値
#define LEFT    OUT_A      //  左モータ
#define RIGHT   OUT_C      //  右モータ
#define BOTH    OUT_AC     //  左右のモータ
#define GRABBER OUT_B      //  ピンポン玉をつかむためのモータ

//
//  出発時やコーナーを曲がる時のサウンド
//
#define sound_ready  PlayTone(1047,100); // 出発時のサウンド
#define sound_right  PlayTone(440,2);    // 右急カーブ時のサウンド
#define sound_left   PlayTone(880,2);    // 左急カーブ時のサウンド
#define sound_tight  PlayTone(920,2);    // 左超急カーブ時のサウンド
#define sound_corner PlaySound(SOUND_FAST_UP); // 交差点時のサウンド

//
//  グラバの開閉用
//
#define grabber_open  SetPower(GRABBER,1); OnFwd(GRABBER);
#define grabber_close SetPower(GRABBER,5); OnRev(GRABBER);
#define grabber_stop  Off(GRABBER);

//
//  前進・後進・左折・右折・停止など
//
#define set_power(pwrL,pwrR) SetPower(LEFT,pwrL); SetPower(RIGHT,pwrR)
#define go_forward(pwr)  SetPower(BOTH,pwr); OnFwd(BOTH);
#define go_backward(pwr) SetPower(BOTH,pwr); OnRev(BOTH);
#define turnL(pwr) SetPower(BOTH,pwr); OnRev(LEFT); OnFwd(RIGHT);
#define turnR(pwr) SetPower(BOTH,pwr); OnFwd(LEFT); OnRev(RIGHT);
#define turn_left  turnL(1);  // 1のパワー(最弱)で左旋回
#define turn_right turnR(1);  // 1のパワー(最弱)で右旋回
#define turn_left1  set_power(1,2); Off(LEFT); OnFwd(RIGHT); // 左折
#define turn_right1 set_power(2,1); OnFwd(LEFT); Off(RIGHT); // 右折
#define pause Off(BOTH); Wait(30); 

//
//  その他の定数
//
#define STEP 1           //  一回の判定(whileループ)の動作時間(1/100秒)
#define NTIGHT 3         //  急なカーブの判定基準(白または黒の継続回数)
#define MAXTURNTIME 15   //  交差点判定で使う(黒->白までの時間が
                         //  この値をオーバーすれば交差点!
//
//  グローバル変数
//
int min_follow_time;     //  この時間内なら直角コーナー判定をしない

//
//  出発までのカウントダウン
//
sub ready() {
  repeat(2){Wait(100); PlaySound(SOUND_CLICK);}
  Wait(100); sound_ready; Wait(100);
}

//
//  ピンポン玉を取りにいって黒線に戻ってくるまでの動作
//
sub fetch() {
    grabber_open; Wait(50); grabber_stop;

    // 黒線を過ぎてからグラバーを閉じ始める
    go_forward(2); until(EYE < BORDER);
    grabber_close; Wait(60); pause; grabber_stop;

    // 黒線まで戻ってくる
    turnR(5); Wait(80); pause;
    go_forward(1); until(EYE<=L3); pause;
    turnL(1); Wait(10); until(EYE > L4); Wait(1); 
    go_forward(1); until(EYE <= L4); pause;
}

//
//  シュート
//
sub shoot() {
    turn_left; until(EYE >= L4); pause;
    go_backward(3); until(EYE < L1); until(EYE > L4); Wait(3);
    pause;
    turnL(5); Wait(22); pause;
    turnR(5);
    grabber_open;  Wait(25);  //  回転しながらグラバを開く
    grabber_close; Wait(10);  //  グラバを少し閉じてシュート
    grabber_stop;  Wait(25);
    go_backward(7); Wait(25); pause;
    PlaySound(SOUND_UP);
}

//
//  ライントレース
//  黒線の左側と白い部分の境界をトレースする
//  (T字路または直角コーナーまで)
//
sub follow_line() {
    int brightness;  //  明るさ
    int nOnline=0;   //  L1より暗い値が続く回数
    int nOffline=0;  //  L4より明るい値が続く回数
    int nCorner=0;   //  コーナー判定のための変数
 
    ClearTimer(0);   //  ライントレースの時間測定開始
 
    while (nCorner==0) {  // コーナーに到達するまでライントレースする
        brightness = EYE;
 
        //
        //  基本的なライントレースの動作
        //
        if (brightness < L2) {         //  L2未満の時は左折
            turn_left1;
        } else if (brightness > L3) {  //  L3を超える時は右折
            turn_right1;
        } else {               //  L2 <= brightness <= L3 の時だけ直進
            go_forward(4);
        }
 
        //
 	//  急カーブの対応
 	//
        //  黒(L1未満) または 白(L4超過)が続く回数をカウント
        //  またはリセット
        //
        if (brightness < L1) {
            nOnline++; nOffline=0;
        } else if (brightness > L4) {
            nOnline=0; nOffline++;
        } else {
            nOnline=0; nOffline=0;
        }
 
        //
        //  白がNTIGHT(3回)以上続いたときは黒くなるまで右に旋回
        //  黒がNTIGHT(3回)以上続いたときは白くなるまで右に旋回
        //  ただしその時、MAXTURNTIME以上時間がかかったら
        //  直角コーナーと判定 (旋回時間をFastTimer(1)で測定)
        //
        if (nOffline >= NTIGHT) {  //  急な右カーブの場合
            
            sound_right;
            turn_right; until(EYE <= L4);  // 黒くなるまで右旋回

        } else if (nOnline >= NTIGHT) {  //  急な左カーブの場合
	    
            sound_left;

            //
            //  左旋回を始めてから白い部分に達するまでの時間を計測する
            // 	
            ClearTimer(1);
            turn_left; until(EYE >= L1);  // 白くなるまで左旋回

            //
            //  左折に時間がかかりすぎた場合は
            //  さらに急カーブか交差点かを判定する
	    //
            if (FastTimer(1) > MAXTURNTIME) {
		//
               //  起点から min_follow_time 以内の時は
		//  急カーブと判定
		//
                if (FastTimer(0) < min_follow_time) {
                    //  急カーブ
                    sound_tight;
                } else {
                    //  交差点
                    sound_corner; nCorner++;
                }
            }
        }
        Wait(STEP);
    }
}

//
//  交差点を通過する動作
//
sub cross_line(){
    turn_right; Wait(35);
    go_forward(1); until(EYE >= L4); Wait(2);
    turn_right; until (EYE<=L4);
}

//
//  main タスク:
//    それぞれのT字路または直角コーナーまでを4つの
//    セクションに分け、2番目のT字路だけは通過(直進)する。
//
//    T字路または直角コーナーを通過後、直角コーナーの
//    判定をせずにライントレースし続ける最低限の時間を
//    グローバル変数(min_follow_time)でサブルーチンに渡す。
//    (各セクションで失敗した場合の動作は未実装)
//
task main () {
    SetSensor(EYE,SENSOR_LIGHT);
    ready();
    fetch();
    min_follow_time=150; follow_line();
    min_follow_time=300; follow_line(); cross_line();
    min_follow_time=650; follow_line();
    min_follow_time=700; follow_line(); pause;
    shoot();
}

2015年7月4日からのこのページのだいたいの訪問者数: 本日1 昨日0 合計627


トップ   編集 凍結 差分 バックアップ 添付 複製 名前変更 リロード   新規 一覧 単語検索 最終更新   ヘルプ   最終更新のRSS
Last-modified: 2015-07-20 (月) 14:20:48 (1158d)