ETロボコンにおける自己位置の応用（速度計） with EV3

#概要
　速度計はそのまま「車体の走行速度」を計測する計測器になります。速度計を用いることには大きなメリットがあります。そのメリットとしては、大きく分けて「モータの個体差を吸収」「バッテリー残量の影響軽減」の2点が挙げられます。
　これらのメリットについて、あまりイメージがつかない方もいらっしゃるかもしれませんが、実際はコース上の全ての動作に影響を与える大きなものになります。プログラムの調整中に「さっきはこのPID係数でうまく走っていたのに…」とか、「さっきまでここの段差をうまく登っていたのに….」といった経験がどなたにもあるかと思います。これには様々な理由があり、一概には言えませんが、この「モータの個体差」だったり「バッテリー残量」に原因があることが多くあります。
　そして、この速度計を基準にして速度を一定に保つように制御すれば、モータの個体差やバッテリー残量に関係なく、一定速度の走行が実現でき、より安定して車体を動作をさせることができます。

#計算式

式自体はとても単純で、距離と時間の関係から、ただ単に距離を時間で割っただけになります。詳細は以下の図の通り。

　実際のプログラムでは、上図のような周期タスクの周期ごとに進んだ走行距離を一定回数分集めて合計することで、速度として利用します。問題は、計算に用いるためには、どれほどの時間間隔の間での距離を参照するのが適しているのか。長すぎるとノイズは入りにくいが速度変動の反応が悪くなり、短すぎると速度変動の反応はいいがノイズに弱くなります。ここについては、少し微調整が必要となります。

#サンプルプログラム
　以下が速度計のサンプルプログラムになります。ここでは、プログラムのタスク周期が4[ms]に設定していることから、それに合わせて区切りがよくなるように、サンプリング間隔も80[ms]として設定しております。
　main.c から Speed_setSpeed() 関数をタスク周期ごとに一回呼び出すことで、関数内部で80[ms]経過するまで計測してサンプリングを実行してくれます。
　現在の速度値が欲しいときは、Speed_getSpeed() 関数を呼び出せば、戻り値として「現在の走行速度」を受け取ることができます。
　また、ここではより速度を扱いやすいように、速度の単位をmm/msから、mm/sに変換してから用いています。「1秒間どれくらい進むか」と考えたほうが、目標速度の設定時にイメージしやすいですからね。

Speed.c

#include "Speed.h"

#define SAMPLING_INTERVAL 80.0 //速度値更新間隔[ms]

static float speed = 0.0;   //車体の現在の速度 mm/s
static float pre_dis = 0.0; //サンプリングの過去値
static float cur_dis = 0.0; //サンプリングの現在値
//サンプリング待機時間計測用変数
static int intervalTimer = (int)(SAMPLING_INTERVAL / TASK_INTERVAL);

/*///////////////////
速度計の初期化関数
*////////////////////
void Speed_init(void)
{
	//各変数値を初期化
	Distance_init();
	speed = 0.0;
	pre_dis = 0.0;
	cur_dis = 0.0;
}

/*/////////////////////////
現在の速度を更新する関数
タスク周期ごとに呼び出す
*//////////////////////////
void Speed_update(void)
{
	/* SAMPLING_INTERVAL[ms]が経過した毎にサンプリングを行う */
	if(++intervalTimer >= (int)(SAMPLING_INTERVAL / TASK_INTERVAL)) {
		intervalTimer = 0;   // 時間計測用変数初期化
		pre_dis = cur_dis;   // サンプリング過去値更新
		cur_dis = Distance_getDistance(); //サンプリング現在値更新
		// 現在の速度を計算する1000倍してmm/msから，mm/sに変換している
		speed = ((cur_dis - pre_dis) / SAMPLING_INTERVAL) * 1000.0;
	}
	
}

/*/////////////////////////
現在の速度を取得する関数
*//////////////////////////
float Speed_getSpeed(void)
{
	return speed;
}

Speed.h

#ifndef _SPEED_H_
#define _SPEED_H_

#include "Distance.h"

//タスク周期
#define TASK_INTERVAL 4.0

/*///////////////////
速度計の初期化関数
*////////////////////
void Speed_init(void);

/*/////////////////////////
現在の速度を更新する関数
タスク周期ごとに呼び出す
*//////////////////////////
void Speed_update(void);

/*/////////////////////////
現在の速度を取得する関数
*//////////////////////////
float Speed_getSpeed(void);

#endif

　また、app.c では、速度計で得られた現在の速度をもとにその速度が一定に収束するようにモータパワーを調節しています。ここでは100mm/sの一定速度で走行するプログラムになります。
　本来ならば、ここの速度調整に対してもPID制御を行って速度制御すると、より安定した走行ができるのですが、プログラムの記述内容がごちゃごちゃしてしまうので、ここでは簡易的に設定速度に合わせて、パワーを上げ下げしているだけにしています。
　また app.c 内において、main_task 内のループの最後に tslp_tsk(4) を入れることで4[ms]周期の動作を実現しています。ここの周期タスクの実現方法は、他にも app.cfg 内の「CRE_TSK」を「EV3_CRE_CYC」に変更して、各値に適切な項目を設定することでも実現できます。

app.c

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


#define TARGET_SPEED 100.0        // 目標走行速度 [mm/s]
#define SPEED_CALC_INTERVAL 800.0 // 速度調節の動作周期 [ms]

/* LCD周辺定義 */
#define CALIB_FONT (EV3_FONT_SMALL)
#define STR 5
#define CALIB_FONT_WIDTH (6/*TODO: magic number*/)
#define CALIB_FONT_HEIGHT (8/*TODO: magic number*/)


void main_task(intptr_t unused) {
	float forward = 0.0; // モータの前進量
	int speed = 0;       // 現在の速度
	int timer = 0;       // 速度調節の動作周期用カウント変数
	char str[STR][30];   // LCD出力用文字列変数

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

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

	
	while(1) {
		/* 計測器更新 */
		Distance_update();
		Speed_update();

		/* 現在の走行速度を格納 */
		speed = Speed_getSpeed();

		/* 目標速度に達しているかどうかで、モータパワーを調整 */
		if ( ++timer >= (SPEED_CALC_INTERVAL / TASK_INTERVAL) ) {
			timer = 0;
			if( (speed > TARGET_SPEED) && (forward > -100) ) {
				forward -= 2;
			} else
				if( (speed <= TARGET_SPEED) && (forward < 100) ) {
					forward += 2;
				}

			/* ここからLCD（EV3の画面）に速度を出力する処理 */
			ev3_lcd_fill_rect(0, 0, EV3_LCD_WIDTH, EV3_LCD_HEIGHT, EV3_LCD_WHITE);
			sprintf(str[0], "speed = %d", (int)(speed));
			ev3_lcd_draw_string(str[0], 0, CALIB_FONT_HEIGHT*1);
		}

		/* モータのパワーを設定 */
		ev3_motor_set_power(EV3_PORT_C, forward);
		ev3_motor_set_power(EV3_PORT_B, forward);

		/* タスクを4[ms]動作周期にするために、4[ms]のタスクスリープを入れる */
		tslp_tsk(4);
	}
}

app.cfg

INCLUDE("app_common.cfg");

#include "app.h"

DOMAIN(TDOM_APP) {
CRE_TSK(MAIN_TASK, { TA_ACT, 0, main_task, TMIN_APP_TPRI + 1, STACK_SIZE, NULL });
}

ATT_MOD("app.o");
ATT_MOD("Distance.o");
ATT_MOD("speed.o");

#実行結果（動画）
以下が、上記プログラムの実行結果です。