目次
モーターAで前方のアーム、モーターB・Cでそれぞれ右前輪・左前輪を稼働させる。
今回は「光センサが線外に出た時線上に戻る」「光センサが線上にある時線外に出る」「光センサが線の境目にある時は直進する」という3つの動作を光センサの値によって制御することで、ロボットがライントレースを行えるようにした。つまり線をトレースするのではなく、線の境目をトレースしていくことになる。
自分の実施コースにおいて光センサの値はおよそ[線外:55][線上:35][線の境目:45]を示すため、{光センサが線外(値が50以上)→線上に戻る} {光センサが線上(値が40以下)→線外に出る} {光センサが中間(値が40から50の間)→直進}のように動作を制御する。
ゆえに、左の境目をトレースしている場合、線上に戻る時は右に曲がり、線外に出る時は左に曲がる という動作を行い、右の境目をトレースしている場合はその逆の動作を行う。
また、交差点や急なカーブにおいては片方の前輪を動かすだけでは曲がり切れないため、曲がりたい方向の前輪を逆回転させることで、ロボットが急激に曲がるようにしている(旋回)。そのような道では直線や緩やかなカーブの道と比べ、光センサが線上または線外に時間的に長く ある。そのため旋回は一定時間以上光センサが線上か線外にあるときに行われるようにした。 加えて交差点と急なカーブでは、交差点の方がより長く光センサが線上にある。なので交差点判断として、旋回を行っても一定時間以上線外に光センサが出なければ、そこは交差点であると認識するようにしている。
今回はボールを当てる空き缶がロボットに一番近い障害物であるため、一度ロボットを旋回により一回転させ、その後一番近い障害物の方を向くような動作を行わせる。
ロボットを一回転させる方法として、まずロボットの前輪間の距離を直径とするような円を考えた。その円周を前輪がどれだけ回転したかで、ロボットが何度旋回するかがわかるので、その値が360度になるまで前輪を回転させることでロボットを一回転させた。
なお (タイヤ直径/前輪間の距離)*タイヤの回転角度=ロボットの旋回角度 である。
そのようにしてロボットを回転させていき超音波センサが物体を捉え、その距離が一番短かったのは前輪がどれだけ回転した時だったのかを記憶しておくことで、一回転終了後 その分 前輪を回転させることにより、一番近い物体の方を向くようにした。
今回のプログラムでは3つの時間関数 t00,t0,t1 を定義、使用することで各動作の持続時間を測定し、条件分岐を行っている。
まず、t00にはロボットがライントレースを開始した時刻が代入される。これは1つのライントレースプログラムを終えるまで更新されることはない。
次に、t0はロボットが直進を行うたびにその現在時刻が代入され、t1は直進、右折、左折のいずれかの動作を行うたび、すなわち今その時の時刻が代入され続ける。
以上のように時間関数を定義し更新することで、各動作を持続している時間を算出し、条件分岐に使用することができる。例えば、急なカーブを曲がるために旋回を行う条件分岐として{t1-t0>150}を考える。この時150とは0.15秒を表す。ロボットが直進しているときはt0は常に更新され、t1も常に更新される。つまりt1-t0=0であるから旋回は行われない。次にロボットが右または左に曲がる場合を考えると、t1は変わらず更新され続けるが、t0は直進を最後に行ったとき、つまり右左折を行い始めた時刻から更新されない。ゆえにロボットが右左折を継続して行っている限りt1-t0の値は増加する。また上記にも書いてあるが、急なカーブや交差点では右左折を行う時間が、直線のライントレースを行う時間より長くかかる。そのため0.15秒以上右左折を行っていれば旋回を行うという条件がたてられる。
このように{t1-t0}はロボットが右左折(旋回を含む)を継続して行っている時間を示す。
そのほかにも今回は{t00-t1}という式も使用しており、これは{今その時の時刻[t1]-ライントレース開始時刻[t00]}を表すため、ライントレースを開始してから何秒間経過したかを示す。
左の境目をトレースする動作をl_line_trace、右の境目をトレースする動作をr_line_traceとしている。
動作説明の通り{t1-t0}は右左折持続時間を示すので、その値が0.15より大きく0.25未満であれば急カーブとみなし、0.25より大きければ交差点であると判断している。またライントレース開始から指定した秒数間[min]の間は交差点判断を行わないように{t1-t00<min}という条件を組み込んでいる。
void l_line_trace(long min){ t1=CurrentTick(); // 今その時の時刻 t0=CurrentTick(); // 直進終了・右左折開始時刻 SetSensorLight(S1); // 光センサ定義 SetSensorLowspeed(S2); // 超音波センサ定義 while(t1-t0<250|t1-t00<min){ // 光センサが一方に0.25s以下ある(交差点判断) 又は // 動き始めてからmin以下しか経過していないとき 繰り返す if (SENSOR_1>50){ // 線外:白 if (t1-t0>150){ // 光センサが0.15s以上線外にある時右旋回 rrt; }else{ // 右に曲がる rt;} }else if (SENSOR_1>40){ // 線の境目:灰 fw; t0=CurrentTick(); // 曲がる時の初期時間の更新 }else{ // 線上:黒 if (t1-t0>150){ // 光センサが0.15s以上線上にある時左旋回 llt; }else{ // 左に曲がる lt;} } t1=CurrentTick(); //今その時の時刻 } Off(OUT_BC); }
void r_line_trace(long min){ t1=CurrentTick(); // 今その時の時刻 t0=CurrentTick(); // 直進終了・右左折開始時刻 SetSensorLight(S1); // 光センサ定義 SetSensorLowspeed(S2); // 超音波センサ定義 while(t1-t0<250||t1-t00<min){ // 光センサが一方に0.25s以下ある(交差点判断) 又は // 動き始めてからmin以下しか経過していないとき 繰り返す if (SENSOR_1>50){ // 線外:白 if (t1-t0>150){ // 光センサが0.15s以上線外にある時左旋回 llt; }else{ // 左に曲がる lt;} }else if (SENSOR_1>40){ // 線の境目:灰 fw; t0=CurrentTick(); // 曲がる時の初期時間の更新 }else{ // 線上:黒 if (t1-t0>150){ // 光センサが0.15s以上線上にある時右旋回 rrt; }else{ // 右に曲がる rt;} } t1=CurrentTick(); //今その時の時刻 } Off(OUT_BC); }
暫定的に物体までの距離の最小値[int d_min]を長めに取っておくことで、必ず更新されるようにしている。
void search_throw(){ SetSensorLowspeed(S2); int d_min=1000; //物体までの最小値
動作説明の通り(タイヤ間の距離[tire_diameter]/タイヤの直径[tire_axis])*タイヤの回転角度[モーターCの回転角度]=ロボットの旋回角度である
ResetTachoCount(OUT_BC); //回転角リセット OnFwdSync(OUT_BC,40,100); //時計回りに旋回 while(((tire_diameter/tire_axis)*MotorTachoCount(OUT_C))<=360.0){ //ロボットが360度旋回するまで 繰り返す if(d_min>SensorUS(S2)){ //最小値が更新された時 d_min=SensorUS(S2); //最小値更新 angle=MotorTachoCount(OUT_C);//タイヤ回転角更新 } } Off(OUT_BC);
旋回開始を基準として、360度旋回するまでのタイヤの回転角度[MotorTachoCount(OUT_C)]から障害物を探知した時のタイヤの回転角度[angle]を引くことで、より旋回角度が小さくなるような前輪の回転角度[ang_min]で旋回するようにしている。
ang_min=MotorTachoCount(OUT_C)-angle; ResetTachoCount(OUT_BC); RotateMotorEx(OUT_BC, 40, ang_min, -100, true, true);//物体の方を向く Off(OUT_BC);
throw_a; //ボールを投げる
向きをもとに戻すときには旋回時とタイヤの回転を逆にしているのでangleを使用している。
ResetTachoCount(OUT_BC); RotateMotorEx(OUT_BC, 40, angle, -100, true, true);//向きを戻す。 Off(OUT_BC); }
task main () { t00=CurrentTick(); //ライントレース開始時刻 OnFwd(OUT_BC,20); //X内から出る until(SENSOR_1<40); Off(OUT_BC); RotateMotor(OUT_BC,30,100); //線AB上に乗る r_line_trace(7000); //右の境目を伝ってBを曲がってKへ向かう(7秒間交差点判断をしない) llt; //Hの方を向いているのでIの方を向くように調整 until(SENSOR_1<40); Off(OUT_BC); Wait(1000); //Kで一時停止 fw; until(SENSOR_1>50); Off(OUT_BC); lt; //Jの方を向く until(SENSOR_1<40); Off(OUT_BC); t00=CurrentTick(); //ライントレース開始時刻を更新 r_line_trace(6500); //右の境目を伝って円を回る(Iを通りすぎるあたりまで)
search_throw(); //障害物探知及びボール投射
lt; //左の境目に移動 until(SENSOR_1<40); //線上 Off(OUT_BC); lt; until(SENSOR_1>50); //線外 Off(OUT_BC); t00=CurrentTick(); l_line_trace(4000); //左の境目を伝ってHを左折しGへ向かう Wait(1000); //Gで一時停止 fw; //Gを左折 until(SENSOR_1<40); Off(OUT_BC); rt; until(SENSOR_1>50); Off(OUT_BC); t00=CurrentTick(); r_line_trace(26000); //右の境目を伝って急カーブを曲がり、Fを曲がってEへ向かう Wait(1000); //Eで一時停止 t00=CurrentTick(); r_line_trace(4000); //右の境目を伝ってEを右折しLへ向かう(Lで一瞬止まる) lt; //Y内に入るため向きを調整 until(SENSOR_1<40); Off(OUT_BC); fw; //Y内に入る Wait(1500); Off(OUT_BC); }
前回の課題1に比べて順調に進み、発表にも間に合い成功できたので良かった。
電池出力変動によってロボットの速度が変化することで、{一定時間交差点判断を行わない}という条件の時間指定に難儀して、調整に手間取った。
ロボットを一回転旋回させるプログラムは相棒に頼り切りになってしまったが、全体を通しては良く協力できたように思う。
見返してみるとプログラム内のt00の更新は、void関数内に組み込んでも問題ないように感じる。
次回の課題はロボコンで、今回の課題の発展的なことを行うようなので、今回の成功を活かして頑張っていきたい。