今回の課題はライントレースカーのプログラミングで、
”コースで黒い線に沿って動き、途中でボールをゴール付近に立てた350mlの空き缶(黄色で表示)に当てるロボットを製作”
である。
モータに直結した二輪をメインに、後輪一つ、前方に光センサを取り付けたスタンダードな形状を採用した。
この機体で使用している光センサは明るさを0から100で示す(下図)。ここで、コースのラインで反射した光とライン外で反射した光の明るさから、白と黒を判別する。
コースを追う時は、センサが一つであることから、ラインの片側をジグザグ進んでいくようにする。
急カーブや交差点があると、片側をジグザグ進んで行くだけでは、本来辿るコースから外れてしまう等のミスに繋がる。そこで・・・
同じ色のまま一定時間経過した場合、急カーブを行う等判断を行えるようにした。図は急カーブする場合のものである。
二本のレール上にボールを固定し、押さえを離すことでボールを転がす構造となっている。
今回の課題では、走行途中で缶を探し、ボールを当てる。なので、超音波センサを使用して、缶を探す。
使用する超音波センサはセンチ単位で対象との距離を測ることができる。
/************************************************ ライントレ____@(;ω;`)@彡_____ス・プログラム *************************************************/ /*マクロ*/ 1 #define fw Off(OUT_BC);OnFwd(OUT_BC,30); // 直進 2 #define lt Off(OUT_BC);OnFwd(OUT_B,35); // 左回 3 #define rt Off(OUT_BC);OnFwd(OUT_C,35); // 右回 4 #define rrt Off(OUT_BC);OnFwd(OUT_C,30);OnFwd(OUT_B,-55); // 右旋回 5 #define llt Off(OUT_BC);OnFwd(OUT_B,30);OnFwd(OUT_C,-55); // 左旋回 6 #define wait_sec Off(OUT_BC);Wait(1000); //1秒停止 7 #define mini_go Off(OUT_BC);OnFwd(OUT_BC,30);Wait(500);Off(OUT_BC); //少し直進 8 #define throw_ball Wait(500);RotateMotor(OUT_A,20,-50);Off(OUT_A);//ボールを放す 9 const float tire_diameter = 5.75; //タイヤの直径 10 const float tire_axis = 11.5; //タイヤ間の幅
モータA(OUT_A)は押さえの上下、モータB(OUT_B)・モータC(OUT_C)はタイヤの回転を担当する。
缶を探す時は、一度車体を一周旋回する。ここで、超音波センサの値が最小値になる方向が、缶のある方向と考える。
そこでプログラムは、最小値を取得した方向までの角度を記録し、一周回った後で缶の方向を向くようにする。
下がそのプログラムである。
/*探して投げる*/ 1 void search_throw(){ 2 SetSensorLowspeed(S2);//超音波センサの定義 3 int d_min=1000; //物体までの最小値 4 long ang_min=0,angle=0; //最小値までのタイヤの回転角 5 ResetTachoCount(OUT_BC); //回転角リセット 6 OnFwdSync(OUT_BC,40,100); //時計回りに旋回 7 while(((tire_diameter/tire_axis)*MotorTachoCount(OUT_C))<=360.0){//車体が一周するまで続行 MotorTachoCount(OUT_C)はタイヤ(モータC)の回転角が格納される 8 if(d_min>SensorUS(S2)){ //最小値かどうか 9 d_min=SensorUS(S2); //最小値更新時 10 angle=MotorTachoCount(OUT_C);//タイヤ回転角更新 11 } 12 } 13 ang_min=MotorTachoCount(OUT_C)-angle;//回転終了地点から最小値までの角度 14 Off(OUT_BC);//モータB・C停止 15 Wait(1000);//1秒待機 16 ResetTachoCount(OUT_BC);//モータB・Cの回転角をリセット 17 RotateMotorEx(OUT_BC, 40, ang_min, -100, true, true);//物体の方を向く 18 Off(OUT_BC);//モータB・C停止 19 throw_ball;//モータA回転(押さえが上がる) 20 ResetTachoCount(OUT_BC);//モータB・Cの回転角をリセット 21 RotateMotorEx(OUT_BC, 40, angle, -100, true, true);//元の向きへ戻す 22 Off(OUT_BC);//モータB・C停止 23 }
1行目から4行目までは、それぞれ変数等の定義である。
5行目でモータB・Cの回転角をリセットし、6行目で反転同期させて右回転させる。
5 ResetTachoCount(OUT_BC); //回転角リセット 6 OnFwdSync(OUT_BC,40,100); //時計回りに旋回
7〜12行目は車体が一周するまで超音波センサの値を調べ、最小値であった場合には更新し、そこまでのタイヤの回転角をangleに代入する。
7 while(((tire_diameter/tire_axis)*MotorTachoCount(OUT_C))<=360.0){//車体が一周するまで続行。MotorTachoCount(OUT_C)はタイヤ(モータC)の回転角が格納される。 8 if(d_min>SensorUS(S2)){ //最小値かどうか 9 d_min=SensorUS(S2); //最小値更新時 10 angle=MotorTachoCount(OUT_C);//タイヤ回転角更新 11 } 12 }
車体の回転角は次の式から求められる。
13 ang_min=MotorTachoCount(OUT_C)-angle;
この行では、一周終わった後の方向から、缶までの角度がang_minに格納される。
17 RotateMotorEx(OUT_BC, 40, ang_min, -100, true, true);//物体の方を向く
缶の方向へang_minだけ左旋回する。
21 RotateMotorEx(OUT_BC, 40, angle, -100, true, true);//元の向きへ戻す
元の方向(angle)まで左旋回して戻る。
まず、交差点にて行う"通過"や"1秒停止"の選択を行う関数を定義した。
/*T字路、十字路における選択*/ // 0: 1秒停止 // 1: 少し直進 // 2: 1秒停止して少し直進 1 void blk_selection(int blk_select){ 2 if(blk_select!=1){ // 一時停止 3 wait_sec; 4 } 5 if(blk_select!=0){ // 少し直進 6 mini_go; 7 } 8 }
引数(blk_select)に入っている値によって、1秒停止、通過、1秒停止して通過のどれかを選択するようになっている。
次にライントレースのメインである関数を定義した。while文の繰り返しを用いて、光センサの値を常に参照しながら、直進やカーブ、旋回を随時選択するという構造を軸にしたものである。
/*:::ライントレースのメイン部分:::*/ // しきい値の反転(この機体の場合):var*(-1)+91 1 void follow_line(int bw_time,long min_t,int blk_cnt,string invert_info){ 2 long t0,t1,start_t; // t0,t1,start_t 時間変数の定義 3 SetSensorLight(S1); // 光センサ定義 4 t1=CurrentTick(); // 経過時間 5 t0=CurrentTick(); // カーブ初期時間 6 start_t=CurrentTick(); // 追尾初期時間 7 int right_var; // 光センサ値用変数 8 while(t1-start_t<min_t || min_t<0){//経過時間がmin_t未満、もしくはmin_tがマイナスの時繰り返す /*----------------↓繰り返し↓-----------------*/ 9 right_var = SENSOR_1; // 光センサ値代入 10 if(invert_info=="on"){ // ラインの右か左か "off"で左,"on"で右 11 right_var = right_var*(-1)+91;} 12 if (right_var>50){ /*********右回り↓********/ 13 if (t1-t0>bw_time){ // 右回でbw_time[ms]以上のとき右旋回 14 if (blk_cnt>=0 && blk_cnt<=2 && invert_info=="on"){//ラインの右側でblk_cntが0から2なら 15 blk_selection(blk_cnt);//交差点での選択 16 break;//whileから抜ける 17 }else{//そうでないなら 18 rrt;//右旋回 19 } 20 }else{ //カーブの時間がbw_time[ms]未満なら 21 rt;//右回 22 } /**********直進↓********/ 23 }else if (right_var>40){//色はグレー 24 fw;//直進 25 t0=CurrentTick(); // カーブ初期時間の更新 /*********左回り↓*******/ 26 }else{ 27 if (t1-t0>bw_time){ // 左回でbw_time[ms]以上のとき左旋回 28 if (blk_cnt>=0 && blk_cnt<=2 && invert_info!="on") {//ラインの右側でblk_cntが0から2なら 29 blk_selection(blk_cnt);//交差点での選択 30 break;//whileから抜ける 31 }else{//そうでないなら 32 llt;//左旋回 33 } 34 }else{ //カーブの時間がbw_time[ms]未満なら 35 lt;//左回 36 } 37 } 38 t1=CurrentTick(); //経過時間の更新 /*----------------↑繰り返し↑-----------------*/ 39 } 40 Off(OUT_BC);//モータB・C停止 41 }
2〜7行目は変数等の定義である。
まず、CurrentTick()はNXT本体が記録している"現在時刻"を返す組み込み関数だ。その為、一定時間を測る場合、"初期時間とする時刻"を取得し、一定時間後に"経過時間となる時刻"を取得。その差を求めるという段階を踏まなければならない。
follow_line関数では、t0,t1,start_tという三つの変数で時間の計測を行っている。
この変数の役割は以下の通りである。
変数名 | 役割 | 更新 |
start_t | 繰り返し開始時の時間を格納 | 更新無し |
t0 | カーブ開始時の時間を格納 | 直進時は常時更新 |
t1 | 繰り返し中、時間を格納 | 常時更新 |
次に、
7 int right_var; // 光センサ値用変数
についてだ。
このinvert_varは繰り返しの初めで光センサの値を常に更新し続ける。この値を元に白黒を判断するのだ。
9 right_var = SENSOR_1; // 光センサ値代入
光センサで示した図から、40以下を黒、50より上を白として、中間をグレー(直進)と定義した。
ここで、仮にラインの左側に沿うとした場合、黒で右に、白で左に行くようになる。これは当然、ラインの右側では使用できない。そこで・・・
10 if(invert_info=="on"){ // ラインの右か左か "off"で左,"on"で右 11 right_var = right_var*(-1)+91;}
この文で、左右の変更を行えるようにした。式は単純で、光センサ値に"-1"をかけて91を足すだけ。この操作で50より上の数値が40以下に、40以下が50より上の数値に変わり、左右の反転が可能となった。
メイン関数はできるだけfollow_line関数だけを使うようにしてある。
/*^^^^^^^^メイン関数^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ follow_line("白黒時間","追尾継続時間※1","交差点等行動選択※2",invert_str); 時間の単位は[ms] ※1 負の値で連続 ※2 // 0: 1秒停止 // 1: 少し直進 // 2: 1秒停止して少し直進 その他:llt or rrt (旋回) ~~~~~~~~~:::::~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/ 1 task main(){ 2 string invert_str="off"; // L-E 3 follow_line(200, 2000,-1,invert_str); // 追跡 4 follow_line(200, -1, 2,invert_str); // 追跡 // E-I 5 follow_line(200, 2000,-1,invert_str); // 追跡 6 follow_line(200, -1, 0,invert_str); // 追跡 // I-H 7 follow_line(200, 3000,-1,invert_str); // 追跡 8 follow_line(200, -1, 1,invert_str); // 追跡 // H-K 9 follow_line(200, 3000,-1,invert_str); // 追跡 10 search_throw(); //投げる 11 follow_line(200, -1, 1,invert_str); // 追跡 // K-J-C 12 follow_line(200, 7000,-1,invert_str); // 追跡 13 follow_line(220, -1, 0,invert_str); // 追跡 14 RotateMotor(OUT_C,30,360); //右折 15 invert_str="on";//ここからラインの左右変更 // C-D-・・・ 16 follow_line(200,21500,-1,invert_str); // 追跡 // ・・・-E 17 follow_line(220, -1, 2,invert_str); // 追跡 // E-F-・・・ 18 follow_line(200,26000,-1,invert_str); // 追跡 // ・・・-G 19 follow_line(220, -1, 2,invert_str); // 追跡 // G-X 20 follow_line(200, 1500,-1,invert_str); // 追跡 21 follow_line(220, -1, 1,invert_str); // 追跡 // X- 22 fw;Wait(1000);Off(OUT_BC); // 枠の中へ 23 }
コースでは、交差点前までを継続時間を指定し、その後、交差点で選択を行うようにしたものをセットにして、各ポイントまで進むようにした。
14 RotateMotor(OUT_C,30,360); //右折
これは、C地点にてラインの左右を変更しながら曲がる時、偶然にも、左のタイヤのみ回転させて車体を約90°回転させるとラインにぴったり合った。なのでそのまま使用したものである。