課題については課題2を参照。コースはE地点直進コース。
今回の課題ではロボットの大きさ制限(幅15cm長さ18cm以内)があったため、説明書に載っている基本の形を少し改良して収まるようにした。
その際、後輪の位置によって機体の角度が微妙に変わるため、光センサーを完全には固定せず、常に地面に接するようにした。
また、地面に接することで、より正確なライン判断にもつながっている。
今回は上図で色分けしたように11個のサブルーチンに分けてプログラミングをした。
まず、スタート地点からライントレースを開始する位置まで移動するためのプログラムを作成した。前回の課題で、電池の残量によってモーターのスピードに変化が出てしまい苦労したため、今回は関数を用いて、進む距離による制御とした。タイヤの半径が2.7cmであったため、以下のようなプログラムとした。
float GetAngle(float d) // 距離dからタイヤの回転数を計算する関数 { const float diameter = 2.7; // タイヤの半径 const float pai=3.1415; // 円周率 float ang = d/(2*diameter*pai)*360.0; // 角度を計算する return ang; }
そして、スタート地点からライントレース開始地点までのセンサーの移動距離(5cm)を測り、以下のようなサブルーチンを設定した。
sub START() // スタート用サブルーチン { int angleA = GetAngle(5); // 5.0cm進むのに必要な回転角度 RotateMotor(OUT_BC,SPEED_2,angleA); // 指定した角度でモータを回転 }
次にライントレース用のサブルーチンを作成した。私のコースはE地点直進コースで、最初のE地点を判断する必要がないため、黒線の左側の境目をトレースするようにした。
黒線の値を測ると画像のようになったため、以下のようにしきい値を定義した。
#define THRESHOLD 44
また、黒線の境目から大幅にそれた場合は両方のタイヤを回転させ、旋回するようにしたため、以下の5つの動きを定義した。モーターの速度はトレースの正確さを高めるために遅めに設定した。
#define SPEED_1 20 // ライントレース用
#define STEP 1
#define go_stright OnFwd(OUT_BC,SPEED_1);Wait(STEP);Off(OUT_BC); // 直進 #define turn_right_1 OnFwd(OUT_C,SPEED_1);Wait(STEP);Off(OUT_C); // 右折 #define turn_right_2 OnFwd(OUT_C,SPEED_1);OnRev(OUT_B,SPEED_1);Wait(STEP);Off(OUT_BC); // 右旋回 #define turn_left_1 OnFwd(OUT_B,SPEED_1);Wait(STEP);Off(OUT_B); // 左折 #define turn_left_2 OnFwd(OUT_B,SPEED_1);OnRev(OUT_C,SPEED_1);Wait(STEP);Off(OUT_BC); // 左旋回
次に、直角・交差点に差し掛かった時にはライントレースのサブルーチンから抜けるようにするために最も黒い値である「33」が、nMax回連続すれば、繰り返しが終わるようにした。
コースでの試運転を続けた結果、150回の連続が妥当だと判断したため、nMaxを以下のように定義した。
#define nMax 150
サブルーチン本体は以下の通りである。
sub Line_tolesu_left() // 左側のライントレース用サブルーチン { SetSensorLight(S3); int n=0; while (n <= nMax) { if (SENSOR_3 < THRESHOLD-11) { turn_left_2; // 左旋回 n++; // カウントを増やす } else { if (SENSOR_3 < THRESHOLD-6) { turn_left_1; // 左折 } else if (SENSOR_3 < THRESHOLD+6) { go_stright; // 直進 } else if (SENSOR_3 < THRESHOLD+11) { turn_right_1; // 右折 } else { turn_right_2; // 右旋回 } n=0; //回数リセット } } Off(OUT_BC); }
コースのF・G・Hはクランクのカーブであるため、ライントレースとは別のサブルーチンを作成した。直角に回転する方法は、一方のタイヤで45度回転→もう一方のタイヤで45度回転とすることで90度の回転をその場でできるようにしている。ここでも、進む距離による制御にした。回転する際の距離の求め方は以下の通りである。
・タイヤとタイヤの幅・・・11.2cm 11.2cm×2×3.1415(円周率)×45度/360度=8.7962cm
クランクや交差点などでは少しでも遂行タイムを短くするために、ライントレースとは別のスピードを定義した。
#define SPEED_2 30 // ライントレース以外
試運転をしてみると、ただ回転するだけでは、回転した後のセンサーの位置とラインの位置が離れてしまうことがわかったため、回転する前に1cm直進するようにした。また、課題の条件として、クランクでは音を鳴らすように指定されているので、このサブルーチンに組み込んでいる。
sub FGH() // F,G,H用サブルーチン { PlaySound(SOUND_UP); int angleB = GetAngle(1); // 1.0cm進むのに必要な回転角度 RotateMotor(OUT_BC,SPEED_2,angleB); // 指定した角度でモータを回転 int angleC = GetAngle(8.79); // 8.79cm進むのに必要な回転角度 RotateMotor(OUT_B,SPEED_2,angleC); // 指定した角度でモータを回転 RotateMotor(OUT_C,SPEED_2,-angleC); }
このサブルーチンは基本的に前項の「F・G・H用サブルーチン」と同じであるが、Q・Hではロータリーに進入するために、一時停止した後に音色の違う音を鳴らさなくてはいけないため、別のサブルーチンとして設定した。
sub QR() // Q,2回目のR用サブルーチン { Wait(1000); PlaySound(SOUND_DOWN); int angleD = GetAngle(1); // 1.0cm進むのに必要な回転角度 RotateMotor(OUT_BC,SPEED_2,angleD); // 指定した角度でモータを回転 int angleE = GetAngle(8.79); // 8.79cm進むのに必要な回転角度 RotateMotor(OUT_B,SPEED_2,angleE); // 指定した角度でモータを回転 RotateMotor(OUT_C,SPEED_2,-angleE); }
R・Sでは一方のタイヤを4cm分進めることで、そのままロータリーを回転し続けるようにした。また、ここでも音を鳴らすためのプロクラムを組み込んでいる。
sub RS() // R,2回目のS用サブルーチン { PlaySound(SOUND_UP); int angleF = GetAngle(4); // 4cm進むのに必要な回転角度 RotateMotor(OUT_C,SPEED_2,angleF); // 指定した角度でモータを回転 }
ロータリーから抜け出す地点では一方のタイヤを回転させる事で次のライントレースの地点に移動するようにした。動かす距離は試運転を行い調整した。また、ここでも音を鳴らすためのプロクラムを組み込んでいる。
sub SP() // S,P用サブルーチン { PlaySound(SOUND_UP); int angleG = GetAngle(10); // 10cm進むのに必要な回転角度 RotateMotor(OUT_B,SPEED_2,angleG); // 指定した角度でモータを回転 }
この地点はカーブの半径が最小の4cmであるのに加え、センサーがカーブの内側を通ってしまうため、最も黒い値である「33」が150回連続してしまい、ライントレースのサブルーチンから外れてしまう。そこで、カーブの外側に移動するサブルーチンを作ることでこの急カーブを克服した。
sub Curve() // 急カーブ用サブルーチン { int angleH = GetAngle(3); // 3cm進むのに必要な回転角度 RotateMotor(OUT_BC,SPEED_2,angleH); // 指定した角度でモータを回転 }
黒線の右側に移動した後は、これまでのライントレース用サブルーチンは使えないため、右旋回と左旋回、左折と右折を逆にした、右側のライントレース用のサブルーチンを新たに設定した。
sub Line_tolesu_right() // 右側のライントレース用サブルーチン { SetSensorLight(S3); int n=0; while (n <= nMax) { if (SENSOR_3 < THRESHOLD-11) { turn_right_2; n++; } else { if (SENSOR_3 < THRESHOLD-6) { turn_right_1; } else if (SENSOR_3 < THRESHOLD+6) { go_stright; } else if (SENSOR_3 < THRESHOLD+11) { turn_left_1; } else { turn_left_2; } n=0; //回数リセット } } Off(OUT_BC); }
Tでは一時停止した後に音を鳴らし、交差点を直進する。ラインの幅は2cmであるが、最も黒い値である「33」が150回連続するまでに少し左に曲がってしまうため、2.5cm進むことにした。
sub T() // T用サブルーチン { Wait(1000); PlaySound(SOUND_DOWN); int angleI = GetAngle(2.5); // 2.5cm進むのに必要な回転角度 RotateMotor(OUT_BC,SPEED_2,angleI); // 指定した角度でモータを回転 }
このサブルーチンはQ・R用サブルーチンと同じであるが、サブルーチンごとの動作確認を行うために別々にした。
sub E() // E用サブルーチン { Wait(1000); PlaySound(SOUND_DOWN); int angleJ = GetAngle(1); // 1cm進むのに必要な回転角度 RotateMotor(OUT_BC,SPEED_2,angleJ); // 指定した角度でモータを回転 int angleK = GetAngle(8.79); // 8.79cm進むのに必要な回転角度 RotateMotor(OUT_B,SPEED_2,angleK); // 指定した角度でモータを回転 RotateMotor(OUT_C,SPEED_2,-angleK); }
ゴール用サブルーチンでは、T用サブルーチンでの理由と同様に少し左に曲がってしまうため、一方のタイヤを1cm分だけ回転させ、きちんと四角形の中に入るようにした。
sub GOAL() // ゴール用サブルーチン { int angleL = GetAngle(1); // 1cm進むのに必要な回転角度 RotateMotor(OUT_C,SPEED_2,angleL); // 指定した角度でモータを回転 int angleM = GetAngle(17); // 17cm進むのに必要な回転角度 RotateMotor(OUT_BC,SPEED_2,angleM); // 指定した角度でモータを回転 }
ロボットに送信したtask mainは以下の通り
task main() { START(); Line_tolesu_left(); FGH(); Line_tolesu_left(); QR(); Line_tolesu_left(); RS(); Line_tolesu_left(); SP(); Line_tolesu_left(); FGH(); Line_tolesu_left(); Curve(); Line_tolesu_right(); Curve(); Line_tolesu_left(); FGH(); Line_tolesu_left(); T(); Line_tolesu_left(); T(); Line_tolesu_left(); QR(); Line_tolesu_left(); RS(); Line_tolesu_left(); SP(); Line_tolesu_left(); E(); Line_tolesu_left(); GOAL(); }
前回の課題ではプログラムが長くなってしまったため、今回はプロクラムの簡素化を目標にして行った。今回の課題でサブルーチンを初めて使ったが、プロクラムの簡素化に加え、「sub」というところを「task main」にすることでサブルーチンごとの動作チェックが行えるという利点があり、プロクラム作成もしやすかった。また、関数を使うことで電池残量に関係なくロボットを制御することも可能となり、完成直前に電池を替えてしまい、プログラムの一部を変えなければならないということもほとんど無かった。今回は移動距離のために関数を使用したが、次の課題で関数を利用できる場面があれば積極的に使用していきたいと感じた。