ETロボコンにおける自己位置計測（その3、座標移動編） with EV3

#座標移動計算
##座標移動の概要
　第一回の距離計、第二回の方位計ときて、今回の第三回では「座標移動」を紹介します。これは、距離と方位を利用した応用技術になります。
　座標移動とは、ロボットが移動する範囲を座標として考えて、その座標上の位置を指定しながらロボットを移動させていく技術となります。この技術を用いれば、座標位置を指定するだけでロボットを自由に移動させて動かすことができます。ただし、座標移動に用いている距離計、方位計には、移動するごとにタイヤのスリップなどで誤差が生じていきます。よって、正確な移動が求められる場合には、「床の色」や「障害物にぶつかった際のジャイロの変動」などをトリガに、細かい間隔で距離計や方位計をリセットしていく必要があります。

##座標移動の計算式
　それでは、座標移動の考え方を説明していきます。まずロボットがいる現在位置座標 a のXY座標を[aX, aY]とします。また移動目標座標 b のXY座標を[bX, bY]とします。すると、その座標 a と座標 b の2点間の関係性は下図のようになります。

　この図での赤い線が「現在位置座標から目標座標までの距離」となります。また上図での角度 θ 、つまり「現在位置座標からの目標座標への方位」は逆三角関数から計算することができ、arctanで表すことができます。逆三角関数について詳しく知りたい場合は、検索してみると多く説明されているので調べてみましょう。

#座標移動のサンプルプログラム
それでは座標移動関数のサンプルプログラムを用意しましたので、その説明をします。といっても、Grid.c内に記述されていることは、基本的に上記で説明した計算式を記述しているだけです。
　Grid_setDistance関数、Grid_setDirection関数の設定関数から、「移動目標座標までの距離」「移動目標座標への方位」を設定します。その後、Grid_getDistance関数とGrid_getDirection関数の取得関数を用いて、それぞれの値を利用します。
　この部分についての使い方も以前説明した「距離計」※1、「方位計」※2と大体同じ使い方になります。目標座標に移動を開始する最初の段階で、まず設定関数を呼び出して距離と方位を設定し、条件式などで値が必要な場合は取得関数から値を取り出します。

※1 「ETロボコンにおける自己位置計測（その1、距離計編） with EV3」

• http://qiita.com/PsychoGundam009/items/ba6190f08d26df7cc8ad
  ※2 「ETロボコンにおける自己位置計測（その２、方位計編） with EV3」
• http://qiita.com/PsychoGundam009/items/4e7de30523d9c7ec6241

Grid.c

#include "Grid.h"

#define GRID_SIZE 100.0 //座標のマス幅（100mm） 

static float grid_distance = 0.0; //現在座標から目標座標までの距離
static float grid_direction = 0.0;//現在座標から目標座標の方位

/* 初期化関数 */
void Grid_init() {
	grid_distance = 0.0;
	grid_direction = 0.0;
}

/* 座標aから座標bまでの移動距離を設定する関数 */
void Grid_setDistance(int aX, int aY, int bX, int bY) {
	grid_distance = sqrt( pow((float)(bX-aX),2) + pow((float)(bY-aY),2) ) *  GRID_SIZE;
}

/* 座標aから座標bまでの移動距離を取得する関数 */
float Grid_getDistance() {
	return grid_distance;
}

/* 目標座標の方位を設定する関数 */
void Grid_setDirection(int aX, int aY, int bX, int bY) {
	float targetDir = 0.0;// 目標方位

	//　座標aから座標bへの方位（ラジアン）を取得
	targetDir = atan2((float)(bY-aY), (float)(bX-aX));
	//ラジアンから度に変換
	targetDir = targetDir * 180.0 / PI;

	grid_direction = targetDir;
}

/* 目標座標の方位を取得する関数 */
float Grid_getDirection() {
	return grid_direction;
}

　Grid.hでは、cファイルで記述されている関数をまとめて宣言しています。
　ここで"Distance.h"をincludeしている理由は、Distance.h内で定義されている円周率を利用したいという理由だけです。また．math.hをincludeしていますが、これはたまにインクルードしてくれない（読み込めない）エラーが出ることがあります。これはMakefile.inc内の末尾に「LIBS += -lm -lc」を追加して書けば解決するので、もしエラーが出た場合は試してみてください。

Grid.h

#ifndef _GRID_H_
#define _GRID_H_

#include "math.h"
#include "Distance.h"

/* 初期化関数 */
void Grid_init();
/* 座標aから座標bまでの移動距離を設定する関数 */
void Grid_setDistance(int aX, int aY, int bX, int bY);
/* 座標aから座標bまでの移動距離を取得する関数 */
float Grid_getDistance();

/* 目標座標の方位を設定する関数 */
void Grid_setDirection(int aX, int aY, int bX, int bY);
/* 目標座標の方位を取得する関数 */
float Grid_getDirection();

#endif

