こんにちは。ソフト担当のSYです。
今回は、以前紹介したエンジェルリングの解説のつづきを話していきたいと思います。
前回のおさらい
エンジェルリングとは
・・・ ラインセンサを隙間なく円形に並べたもの
ラインの位置が正確に分かる!
→→→ 単純なアルゴリズムと動きで間違えることなくライン内に戻れる!!
ラインの位置の変化が分かる!
→→→ 外のボールを取りに行くことができる!!
どんなアルゴリズム?
ここでは、実際にどのようなプログラムを書いたかを簡単に説明していきたいと思います。
下記のリンクは世界大会で使ったラインセンサ処理とほぼ同じプログラムの該当部分です。
ラインセンサ処理のソースコード
1.明るさを読み取る
それぞれのラインセンサの値を読み取り、値に応じて白いライン上・緑のコート上・暗い空中のどれなのかを判別します。 具体的には、ライン上(明るい)かどうかとロボットが持ち上げられている(暗い)かどうかを入れたラインセンサの個数分の配列をつくっています。
また、何個が明るく何個が暗いかも計算しています。
2.審判の持ち上げに対応
暗いと判断したセンサが一定個数以上あればロボットが持ち上げられていると判断し、何回か連続で持ち上げられている場合はコート内に戻ったと判断します。 これは押し出しでコート内に戻されたときに対応するためのものです。
3.ライン上のときは・・・
まず、明るいと判断したラインセンサの間隔を調べます。 すると、間隔が最も広い部分の真ん中の方向がコートの方向とわかるので、その方向に行けばコート内に戻れるとわかります。
次に、直前のコートの方向と先ほど計算したコートの方向を比較します。 180度近く変化していた場合、ロボットが半分以上外に出たとわかり、先ほど計算したコートの方向に180度を足したものが正しい現在のコートの方向になります。
また、現在のコートの方向が出た後に、直前のコートの方向との平均を出してコートの方向の急激な変化を抑えています。
4.ライン上でなかったら・・・
ライン上でなく、空中でもない場合は、コート内にいるかほぼ完全に外に出たかのどちらかです。 なので、ロボットが半分以上外に出たかどうか=ほぼ完全に外に出たかとなります。 ほぼ完全に外に出たとなった場合は、先ほど計算した現在のコートの方向へ進めばよいということになります。
このようにして、条件分岐もほとんどなく簡単に、アウトオブバウンスを防ぐことができます。
実際のプログラムでは・・・
少し専門的な話になりますので、読み飛ばしてもらっても構いません。 ラインセンサ処理のソースコードをもとに実際のプログラムの説明を行っていきたいと思います。
1.明るさを読み取る
qtyILB は暗いと判断した個数、qtyILW は明るいと判断した個数で、QTY_LINE 回のループでセンサの値を読んでいます。
そして、isLineBlack[] にそれぞれが暗いかどうかの真偽、isLineWhite[] にそれぞれが明るいかどうかの真偽を入れています。
int qtyILB = 0; int qtyILW = 0; for(int numLine = 0; numLine < QTY_LINE; numLine ++) { //ライン読み取り valueLine[numLine] = analogRead(P_LINE[numLine]); isLineBlack[numLine] = valueLine[numLine] >= BORDER_BLACK_LINE; isLineWhite[numLine] = valueLine[numLine] <= BORDER_WHITE_LINE; if(isLineBlack[numLine]) { qtyILB ++; } if(isLineWhite[numLine]) { qtyILW ++; } }
2.審判の持ち上げに対応
countIIA は何回連続で持ち上げられているか、countIIA がMAX_CIIA 以上になるとコート内に戻ったと判断し、isOutsideLine 、isHalfOut 、dirInside といったラインの方向に関する変数を初期化します。
そして、isLineBlack[] にそれぞれが暗いかどうかの真偽、isLineWhite[] にそれぞれが明るいかどうかの真偽を入れています。
if(qtyILB >= BORDER_IS_IN_AIR) { //持ち上げられている countIIA ++; if(countIIA >= MAX_CIIA) { countIIA = MAX_CIIA; isOutsideLine = false; isHalfOut = false; dirInside = -1; }
3.ライン上のときは・・・
まず、posILW[] に何番目のラインセンサがライン上にあるかを入れていき、intvLine[] に連続する2つのposILW[] の差、つまりライン上のラインセンサの間隔の大きさを入れていきます。
}else { //ライン上 countIIA = 0; isOutsideLine = false; //白の番号を調べる int posILW[qtyILW]; int numILW = 0; for(int numLine = 0; numLine < QTY_LINE; numLine ++) { if(isLineWhite[numLine]) { posILW[numILW] = numLine; numILW ++; } } //白の間隔を調べる int intvLine[qtyILW]; for(int numLine = 0; numLine < qtyILW - 1; numLine ++) { intvLine[numLine] = posILW[numLine + 1] - posILW[numLine]; } intvLine[qtyILW - 1] = posILW[0] - posILW[qtyILW - 1] + QTY_LINE;
次に、intvLine[] の値を順々に比べていくことで、どの間隔が一番広いかがわかり、posMaxIntvL に間隔の場所、maxIntvL に間隔の大きさを入れています。これによって、ロボットが戻る方向が計算でき、その結果をdirInside に入れています。
ちなみに、simplifyDeg()は角度を0度~360度に収めるための自作の関数なので気にしないでください。
//ラインの方向を調べる int maxIntvL = 0; int posMaxIntvL = 0; for(numILW = 0; numILW < qtyILW; numILW ++) { if(maxIntvL < intvLine[numILW]) { maxIntvL = intvLine[numILW]; posMaxIntvL = numILW; } } double numDirInside = posILW[posMaxIntvL] + maxIntvL * 0.5; dirInside = simplifyDeg(numDirInside / QTY_LINE * 360);
次に、dirInside prvDirInside + 110 ~prvDirInside + 250 の範囲にあれば半分以上外に出たとわかり、isHalfOut は半分以上外に出ているかどうかを表しています。prvDirInsideには事前に直前のdirInside を入れておきます。
//前回と比較 if(prvDirInside >= 0) { //半分以上外か isHalfOut = false; if(insideAngle(dirInside, prvDirInside + 110, prvDirInside + 250)) { dirInside = simplifyDeg(dirInside + 180); isHalfOut = true; }
最後に、誤差等を減らすためにprvDirInsideとdirInside の平均値的なものを計算します。
//平均値計算 if(abs(dirInside - prvDirInside) <= 180) { dirInside = prvDirInside * MULTI_AVG_DI + dirInside * (1 - MULTI_AVG_DI); }else { dirInside = prvDirInside * MULTI_AVG_DI + dirInside * (1 - MULTI_AVG_DI) + 360 * (dirInside >= prvDirInside ? MULTI_AVG_DI : 1 - MULTI_AVG_DI); } dirInside = simplifyDeg(dirInside); } }
4.ライン上でなかったら・・・
countIIA を初期化し、isOutsideLine というライン外かどうかの変数にはisHalfOut という半分以上外に出ているかどうかの変数をそのまま入れます。
dirInside という戻るべき方向を示す変数には、ライン外ならprvDirInside を入れて直前のままにし、ライン内なら-1 を入れて戻るべき方向は存在しないとします。
}else if(qtyILW <= 2) { //ライン上でない countIIA = 0; isOutsideLine = isHalfOut; dirInside = isOutsideLine ? prvDirInside : -1;
以上でプログラムの説明は終わりです。お疲れさまでした( ´∀` )ノ
エンジェルリングのここがよくない...(´・ω・`)
配線が大変
円形上にセンサを配置するため、特にプリント基板の場合は配線をきれいに行うのが難しいです。
重い
センサも多く、センサそれぞれに抵抗をつける必要もある上、円形なので基板自体のサイズも大きいことから、普通より重くなってしまうという欠点があります。
モーターとの物理的干渉を考慮しないといけない
隙間なくセンサを配置する必要があるため、モーターの内側か下にセンサを配置できるような設計が必要です。
まとめ
エンジェルリングはハード的には欠点もいくつかありますが、うまくいけば大変有能な武器になってくれると思います。
何かわかりにくいところや質問があればTwitterの質問箱やブログのコメントにてよろしくお願いしますm(_ _)m
それではさようなら!(´∀`)ノシ