ライントレースし、ボールを目的地まで運ぶロボット
ロボットの前方に赤外線センサーが取り付けられており、このセンサーの真下にボールが来ると、ボール囲いが紫矢印のように降りてくる仕組みになっている。
ロボットの車部分の構造は、設計図に載っていたものを用いた。今回はセンサー類の配置が非常に重要であり、難しい部分だと感じた。超音波センサーは、最初ロボットの前方を調べる向きに取り付けていたが、その状態だとボールがうまく探せなかった。そこで、センサーを下向きに取り付けることによって、ボールの真上にセンサーが来たときにセンサーが反応するように工夫した。また、ボールをつかむ部分に関しては、挟み込む形から上から被せるような形に変更し、ボールをつかむ際にボールがはじかれないようにした。結果として、ロボットの重心が前にきてしまい、少しバランスが悪くなってしまった。
プログラムの全体は次の通りである。
#define BLACK 45 #define B_GRAY 53 #define GRAY 61 #define W_GRAY 69 #define WHITE 75 #define SPEED 30 #define r_rot OnFwd(OUT_C,SPEED);Off(OUT_B); #define l_rot OnFwd(OUT_B,SPEED);Off(OUT_C); #define r_turn OnFwd(OUT_C,SPEED);OnRev(OUT_B,SPEED); #define l_turn OnFwd(OUT_B,SPEED);OnRev(OUT_C,SPEED); #define up RotateMotor(OUT_A,10,-40); void line_follow_r(long tmin = 0,long tmax = 60000) { long t0 = CurrentTick(); long t = CurrentTick(); //開始時間// SetSensorLight(S1); while(CurrentTick() - t0 < 110) { if(SENSOR_1 <= BLACK) { l_rot; } else { if(SENSOR_1 <= B_GRAY) { l_turn; } else if(SENSOR_1 <= GRAY) { OnFwd(OUT_BC,SPEED); } else if(SENSOR_1 <= W_GRAY) { r_turn; } else { r_rot; } t0 = CurrentTick(); } } Off(OUT_BC); } void line_follow_l(long tmin = 0,long tmax = 60000) { long t0 = CurrentTick(); long t = CurrentTick(); //開始時間// SetSensorLight(S1); while(CurrentTick() - t0 < 110) { if(SENSOR_1 <= BLACK) { r_rot; } else { if(SENSOR_1 <= B_GRAY) { r_turn; } else if(SENSOR_1 <= GRAY) { OnFwd(OUT_BC,SPEED); } else if(SENSOR_1 <= W_GRAY) { l_turn; } else { l_rot; } t0 = CurrentTick(); } } Off(OUT_BC); } void fetch_ball(long tmax = 10000) //tmax:探索終了時間// { SetSensorLowspeed(S2); long t0 = CurrentTick(); while(1) { if(CurrentTick() - t0 <= tmax) { if(SensorUS(S2) >= 4) { OnFwd(OUT_BC,15); } else { up; l_rot; Wait(1000); break; } } else { l_rot; Wait(1000); break; } } } task main() { line_follow_r(0,60000); //最初は右側トレース// Wait(1000); OnFwd(OUT_C,SPEED); Off(OUT_B); Wait(100); fetch_ball(10000); //ボール探索// line_follow_l(0,1000); //左側トレース// r_turn; Wait(1000); line_follow_l(0,1000); r_turn; Wait(1000); line_follow_l(0,10000); OnFwd(OUT_BC,SPEED); Wait(1000); line_follow_r(0,10000); }
プログラムの定義部分である。右旋回、左旋回などをマクロとして定義した。dwnはボールを囲うためのモーターを回す部分である。大きく曲がるときにはr_rot,l_rotを、緩やかに曲がるときはr_turn,l_turnを使用する。
#define BLACK 45 #define B_GRAY 53 #define GRAY 61 #define W_GRAY 69 #define WHITE 75 #define SPEED 30 #define r_rot OnFwd(OUT_C,SPEED);Off(OUT_B); #define l_rot OnFwd(OUT_B,SPEED);Off(OUT_C); #define r_turn OnFwd(OUT_C,SPEED);OnRev(OUT_B,SPEED); #define l_turn OnFwd(OUT_B,SPEED);OnRev(OUT_C,SPEED); #define dwn RotateMotor(OUT_A,10,-40);
line_follow_rで線の右側部分、line_follow_lで左側部分をトレースする。tは関数を呼び出してからたった時間を表す変数である。両方ともtmax以上の時間がたっていたら、交差点の判定を行う。
void line_follow_r(long tmin = 0,long tmax = 60000) { long t0 = CurrentTick(); long t = CurrentTick(); SetSensorLight(S1); while(CurrentTick() - t0 < 110) { if(SENSOR_1 <= BLACK) { l_rot; } else { if(SENSOR_1 <= B_GRAY) { l_turn; } else if(SENSOR_1 <= GRAY) { OnFwd(OUT_BC,SPEED); } else if(SENSOR_1 <= W_GRAY) { r_turn; } else { r_rot; } if(CurrentTick() - t >= tmax) { t0 = CurrentTick(); } } } Off(OUT_BC); } void line_follow_l(long tmin = 0,long tmax = 60000) { long t0 = CurrentTick(); long t = CurrentTick(); SetSensorLight(S1); while(CurrentTick() - t0 < 110) { if(SENSOR_1 <= BLACK) { r_rot; } else { if(SENSOR_1 <= B_GRAY) { r_turn; } else if(SENSOR_1 <= GRAY) { OnFwd(OUT_BC,SPEED); } else if(SENSOR_1 <= W_GRAY) { l_turn; } else { l_rot; } if(CurrentTick() - t >= tmax) { t0 = CurrentTick(); } } } Off(OUT_BC); }
fetchでボールを探し、ボールを囲う。ボールが赤外線センサーの真下に来るまで前進し、真下に来た場合はボールを囲う機構を下ろし、左に回転する。この左回転は、トレースするところを左側に変更するためである。tmax以上の時間がたってもボールがセンサーの真下に来ない場合は、ボールの探索を終了する。
void fetch_ball(long tmax = 10000) { SetSensorLowspeed(S2); long t0 = CurrentTick(); while(1) { if(CurrentTick() - t0 <= tmax) { if(SensorUS(S2) >= 4) { OnFwd(OUT_BC,15); } else { dwn; l_rot; Wait(1000); break; } } else { l_rot; Wait(1000); break; } } }
センサー類のセットはmainの部分でやった方が簡潔だと感じた。また、スピードが遅く、目的地にたどり着くまでの時間の誤差が大きく、正確な時間を計ることができなかった。そのため、交差点を通過してしまうということがしばしばあった。
ハードウェアとソフトウェアの両方がうまくいくようなロボット作成は難しいことが分かった。特に、ロボット作りに関して、今回のロボットは前に重心がかかり、光センサーの値がぶれることもあった。また、ロボット自体が大きく、もう少しコンパクトに組み立てたいと感じた。ソフトウェアに関しては、うまくいかないことが多く、プログラムを修正するのが大変だった。このことから、次はデバッグしやすいような、もっと簡潔なプログラム作成をしようと思った。