#概要（ライン復帰とは）
ライン復帰について解説していきたいと思います。まず、ライン復帰についてですが、これは
- 自分の周囲にあるライン（黒線）を見つけて
- そのライン対して、トレース（黒線に沿って走行する）できるように車体の向きを調整する
までの一連の動きが「ライン復帰」になります。
この動作は、難所などのラインが存在しない部分を走行した後に、通常のライントレース戻る際に用いられます。今回は、私達のチームがよく利用している復帰方法を一例としてご紹介します。
#ライン復帰の動作
ライン復帰を構成する上記の二つ動作について、それぞれ詳しく説明していきます。
##ラインを見つける動作
ラインを見つける動作は、下図の動作を繰り返し行うことでラインを見つけます。（どうしても説明の文章が長くなってしまうので、読むのが面倒な方は、最後の実行動画を見ると手っ取り早いかもしれません）
動作の流れとしては、1番の動作から4番の動作まできたら、5、6番の動作のように「前回よりもちょっと大きい範囲で」1～4番の動作と同じ動きをします。このような動作を繰り返しながら、ラインが見つかるまでどんどん探索範囲を広げながらラインを探します。そして、上記の繰り返しの動作の最中にラインを見つけたら、次の「車体の向きを調整する」の動作に移ります。
しかし、この「ラインを見つける」という判断にも工夫が必要です。ただ単に、カラーセンサが黒を検知したら「ラインを見つけた」と判断するのは少し危険なのです。
例えば、ライン復帰動作の近くにコース外の緑色のエリアがあったらどうなるでしょうか？答えは「緑の部分をラインと誤検知する」です。コース外の緑色はセンサから見ると結構黒に近いのです。これは、コース外の緑色は結構暗い色なので、ラインの黒と明度が同じくらいになるためです（ただし、ETロボコン2016コースだけは例外的に明るい緑色が使われていました）。
それでは、どのように検知すればいいかというと、下図のように、カラーセンサが「白 → 黒 → 白」の順に色を検知したら、「ラインを見つけた」という判断をするのが安全です。
こうすることで、もしコース外の緑色を見つけても「黒 → 白」になることは無いので、ラインと誤検知することがありません。
##ラインに対して車体の向きを調整する動作
上記の動作の最中にラインを見つけたら、この動作に移ります。しかし、ラインを見つけた直後において、ライン対しての車体の向きが定まっていません。よって、ライン発見直後にすぐライントレースを開始してしまうと、もしかしたら関係ない方向に進み始めてしまうかもしれません。よって、下図のような動作で車体の向きを調整したのちに、ライントレースを開始します。
この動作では左右に車体を動かしながらラインを検知する動作を繰り返すのですが、「左側、もしくは、右側の旋回方位の差が60度以内に納まったら」動作を終了します。つまり、「1番の旋回時の方位変化量」か、「2番の動作の方位変化量」のどちらかが60度以内であれば、車体向きの調整が完了したと判断する、ということです
まだ車体の向きが調整しきれていない場合、「車体が向いている方位」と「ラインが伸びている方位」の差が大きくなっています。そういった場合において、ラインを見つけるためには車体を大きく旋回させなくてはならず、結果として左右に振れる方位量も大きくなります。よって、その左右に振れる方位量が小さく収まるまで繰り返すことによって、正しい向きになるまで動作させることができます。
ただし、この時に「ライントレースのエッジ方向」（黒線の右側に沿って走るのか、左側に沿って走るのか）について考えなければなりません。つまり、右エッジでライントレースする場合は動作2を最後に実行し、左エッジの場合は動作1を最後に実行する必要があります。
ここについての詳細は下記サンプルプログラムの内容を参考にしてください。
#サンプルプログラム
以下が、サンプルプログラムになります。内容としては「ライントレース実行中に、ボタンの押下を検知して、3秒間停止したのちにアイン復帰を実行する」というものになります。
このプログラムで使用している距離計、方位計は過去の記事※のプログラムを利用しています。詳しく知りたい場合は参考にしてください。
※ 1.ETロボコンにおける自己位置計測（その1、距離計編） with EV3
URL：http://qiita.com/TetsuroAkagawa/items/ba6190f08d26df7cc8ad
※ 2.ETロボコンにおける自己位置計測（その２、方位計編） with EV3
URL：http://qiita.com/TetsuroAkagawa/items/4e7de30523d9c7ec6241
ReturnLine.c内では、基本的に ReturnLine_do() のみが外部から呼び出されてライン復帰が実行されます。この関数の引数 bool_t edge によって、ライントレースとしてのエッジを指定することができますので、行っているライントレースが左エッジの場合は「L_EDGE」を，右エッジの場合は「R_EDGE」を指定します（定義はhファイルに記述されています）。他の関数の内部処理については、基本上記の説明と同様になります。
ReturnLine.c
#include "ReturnLine.h"
/* 各関数の状態遷移変数を宣言 */
ReturnLine_enum ReturnLine_state;
/*********** ラインを見つける動作をする関数 ***********/
/* 定義 */
#define SERCH_LINE_TURN_DIR 45.0 // 探索時左右旋回角度
#define SERCH_LINE_MOVE_POWER 10 // 動作速度
#define SERCH_LINE_MOVE_DIS 100.0 // 探索距離
/* 関数宣言 */
bool_t ReturnLine_serchLine(void); // ライン探索する関数
/* 構造体定義 */
typedef enum { // ライン探索時の動作状態
SERCH_TURN, // 探索方向に旋回
SERCH_MOVE // 目標位置まで探索しながら移動
} serchLine_Action_enum;
/* 変数宣言 */
serchLine_enum serchLine_state; // 動作の状態遷移用変数
serchLine_Action_enum serchLine_Action_state; // ライン探索の動作状態
int serchLine_count = 1; // 探索試行回数（1からカウント）
/******** 車体の向きを調整する動作をする関数 **********/
/* 定義 */
#define SET_DIRECTION_MOVE_POWER 5 // 動作速度
#define SET_DIRECTION_dirDiff 90 // 許容される最大旋回範囲
/* 関数宣言 */
bool_t ReturnLine_setDirection(bool_t edge); // 方位を調整する関数
void ReturnLine_setDirection_init(void); // 初期化関数
/* 変数宣言 */
setDirection_enum setDirection_state;
static float setDirection_Ldir = SET_DIRECTION_dirDiff; //左右の旋回量
static float setDirection_Rdir = SET_DIRECTION_dirDiff; //初回時に越えないように閾値を代入
static float setDirection_predir = 0.0; // 方位過去値(旋回開始時方位)
/*********** ライン見つけたか判断する関数 ***********/
/* 関数宣言 */
bool_t ReturnLine_discernLine(void); // ラインの検知を行う関数
void ReturnLine_discernLine_init(void); // 初期化関数
/* 変数宣言 */
discernLine_enum discernLine_state; // 状態遷移関数
/************************************
ライン復帰初期化関数
*************************************/
void ReturnLine_init(void) {
/* ライン復帰関数 */
ReturnLine_state = SERCH_LINE_INIT;
/* ラインを探索する関数 */
serchLine_state = SERCH_L;
serchLine_Action_state = SERCH_TURN;
serchLine_count = 1;
/* 方位を調整する関数 */
ReturnLine_setDirection_init();
/* ラインを検知する関数 */
ReturnLine_discernLine_init();
/* 計測器初期化 */
Direction_init();
Distance_init();
}
/************************************
ライン復帰関数
false：ライン復帰中
true:ライン復帰完了
*************************************/
bool_t ReturnLine_do(bool_t edge) {
switch(ReturnLine_state) {
case SERCH_LINE_INIT: // ライン探索前の初期化処理
ReturnLine_init();
ReturnLine_state = SERCH_LINE;
break;
case SERCH_LINE: // ライン探索実行
if(ReturnLine_serchLine() == true) {
ReturnLine_state = SET_DIRECTION_INIT;
}
break;
case SET_DIRECTION_INIT: // 車体方位調整前の初期化処理
ReturnLine_setDirection_init();
ReturnLine_discernLine_init();
ReturnLine_state = SET_DIRECTION;
break;
case SET_DIRECTION: // 方位調整実行
if(ReturnLine_setDirection(edge) == true) {
ReturnLine_state = SERCH_LINE_INIT;
return true;
}
break;
default:
break;
}
return false;
}
/******************************************
ラインを探索する関数（非公開処理）
false：ライン探索中
true：ライン探索完了
*******************************************/
bool_t ReturnLine_serchLine(void) {
float cur_dir = Direction_getDirection();
float cur_dis = Distance_getDistance();
switch(serchLine_state) {
case SERCH_L: /************** 左側方向を探索する **************/
switch(serchLine_Action_state) {
case SERCH_TURN: /* 探索位置に車体方位を向ける（マイナス角度方位） */
// 左側に旋回
ev3_motor_set_power(EV3_PORT_C, (SERCH_LINE_MOVE_POWER*-1));
ev3_motor_set_power(EV3_PORT_B, SERCH_LINE_MOVE_POWER);
// 左側の指定方位まで旋回したら、前進動作実行
if(cur_dir <= (SERCH_LINE_TURN_DIR*-1.0)) {
ReturnLine_discernLine_init();
serchLine_Action_state = SERCH_MOVE;
}
break;
case SERCH_MOVE: /* 探索位置まで移動する */
// 直進
ev3_motor_set_power(EV3_PORT_C, SERCH_LINE_MOVE_POWER);
ev3_motor_set_power(EV3_PORT_B, SERCH_LINE_MOVE_POWER);
// 指定位置まで移動したら、初期位置に戻る状態に遷移
if(cur_dis >= SERCH_LINE_MOVE_DIS*serchLine_count) {
ReturnLine_discernLine_init();
/* 次はバックしてから旋回するため，移動状態から */
serchLine_Action_state = SERCH_MOVE;
/* 次の状態に遷移 */
serchLine_state = BACK_L;
}
break;
}
break;
case BACK_L: /********** 左側探索位置から探索位置に戻る **********/
switch(serchLine_Action_state) {
case SERCH_MOVE: /* スタート位置まで後ろにバックする */
// 後進
ev3_motor_set_power(EV3_PORT_C, (SERCH_LINE_MOVE_POWER*-1));
ev3_motor_set_power(EV3_PORT_B, (SERCH_LINE_MOVE_POWER*-1));
// 初期位置まで移動
if(cur_dis <= 0.0) {
ReturnLine_discernLine_init();
serchLine_Action_state = SERCH_TURN;
}
break;
case SERCH_TURN: /* 車体を探索開始時の向きに戻す（左（-角度）から右（+角度）） */
// 右向きに旋回
ev3_motor_set_power(EV3_PORT_C, SERCH_LINE_MOVE_POWER);
ev3_motor_set_power(EV3_PORT_B, (SERCH_LINE_MOVE_POWER*-1));
// 初期方位に戻るまで旋回（次は右側探索）
if(cur_dir >= 0.0) {
ReturnLine_discernLine_init();
/* 次は旋回してから進むため */
serchLine_Action_state = SERCH_TURN;
/* 次の状態に遷移 */
serchLine_state = SERCH_R;
}
break;
}
break;
case SERCH_R: /************** 右側方向を探索する **************/
switch(serchLine_Action_state) {
case SERCH_TURN: /* 探索位置に車体方位を向ける(+方向) */
ev3_motor_set_power(EV3_PORT_C, SERCH_LINE_MOVE_POWER);
ev3_motor_set_power(EV3_PORT_B, (SERCH_LINE_MOVE_POWER*-1));
if(cur_dir >= SERCH_LINE_TURN_DIR) {
ReturnLine_discernLine_init();
serchLine_Action_state = SERCH_MOVE;
}
break;
case SERCH_MOVE: /* 探索位置まで移動する */
ev3_motor_set_power(EV3_PORT_C, SERCH_LINE_MOVE_POWER);
ev3_motor_set_power(EV3_PORT_B, SERCH_LINE_MOVE_POWER);
if(cur_dis >= SERCH_LINE_MOVE_DIS*serchLine_count) {
ReturnLine_discernLine_init();
/* 次はバックしてから旋回するため，移動状態から */
serchLine_Action_state = SERCH_MOVE;
/* 次の状態に遷移 */
serchLine_state = BACK_R;
}
break;
}
break;
case BACK_R: /********** 右側探索位置から探索位置に戻る **********/
switch(serchLine_Action_state) {
case SERCH_MOVE: /* スタート位置まで後ろにバックする */
ev3_motor_set_power(EV3_PORT_C, (SERCH_LINE_MOVE_POWER*-1));
ev3_motor_set_power(EV3_PORT_B, (SERCH_LINE_MOVE_POWER*-1));
if(cur_dis <= 0.0) {
ReturnLine_discernLine_init();
serchLine_Action_state = SERCH_TURN;
}
break;
case SERCH_TURN: /* 車体を探索開始時の向きに戻す（右（+方向）から左（-方向）） */
//
ev3_motor_set_power(EV3_PORT_C, (SERCH_LINE_MOVE_POWER*-1));
ev3_motor_set_power(EV3_PORT_B, SERCH_LINE_MOVE_POWER);
if(cur_dir <= 0.0) {
ReturnLine_discernLine_init();
/* 次は旋回してから進むため */
serchLine_Action_state = SERCH_TURN;
/* また左側探索の状態に遷移 */
serchLine_state = SERCH_L;
serchLine_count++;
}
break;
}
break;
default:
break;
}
// ラインを検知したのが、右側か左側かで
// 次の車体方位の調整を行う際の左右旋回の方向を決定
if(ReturnLine_discernLine() == true) {
if(serchLine_state == SERCH_L) {
setDirection_state = TURN_R;
} else
if(serchLine_state == SERCH_R) {
setDirection_state = TURN_L;
}
return true;
} else {
return false;
}
}
/**************************************
車体の方位を調整する関数
[引数]bool_t edge：ライントレースのエッジ
false方位の調整中
true：方位の調整終了
***************************************/
bool_t ReturnLine_setDirection(bool_t edge) {
float cur_dir = Direction_getDirection();
float check_dir = 0.0;
switch(setDirection_state) {
case TURN_R: // 右側方向に旋回しながらライン検知
// 右側旋回
ev3_motor_set_power(EV3_PORT_C, SET_DIRECTION_MOVE_POWER);
ev3_motor_set_power(EV3_PORT_B, (SET_DIRECTION_MOVE_POWER*-1));
// ラインを検知したら，旋回角度計算
if(ReturnLine_discernLine() == true) {
// 右側旋回時の旋回角度を格納
setDirection_Rdir = cur_dir - setDirection_predir;
// 次の左側旋回時角度計算のために，現在方位を過去値として格納
setDirection_predir = cur_dir;
// ライン検知を初期化
ReturnLine_discernLine_init();
setDirection_state = TURN_L;
}
break;
case TURN_L: // 左側方向に旋回しながらライン検知
// 左側旋回
ev3_motor_set_power(EV3_PORT_C, (SET_DIRECTION_MOVE_POWER*-1));
ev3_motor_set_power(EV3_PORT_B, SET_DIRECTION_MOVE_POWER);
// ラインを検知したら，旋回角度計算
if(ReturnLine_discernLine() == true) {
// 左側旋回時の旋回角度を格納
setDirection_Ldir = setDirection_predir - cur_dir;
// 次の右側旋回時角度計算のために，現在方位を過去値として格納
setDirection_predir = cur_dir;
// ライン検知を初期化
ReturnLine_discernLine_init();
setDirection_state = TURN_R;
}
break;
default:
break;
}
// ライントレース時のエッジがどちらかで
// 右側旋回で終わるか，左側旋回で終わるかを決定
if(edge == L_EDGE) {
if(setDirection_Ldir < SET_DIRECTION_dirDiff) {
return true;
}
} else
if(edge == R_EDGE) {
if(setDirection_Rdir < SET_DIRECTION_dirDiff) {
return true;
}
}
return false;
}
/*****************************************
車体の方位を調整する 初期化関数
******************************************/
void ReturnLine_setDirection_init(void) {
setDirection_predir = Direction_getDirection();
setDirection_Ldir = SET_DIRECTION_dirDiff;
setDirection_Rdir = SET_DIRECTION_dirDiff;
}
/*****************************************
ラインを検知する関数
false：ライン未検知
true：ライン検知済み
******************************************/
bool_t ReturnLine_discernLine(void) {
int color_sensor_reflect = ev3_color_sensor_get_reflect(EV3_PORT_2);
switch (discernLine_state) {
case DISCERN_WHITE1: // 白を検知
if(color_sensor_reflect >= WHITE) {
discernLine_state = DISCERN_BLACK;
}
break;
case DISCERN_BLACK: // 黒を検知
if(color_sensor_reflect <= BLACK ) {
ev3_speaker_play_tone(NOTE_D4, 20);
discernLine_state = DISCERN_WHITE2;
}
break;
case DISCERN_WHITE2: // 白を検知
if(color_sensor_reflect >= WHITE) {
ev3_speaker_play_tone(NOTE_D4, 20);
discernLine_state = DISCERN_WHITE1;
return true;
}
break;
default:
break;
}
return false;
}
/***************************************
ラインを検知する 初期化関数
****************************************/
void ReturnLine_discernLine_init(void) {
discernLine_state = DISCERN_WHITE1;
}
ReturnLine.h には、外部から利用される可能性のある定義項目が記述されています。特に ReturnLine_do() 関数を呼び出す際に用いるエッジ定義や，ライントレースに用いる白黒値の定義もここで定義しています。
ReturnLine.h
#ifndef _RETURN_LINE_H_
#define _RETURN_LINE_H_
#include <stdio.h>
#include <stdlib.h>
#include "ev3api.h"
#include "Distance.h"
#include "Direction.h"
/* ライントレースのエッジ定義 */
#define R_EDGE 0 // 左エッジ
#define L_EDGE 1 // 右エッジ
/* カラーセンサの反射光の白黒判断基準値 */
#define WHITE 40 //白値判断基準閾値
#define BLACK 10 //黒値判断基準閾値
#define TASK_INTERVAL 4.0 // タスク周期
/* ライン復帰の状態遷移 */
typedef enum {
SERCH_LINE_INIT,
SERCH_LINE,
SET_DIRECTION_INIT,
SET_DIRECTION
} ReturnLine_enum;
/* ライン探索の状態遷移 */
typedef enum {
SERCH_L,
BACK_L,
SERCH_R,
BACK_R
} serchLine_enum;
/* 車体方位調整の状態遷移 */
typedef enum {
TURN_L,
TURN_R
} setDirection_enum;
/* ライン検知の状態遷移 */
typedef enum {
DISCERN_WHITE1,
DISCERN_BLACK,
DISCERN_WHITE2
} discernLine_enum;
/* 公開関数 */
bool_t ReturnLine_do(bool_t edge); // ライン復帰関数
void ReturnLine_init(void); // 初期化関数
#endif
app.c 内では、冒頭に説明したロボットの動作全体の状態遷移が記述されています。「MAIN_LINE_TRACE状態」ではON/OFF制御でのライントレースを実行しています。そのライントレースの最中にボタンが押下されたのを検知したら、次の「MAIN_WAIT状態」に遷移して、3秒間停止して待機します。3秒経過したら「MAIN_SERCH_LINE状態」に遷移してライン復帰を実行し終えたら、またライントレースを再開します。
app.c
#include "ev3api.h"
#include "app.h"
#include "Distance.h"
#include "Direction.h"
#include "ReturnLine.h"
#include <stdio.h>
#include <stdlib.h>
#if defined(BUILD_MODULE)
#include "module_cfg.h"
#else
#include "kernel_cfg.h"
#endif
#define DEBUG
#ifdef DEBUG
#define _debug(x) (x)
#else
#define _debug(x)
#endif
void main_lineTrace(void); // ライントレース
bool_t main_wait3sec(void); // 3秒間停止
bool_t main_getButton(void); // ボタン押下検知
typedef enum { // メインタスクの状態遷移
MAIN_LINE_TRACE, // ライントレース
MAIN_WAIT, // 3秒間停止
MAIN_SERCH_LINE // ライン探索実行
} main_State_enum;
void main_task(intptr_t unused) {
main_State_enum main_state = MAIN_LINE_TRACE;
/* モーター出力ポートの設定 */
ev3_motor_config(EV3_PORT_C, LARGE_MOTOR);
ev3_motor_config(EV3_PORT_B, LARGE_MOTOR);
ev3_sensor_config(EV3_PORT_2, COLOR_SENSOR);
ev3_sensor_config(EV3_PORT_1, TOUCH_SENSOR);
while(1) {
/* 計測器更新 */
Distance_update();
Direction_update();
switch(main_state) {
case MAIN_LINE_TRACE: /* ライントレースする */
main_lineTrace();
if( main_getButton() == true) {
main_state = MAIN_WAIT;
}
break;
case MAIN_WAIT: /* 3秒間停止して待つ */
// モータを停止
ev3_motor_stop(EV3_PORT_C, true);
ev3_motor_stop(EV3_PORT_B, true);
if(main_wait3sec() == true ) {
main_state = MAIN_SERCH_LINE;
}
break;
case MAIN_SERCH_LINE: /* ライン復帰を行う */
if( ReturnLine_do(R_EDGE) == true) {
ev3_speaker_play_tone(NOTE_D4, 20);
main_state = MAIN_LINE_TRACE;
}
break;
}
/* タスクを4[ms]動作周期にするために、4[ms]のタスクスリープを入れる */
tslp_tsk(4);
}
}
/* 3秒間を計測する関数 */
bool_t main_wait3sec(void) {
static int wait_count = 0;
if(++wait_count >= 3000 / TASK_INTERVAL) {
wait_count = 0;
return true;
} else {
return false;
}
}
/* ライントレースのON/OFF制御関数 */
void main_lineTrace(void) {
if(ev3_color_sensor_get_reflect(EV3_PORT_2) <= BLACK) {
ev3_motor_set_power(EV3_PORT_C, 20);
ev3_motor_set_power(EV3_PORT_B, 0);
} else {
ev3_motor_set_power(EV3_PORT_C, 0);
ev3_motor_set_power(EV3_PORT_B, 20);
}
}
/* ボタン押下検知を行う関数 */
bool_t main_getButton(void) {
static bool_t pre_press = false;
bool_t cur_press = ev3_touch_sensor_is_pressed (EV3_PORT_1);
if( (cur_press == false) && (pre_press == true) ) {
pre_press = cur_press;
return true;
} else {
pre_press = cur_press;
return false;
}
}
#実行結果（動画）
