TOPへ 2.ライントレース(P制御走行)

1.ライントレース(2値化制御走行)


※この章を学習するにはⅡ.基礎編1〜7までを習得することが必要です。

ここからの3章ではライントレースロボットについて解説します。
この章では最もシンプルなライントレース手法として、光センサの値によりライン上にある(黒)かない(白)を 判定して、ラインのエッジを目標として走行する制御について説明します。

まず、その準備として標準の2輪走行体に次のように光センサとタッチセンサを取り付けます。
1.コースの黒・白値の確認

コースの黒線の明るさは、コースの置かれている環境により変化します。試しに、照明を消してロボットを走らせると、ライントレースできなかったりします。
したがって、走行前にコース上の黒と白の値を確認して、その値をライントレースプログラムに反映させる必要があります。
次の light.c を実行し、ライン上の黒の値と、コース上の白の値を確認し、この値を次の trace_bin.c に反映させます。

light.c
#include "kernel.h"
#include "kernel_id.h"
#include "ecrobot_interface.h"
#define PORT_LIGHT NXT_PORT_S3	/* 入出力ポートの定義 */
#define PORT_TOUCH NXT_PORT_S2
#define L_MOTOR NXT_PORT_B
#define R_MOTOR NXT_PORT_C

DeclareTask(Task1);				/* Task1を宣言 */

void ecrobot_device_initialize(){		/* OSEK起動時の処理 */
	nxt_motor_set_speed(L_MOTOR,0,1);
	nxt_motor_set_speed(R_MOTOR,0,1);
	ecrobot_set_light_sensor_active(PORT_LIGHT);
}

void ecrobot_device_terminate(){		/* OSEK終了時の処理 */
	nxt_motor_set_speed(L_MOTOR,0,1);
	nxt_motor_set_speed(R_MOTOR,0,1);
	ecrobot_set_light_sensor_inactive(PORT_LIGHT);
}

void user_1ms_isr_type2(void){}

void sound_beep(){                      /* ビープ音を鳴らすユーザ関数 */
	ecrobot_sound_tone(600, 2, 80);
	systick_wait_ms(20);
	ecrobot_sound_tone(500, 5, 80);
	systick_wait_ms(50);
}

TASK(Task1)
{
	int black=0, white=0, gray=0;

	display_clear(0);		

	// 黒値の読み取り
	while(ecrobot_get_touch_sensor(PORT_TOUCH) == 0){		// TSが押されるまでループする
		black = ecrobot_get_light_sensor(PORT_LIGHT);	
		display_goto_xy(0, 1);
		display_string("BLACK=");
		display_int(black, 4);
		display_update();
		systick_wait_ms(10);
	}

	sound_beep();
	systick_wait_ms(200);			/* wait 200msec */

	// 白値の読み取り
	while(ecrobot_get_touch_sensor(PORT_TOUCH) == 0){		// TSが押されるまでループする
		white = ecrobot_get_light_sensor(PORT_LIGHT);
		display_goto_xy(0, 2);
		display_string("WHITE=");
		display_int(white, 4);
		display_update();
		systick_wait_ms(10);
	}

	sound_beep();
	systick_wait_ms(200);			/* wait 200msec */

	gray=(black+white)/2;		/* 中間値算出 */
	display_goto_xy(0, 3);
	display_string("GRAY=");
	display_int(gray, 4);
	display_update();
	
	TerminateTask();					/* 処理終了 */
}



このプログラムを実行すると、画面にはライトセンサの読み取り値が表示されます。タッチセンサを押すと、それが黒の明るさとして設定されます。
その後、白も同様に設定すると、中間値が計算され、それが閾値となります。
走行体をライン上の黒線の上においた場合の光センサを値と、ラインからはずしてコース面の白部分に置いた場合の光の値を記録し、 trace_bin.c のプログラムの BLACK、WHITE 値に入れます。
2.ライントレース(2値化制御走行)

このプログラムは、白黒中間値(灰色)を閾値として、値がそれより大きければ黒(ライン上)、小さければ白(ライン外)と判定し、光センサの中心がラインエッジ上に来るように車体の向きを制御しながらライントレースさせます。 車体の向きの制御は左右のタイヤの走行スピードに差をつけることで行います。 プログラムははラインの外側エッジ(右エッジ)を反時計回り(左回り)で走行させるように作成しています。

スタート方法

  1. 反時計方向に周回するように走行体をセットし、コースの直接部分の右エッジに車体をセットします。
  2. プログラムを起動したあと、タッチセンサを押すとスターとします。(もう一度押すと停止します。)

trace_bin.c
#include "kernel.h"
#include "kernel_id.h"
#include "ecrobot_interface.h"

#define PORT_LIGHT NXT_PORT_S3	/* 入出力ポートの定義 */
#define PORT_TOUCH NXT_PORT_S2	/* 入出力ポートの定義 */
#define L_MOTOR NXT_PORT_B
#define R_MOTOR NXT_PORT_C

#define BLACK 700
#define WHITE 500

DeclareTask(Task1);				/* Task1を宣言 */

void ecrobot_device_initialize(){		/* OSEK起動時の処理 */
	nxt_motor_set_speed(L_MOTOR,0,1);
	nxt_motor_set_speed(R_MOTOR,0,1);
	ecrobot_set_light_sensor_active(PORT_LIGHT);
}

void ecrobot_device_terminate(){		/* OSEK終了時の処理 */
	nxt_motor_set_speed(L_MOTOR,0,1);
	nxt_motor_set_speed(R_MOTOR,0,1);
	ecrobot_set_light_sensor_inactive(PORT_LIGHT);
}

void user_1ms_isr_type2(void){}

TASK(Task1)
{
	int speed = 70;  // 前進スピード 
	int turn = 20; // ターン値 
	int white, black, gray;
	int threshold ;

	black = BLACK;
	white = WHITE;
	gray = (black + white) / 2;
	threshold = gray;

	while(ecrobot_get_touch_sensor(PORT_TOUCH) == 0){	// TSが押されるまでループする
		display_goto_xy(0, 4);
		display_string("PUSH START");
		display_update();
	}

	systick_wait_ms(500);		/* 500msec wait */

	while(ecrobot_get_touch_sensor(PORT_TOUCH) == 0){ 	// TSが押されるまでループする

		if(threshold >= ecrobot_get_light_sensor(PORT_LIGHT)){
			//右旋回
			nxt_motor_set_speed(L_MOTOR,speed+turn,1);			
			nxt_motor_set_speed(R_MOTOR,speed-turn,1);
		}else{
			//左旋回
			nxt_motor_set_speed(L_MOTOR,speed-turn,1);
			nxt_motor_set_speed(R_MOTOR,speed+turn,1);
		}
		systick_wait_ms(10); /* wait 10msec*/
	}

	//停止
	nxt_motor_set_speed(L_MOTOR,0,1);
	nxt_motor_set_speed(R_MOTOR,0,1);
	TerminateTask();					/* 処理終了 */
}




3.課題

1.サンプルプログラムを実行し、動作を確認せよ。
2.speed値とturn値を調整して、テストコースを周回できるようにせよ。またそのときの走行タイムを記録せよ。



TOPへ 2.ライントレース(P制御走行)