([[課題2>2015a/Mission2]]の参考ページ)

* プログラム例 [#g07d3352]

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

#ref(2012b/Mission/2012b-mission1.png,70%,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();
     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日からのこのページのだいたいの訪問者数:
本日&counter(today);
昨日&counter(yesterday);
合計&counter(all);

トップ   編集 差分 履歴 添付 複製 名前変更 リロード   新規 一覧 検索 最終更新   ヘルプ   最終更新のRSS