はじめに

本記事はETロボコン Unofficial Advent Calendar の13日の記事です。

前編はモニタツールの話が中心でしたので、後編では電文送受信と走行体の自己位置推定について書きます。

電文送受信の話

前編で解説したとおり、モニタツールは運搬ルールを生成して、シリアライズした電文をBluetooth※で送信します。走行体は受信した電文をデシリアライズして、運搬ルールを構築する必要があります。
※「Bluetooth®」は、Bluetooth SIG, Inc. USAの商標または登録商標です。

初期位置コードによって運搬コマンドに含まれる運搬経路の数は異なり、電文は可変長が望ましいです。実際に使用した電文フォーマットは以下の通りです。

送受信される電文は運搬ルール以外にも複数存在するため、受信側の処理はステートマシンを実装してあります。

以下のReceiveメソッドは、電文受信タスクにおいて1byte受信するごとに呼び出されます。

ReceiveState.cpp

// ヘッダコード受信ステート
void HeaderState::Receive(uint8_t data)
{
    // ヘッダコードを受信したら、ヘッダデータ領域受信ステートに遷移
    if(data == 0xff) {
        Context->SetState(new HeaderDataState(Context));
    }
}

// ヘッダデータ領域受信ステート
void HeaderDataState::Receive(uint8_t data)
{
    buff.push_back(data);
    if (buff.size() < sizeof(Header)) return;

    // ヘッダデータ受信完了のため、ヘッダ構造体を構築
    Header header;
    memcpy(&header, buff.data(), sizeof(Header));

    // ヘッダのコマンド領域に応じて、次のステートに遷移
    switch (header.Command) {
        case COMMAND_PID_DATA:
            Context->SetState(new PIDDataState(Context));
            break;
        case COMMAND_SELF_POSITION_DATA:
            Context->SetState(new PositionDataState(Context));
            break;
        case COMMAND_BLOCK_MOVE_RULE_DATA:
            Context->SetState(new BlockMoveRuleState(Context, header.Size));
            break;
        case COMMAND_BLUETOOTH_CONTROL: 
            Context->SetState(new BluetoothControlState(Context)); 
            break;
        default:
            Context->SetState(new HeaderState(Context));
            break;
    }
}

各ステートをクラスとして実装して、条件成立時にContextの管理するステートを切替えることで状態遷移が発生します。

また、ブロック並べの攻略に直接の関係はありませんが、重要なのがCOMMAND_BLUETOOTH_CONTROLの電文です。走行体は定周期で入出力情報と自己位置推定情報をモニタツールに送信していますが、この電文を受信すると、走行体はBluetooth送信機能を開始または停止します。

背景として、南関東地区の試走会で、周期処理がうまく動いていないのか、ライントレースが殆ど出来なくなる現象が発生しました。（試走ではコースアウト）
調査したところ、電波状況の悪い環境で走行体がBluetooth送信処理を繰り返すと、送信が遅延されるだけではなく、優先度の高い他のタスクの動作に影響を与えることが判りました。※BtStackのEV3RTにおける仕様と思われます。要調査。

従って、他の走行体・PC・スマホなどBluetooth機器が多数存在する環境でも動作するように、モニタツールからBluetooth送信機能を開始・停止できるようにしてあります。

自己位置補正の話

以上で、モニタツールで運搬ルールを作って、走行体に送信する部分まで実装できました。
しかし、モデル図の通りブロック並べステートマシンを実装したところ、ブロック1個は正しく運搬できても、2個目以降の経路を誤るケースが多発しました。

モニタツールを眺めていた所、オドメトリによる自己位置推定の誤差が累積していることが判ったので、カラーセンサによる自己位置補正を考えました。自己位置補正の実施は、以下のタイミングが挙げられます。

運搬元のブロック置き場に到達したとき
ライントレースしているとき
運搬先のブロック置き場に到達したとき

これらのタイミングで、ブロック置き場の座標、ラインの角度情報を使って自己位置を補正します。補正値として、到達したブロック置き場や、移動中のライン角度を用いた自己位置補正を実装しました。

重要となるのが、オドメトリによる自己位置推定の基準は車軸の中間点であり、HackEVのカラーセンサの位置とは異なるということです。従って、走行体の角度、及びカラーセンサと車軸間の距離を用いて補正座標を計算します。

BlockMoveStrategy.cpp

// 到達したブロック置き場の座標を取得          
Point *p = BtManager->GetDstBlockPoint();
// 取得した座標にカラーセンサ位置を加味して、X方向の自己位置推定の値を補正
SpManager->ResetX(p->X - cos(SpManager->RobotAngle * M_PI / 180.0) * SENSOR_OFFSET);
// 取得した座標にカラーセンサ位置を加味して、Y方向の自己位置推定の値を補正
SpManager->ResetY(p->Y + sin(SpManager->RobotAngle * M_PI / 180.0) * SENSOR_OFFSET);

XとYの補正で符号が異なるのは、プログラム内の座標系が左上原点のためです。
ブロック置き場に到達したタイミングと、ライントレース中に、自己位置推定の結果が補正されることをモニタツール上で確認しました。

まとめ

最初に書いた通り、モニタツールがブロック並べ攻略のポイントでしたが、同時にモニタツールの開発規模は走行体と同等であり、かなりの工数が掛かりました。ETロボコン活動では唯でさえ時間が不足するため、走行体以外のツールを製作しなかったチームも多いと思います。

しかし、来年のルール発表はまだ先ですが、今年と同様に、外部ツールを使うことで大幅に難易度が下がる課題が用意されるかもしれません。
そのために先行でツールを製作するメリットは十分にあり、実際に今年のルール発表前から、Team ASaGiではモニタツールのプロトタイプを実装していました。来年度のETロボコンで上位入賞を目指す方も、時間の余裕がある時に、各種ツール等に先行着手すると、有利に戦えると思います。

それでは、来年出場される皆様、どうかご安全に。

Team ASaGi チームリーダー兼アーキテクトより

ブロック並べ攻略の話。Team ASaGiの場合。後編。

はじめに

電文送受信の話

自己位置補正の話

まとめ