4．PID制御走行と限界感度法

前回のPD制御走行によるライントレースの走行実験から、微分動作を入れると走行が格段に安定することを確認して頂けたでしょう。今回はそれに加えて積分動作を入れた PID 制御走行について解説します。

なお、このプログラム trace_pid.c を使用する前に「１．ライントレース（２値化制御走行）のlight.c を実行して、ラインの黒値と背景の白値を計測して、プログラム中の BLACK と WHITE の値を書き換える必要があります。

積分動作を入れた場合の制御入力 u(k) は
　　　　
のように表され、この u(k) がライントレースプログラムでは旋回値(turn値）となります。

1．PID制御走行プログラム

以下に前回作成した PD 制御走行のプログラムに積分動作を加えて、PID制御走行に対応したプログラムに変更したものを示します。

trace_pid.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


#define BT_PASS_KEY             "1234"  /* bluetoothパスキー */

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, 50);
	systick_wait_ms(20);
	ecrobot_sound_tone(500, 5, 50);
	systick_wait_ms(50);
}

TASK(Task1)
{
	int speed=70;

	float dt = 0.01; // 制御周期（秒）：10msec 
	float Kp, Ki, Kd;
	int black,white,gray,light,light_tmp;	/* 一つ前の明るさを格納する"light_tmp"変数を定義 */
	int err, err_prev, err_sum ;
	float turn;
	float turn_p, turn_d, turn_i;
	float Kc, Tc;
	int dtm;

	dtm = (int)(dt * 1000);

	//限界感度法によるPIDゲイン
	Kc = 0.4; // 限界感度のKp
	Tc = 0.42 ; //振動周期（秒）
	Kp = Kc * 0.6;
	Ki = Kp / ( 0.5 * Tc) * dt;
	Kd = Kp * ( 0.125 * Tc ) /dt;

	//Ki = 0; //PD制御
	//Kd = 0; //PI制御

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

	//Kp,Ki,Kd の表示
	display_clear(0);
	display_goto_xy(0, 1);
	display_string("KP =");
	display_int( (int)(Kp*1000),0);
	display_goto_xy(0, 2);
	display_string("KI =");
	display_int( (int)(Ki*1000),0);
	display_goto_xy(0, 3);
	display_string("KD =");
	display_int( (int)(Kd*1000),0);
	display_goto_xy(0, 5);
	display_string("PUSH START");
	display_update();


	while(1){

		while(ecrobot_get_touch_sensor(PORT_TOUCH) == 0){		/* TSが押されたらスタート */
			systick_wait_ms(10);
		}
        	systick_wait_ms(500);           /* 500msec待つ */

		err_prev = ecrobot_get_light_sensor(PORT_LIGHT) - gray;
		err_sum = 0;

        	while(ecrobot_get_touch_sensor(PORT_TOUCH) == 0){ // タッチセンサが押されたらストップ
                	light = ecrobot_get_light_sensor(PORT_LIGHT);
			err = light  - gray;
			err_sum += err;
                	turn_p = Kp * err ;
			turn_d = Kd * (err - err_prev);
			turn_i = Ki * err_sum;
			turn = turn_p + turn_d + turn_i; 
			err_prev = err;
                	light_tmp=light;                /* 一つ前の明るさを格納 */
			nxt_motor_set_speed( L_MOTOR,speed-turn, 1 );
			nxt_motor_set_speed( R_MOTOR,speed+turn, 1 );
                	systick_wait_ms(dtm);            /* dtm[msec]待つ */
		}
	
		nxt_motor_set_speed(L_MOTOR,0,1);
		nxt_motor_set_speed(R_MOTOR,0,1);
		systick_wait_ms(1000);		/* 1秒待つ */
	}

	TerminateTask();					/* 処理終了 */
}