([[課題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);