##座標移動プログラムの実行例（動画）
　以下に、座標移動関数の呼び出し実行例を載せます。ここでは開始位置座標[0,0]から順に[2,5]、[3,3]、[4,2]、[5,5]と移動していきます。

　以下がmain文での呼び出し記述例になります。先述の座標移動関数から「現在位置座標から目標座標までの方位」と「現在位置座標から目標座標までの距離」を取得し、その値をもとに旋回→移動→旋回→移動→...と繰り返して、各目標座標を移動していきます。また、ここで使っているDirection_setDirection関数は、前回の記事の方位計に対して、今回新しく追加した関数になります。中身としては、現在の方位を外部から設定できるようにしただけです。

app.c

#include "ev3api.h"
#include "app.h"
#include "Distance.h"
#include "Direction.h"
#include "Grid.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

// 構造体に格納されている目標座標の数
#define GRID_NUM 4

/**
 * Global variables
 */
typedef enum {
	TURN, // 目標座標の方位へ向くまで旋回
	MOVE, // 目標座標に到達するまで前進
	END   // 構造体に格納されている座標を移動しきったら停止
} RUN_STATE;

//目標座標情報の構造体
struct GRID_XY {
	int gridX;
	int gridY;
};

void main_task(intptr_t unused) {
	static RUN_STATE state = TURN;
	struct GRID_XY target_grid[GRID_NUM] = {{2,5},
	                                        {3,3},
	                                        {4,2},
		                                    {5,5} };
	int cur_gridX = 0; // 現在位置座標のX値
	int cur_gridY = 0; // 現在位置座標のY値
	float target_dir = 0.0; // 現在位置座標から目標座標までの距離
	float target_dis = 0.0; // 現在位置座標から目標座標までの方位
	float cur_dir = 0.0; // 方位計の現在値
	float cur_dis = 0.0; // 距離計の現在値
	int grid_count = 0;  // 目標座標構造体への参照カウンタ


	/* モーター出力ポートの設定 */
	ev3_motor_config(EV3_PORT_C, LARGE_MOTOR);
	ev3_motor_config(EV3_PORT_B, LARGE_MOTOR);

	/* 計測器初期化 */
	Distance_init();
	Direction_init();

	// 目標座標までの方位，距離を格納
	Grid_setDistance(cur_gridX, cur_gridY, target_grid[grid_count].gridX, target_grid[grid_count].gridY);
	Grid_setDirection(cur_gridX, cur_gridY, target_grid[grid_count].gridX, target_grid[grid_count].gridY);
	target_dis = Grid_getDistance();
	target_dir = Grid_getDirection();

	while(1) {
		/* 計測器更新 */
		Distance_update();
		Direction_update();
		// 計測器の現在値を格納
		cur_dis = Distance_getDistance();
		cur_dir = Direction_getDirection();

		switch(state) {
		case TURN:
			// 指定方位まで旋回する
			if(cur_dir < target_dir) {
				ev3_motor_set_power(EV3_PORT_C, 10);
				ev3_motor_set_power(EV3_PORT_B, -10);
			} else {
				ev3_motor_set_power(EV3_PORT_C, -10);
				ev3_motor_set_power(EV3_PORT_B, 10);
			}
			// 指定方位の一定範囲内に収まったら，移動開始
			if( (cur_dir > (target_dir-1.0)) && (cur_dir < (target_dir+1.0)) ) {;
				state = MOVE;
			}
			break;
		case MOVE:
			ev3_motor_set_power(EV3_PORT_C, 10);
			ev3_motor_set_power(EV3_PORT_B, 10);

			// 指定位置までたどり着いたら状態遷移
			if( (cur_dis > target_dis)  && (grid_count < (GRID_NUM-1)) ) {
				// 現在位置座標を更新
				cur_gridX = target_grid[grid_count].gridX;
				cur_gridY = target_grid[grid_count].gridY;
				// 計測器情報のリセット
				Distance_init();
				Direction_setDirection(target_dir);
				// 次の座標までの方位，距離を格納する
				grid_count++;
				Grid_setDistance(cur_gridX, cur_gridY, target_grid[grid_count].gridX, target_grid[grid_count].gridY);
				Grid_setDirection(cur_gridX, cur_gridY, target_grid[grid_count].gridX, target_grid[grid_count].gridY);
				target_dis = Grid_getDistance();
				target_dir = Grid_getDirection();

				// 再度，次座標への旋回を開始
				state = TURN;
			} else
				if( (cur_dis > target_dis) && (grid_count >= (GRID_NUM-1)) ) {
					state = END;
				}
			break;
		case END:
			// モータを停止
			ev3_motor_stop(EV3_PORT_C, true);
			ev3_motor_stop(EV3_PORT_B, true);
			break;
		default:
			break;
		}
	}
}

Direction.c（追加分）

/* 方位を設定 */
void Direction_setDirection(float set_dir){
	direction = set_dir;
}