[[2018b/Member]]
*課題２ [#k8739277]
今回の課題はライントレースカーのプログラミングで、
”コースで黒い線に沿って動き、途中でボールをゴール付近に立てた350mlの空き缶(黄色で表示)に当てるロボットを製作”
である。
*目次 [#bbeb5de9]
#contents
*コース [#g110f823]
走るコースは右の通りである。
&ref(kadai2_rrr2.jpg);
-矢印の終点で一時停止
+ロボットを正方形X内におき、Lをスタート
+Eを一時停止して直進
+Iを一時停止して左折
+Hを直進
+Kを直進
+Jを左折
+Cを一時停止して右折
+Eを一時停止して直進
+Gを一時停止して直進
+長方形X内に入って停止
-(一時停止の指定がある場所は、1秒間停止すること)
-ボールはロボットが弧IHKJ上にある時にP地点の空き缶に当てる
*ロボット [#t4e47935]
今回作ったロボットはこれだ！！ ☞☞☞
&ref(車体1.JPG);
**車体 [#y5d2347d]
CENTER:&ref(図_車体1.png);
モータに直結した二輪をメインに、後輪一つ、前方に光センサを取り付けたスタンダードな形状を採用した。
***光センサ [#qe769458]
この機体で使用している光センサは明るさを0から100で示す（下図）。ここで、コースのラインで反射した光とライン外で反射した光の明るさから、白と黒を判別する。
CENTER:&ref(図2_ライン1.png);
&br;
&br;
コースを追う時は、センサが一つであることから、ラインの片側をジグザグ進んでいくようにする。
CENTER:&ref(図2_ライン2.png);
&br;
急カーブや交差点があると、片側をジグザグ進んで行くだけでは、本来辿るコースから外れてしまう等のミスに繋がる。そこで・・・
CENTER:&ref(図2_ライン3.png);
CENTER:&size(100){&color(red){⇓};};
CENTER:&ref(図2_ライン4.png);
同じ色のまま一定時間経過した場合、急カーブを行う等判断を行えるようにした。図は急カーブする場合のものである。
**ボール発射部分 [#ac5995f9]
CENTER:&ref(図_ボール放1.png);
二本のレール上にボールを固定し、押さえを離すことでボールを転がす構造となっている。
|&ref(図_ボール放2.JPG);|CENTER:&size(200){&color(red){ ➡ };};|&ref(図_ボール放3.JPG);|
|&ref(図_ボール放2.JPG);|CENTER:&size(200){&color(red){➡};};|&ref(図_ボール放3.JPG);|
***超音波センサ [#fcf6b174]
今回の課題では、走行途中で缶を探し、ボールを当てる。なので、超音波センサを使用して、缶を探す。
使用する超音波センサはセンチ単位で対象との距離を測ることができる。
&ref(図_音1.png);
*プログラム [#k8de8be2]
**マクロ [#qd2275d5]
/************************************************
ライントレ____@(;ω;`)@彡_____ス・プログラム
*************************************************/
/*マクロ*/
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); //１秒停止
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)はタイヤの回転を担当する。
**探索投射用関数 [#a440809a]
缶を探す時は、一度車体を一周旋回する。ここで、超音波センサの値が最小値になる方向が、缶のある方向と考える。
そこでプログラムは、最小値を取得した方向までの角度を記録し、一周回った後で缶の方向を向くようにする。
下がそのプログラムである。
/*探して投げる*/
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);//１秒待機
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 }
１行目から４行目までは、それぞれ変数等の定義である。
５行目でモータB・Cの回転角をリセットし、６行目で反転同期させて右回転させる。
5 ResetTachoCount(OUT_BC); //回転角リセット
6 OnFwdSync(OUT_BC,40,100); //時計回りに旋回
７〜１２行目は車体が一周するまで超音波センサの値を調べ、最小値であった場合には更新し、そこまでのタイヤの回転角を&color(blue){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 }
車体の回転角は次の式から求められる。
CENTER:&size(20){&color(blue){(タイヤの回転角)・(タイヤの直径)＝(車体の回転角)・(タイヤ間の距離)};};
&br;
&br;
&br;
13 ang_min=MotorTachoCount(OUT_C)-angle;
この行では、一周終わった後の方向から、缶までの角度が&color(blue){ang_min};に格納される。
17 RotateMotorEx(OUT_BC, 40, ang_min, -100, true, true);//物体の方を向く
缶の方向へ&color(blue){ang_min};だけ左旋回する。
21 RotateMotorEx(OUT_BC, 40, angle, -100, true, true);//元の向きへ戻す
元の方向（&color(blue){angle};）まで左旋回して戻る。
&br;
ここまでの車体の動きは次のようになる。
&ref(図_回転1.png);
**ライントレース関数 [#x03857d2]
まず、交差点にて行う"通過"や"１秒停止"の選択を行う関数を定義した。
/*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 }
引数(&color(blue){blk_select};)に入っている値によって、１秒停止、通過、１秒停止して通過のどれかを選択するようになっている。&br;
&br;&br;&br;
次にライントレースのメインである関数を定義した。&color(blue){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 }
２〜７行目は変数等の定義である。
まず、&color(blue){CurrentTick()};はNXT本体が記録している"現在時刻"を返す組み込み関数だ。その為、一定時間を測る場合、"初期時間とする時刻"を取得し、一定時間後に"経過時間となる時刻"を取得。その差を求めるという段階を踏まなければならない。&br;
follow_line関数では、&color(blue){t0};,&color(blue){t1};,&color(blue){start_t};という三つの変数で時間の計測を行っている。
この変数の役割は以下の通りである。
|CENTER:BGCOLOR(#00bfff):変数名|CENTER:BGCOLOR(#00bfff):役割|CENTER:BGCOLOR(#00bfff):更新|
|CENTER:COLOR(blue):start_t|CENTER: 繰り返し開始時の時間を格納 |CENTER:更新無し|
|CENTER:COLOR(blue):t0|CENTER: カーブ開始時の時間を格納 |CENTER:直進時は常時更新|
|CENTER:COLOR(blue):t1|CENTER: 繰り返し中、時間を格納 |CENTER:常時更新|
&br;
CENTER:この変数を使い、"&color(blue){t1};-&color(blue){start_t};"で繰り返しの経過時間を、"&color(blue){t1};-&color(blue){t0};"でカーブでの経過時間を求めることができる。
&br;&br;&br;&br;&br;
次に、
7 int right_var; // 光センサ値用変数
についてだ。&br;
この&color(blue){invert_var};は繰り返しの初めで光センサの値を常に更新し続ける。この値を元に白黒を判断するのだ。
9 right_var = SENSOR_1; // 光センサ値代入
[[光センサ>#qe769458]]で示した図から、40以下を黒、50より上を白として、中間をグレー（直進）と定義した。&br;
ここで、仮にラインの左側に沿うとした場合、黒で右に、白で左に行くようになる。これは当然、ラインの右側では使用できない。そこで・・・
10 if(invert_info=="on"){ // ラインの右か左か "off"で左,"on"で右
11 right_var = right_var*(-1)+91;}
この文で、左右の変更を行えるようにした。式は単純で、[[光センサ>#qe769458]]値に"-1"をかけて91を足すだけ。この操作で50より上の数値が40以下に、40以下が50より上の数値に変わり、左右の反転が可能となった。&br;&br;&br;&br;
**メイン関数 [#r2192ec4]
メイン関数はできるだけ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 }
[[コース>#g110f823]]では、交差点前までを継続時間を指定し、その後、交差点で選択を行うようにしたものをセットにして、各ポイントまで進むようにした。
14 RotateMotor(OUT_C,30,360); //右折
これは、C地点にてラインの左右を変更しながら曲がる時、偶然にも、左のタイヤのみ回転させて車体を約90°回転させるとラインにぴったり合った。なのでそのまま使用したものである。
*反省感想 [#w7554f8c]
-今回の課題は、前回の反省から早めに行っていた為、本番前にそれほど慌てることなく取り組めた。余裕があるのはとてもいいことだと実感した。
-[[マクロ>#qd2275d5]]で定義した直進や旋回等には&color(blue){OnFwd(モータ,パワー)};を使用していたので、電池の残量によって速度に変化が出た。その為秒数指定していた部分が電池交換で狂っしまい、調整に手間取った。この点をどう改良するかが課題の一つだと感じた。
-[[缶を探すところ>#a440809a]]で、1周した後にもう一周するようになってしまった。本来は&color(blue){ang_min};の分だけ動いて戻る仕様だったが、モータの回転速度に、同期させているにも関わらず差が出てしまった為、そうせざるを得なかった。正直、缶の位置によっては無駄が多くなるので直したかった。
-次回は、最も大きな課題となるので今回の反省点を直して次に繋げていきたい。
![[PukiWiki]](image/robotics-logo-small.jpg "[PukiWiki]"){kind=logo}
