前章までのライントレースは、常にロボットが旋回しているため、ジグザグに走行してしまい、あまり速くは走れませんでした。
今回はそれを改良し、速く、安定した走りを実現しましょう。
ロボットは基本的にこれまでのものを引き続き使います。
1．比例制御
これまでのプログラムでは、色の閾値より暗いとき、明るいとき、で旋回方向を指定していました。
しかし、光センサが色の目標付近（この場合は白黒中間値）を読み取っているとき、ロボットを大きく旋回する必要はありません。
そこで、左右のモータの駆動力の差（旋回値）を、光センサ値の目標値（この場合は白黒の中間値）からのずれに比例して大きくなるようにして走行させます（図）。
このような制御を比例制御(P制御)といいます。
次のプログラムをコンパイルして、NXTにダウンロードしてください。
しかし、光センサが色の目標付近（この場合は白黒中間値）を読み取っているとき、ロボットを大きく旋回する必要はありません。
そこで、左右のモータの駆動力の差（旋回値）を、光センサ値の目標値（この場合は白黒の中間値）からのずれに比例して大きくなるようにして走行させます（図）。
このような制御を比例制御(P制御)といいます。
次のプログラムをコンパイルして、NXTにダウンロードしてください。
trace_p.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){}
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 speed = 70; // SPEED値
float Kp = 0.4; // Pゲイン変数
int black,white,gray; // 色変数
float turn;
float turn_prev;
int turn_flag, turn_count;
black = BLACK;
white = WHITE;
gray = (black + white) / 2;
while(1){
while(ecrobot_get_touch_sensor(PORT_TOUCH) == 0){ /* TSが押されるまでループする*/
display_goto_xy(0, 1);
display_string("PUSH START");
display_update();
}
display_clear(1);
systick_wait_ms(500); /* wait 500msec */
turn_prev = 0.0;
turn_flag = 0;
turn_count = 0;
while(ecrobot_get_touch_sensor(PORT_TOUCH) == 0){ //TSが押されるまでループする
// P制御による旋回
turn = Kp * (ecrobot_get_light_sensor(PORT_LIGHT) - gray);
nxt_motor_set_speed(L_MOTOR,speed-turn,1);
nxt_motor_set_speed(R_MOTOR,speed+turn,1);
// TURN値が減少に転じたときにカウントする
if( turn < turn_prev && turn_flag == 1 ){
turn_count++;
//ecrobot_sound_tone(440, 10, 100);
display_goto_xy(0, 3);
display_string("TURN COUNT ");
display_int(turn_count,0);
display_update();
turn_flag = 0;
}
// TURN値が負で上昇しているとき TURN_FLAG を１とする
if( (turn < 0) && (turn > turn_prev) ){
turn_flag = 1;
}
turn_prev = turn;
systick_wait_ms(10); /* wait 10msec*/
}
//停止
nxt_motor_set_speed(L_MOTOR,0,1);
nxt_motor_set_speed(R_MOTOR,0,1);
systick_wait_ms(1000); /* 1秒待つ */
}
TerminateTask(); /* 処理終了 */
}
|
このプログラムを実行しても、ロボットはジグザグに走ってしまうかもしれません。しかし、前の閾値で制御していたものよりも、ロボットの振れは緩やかになっているはずです。
速度と比例ゲインを適切に設定すれば、かなり安定して走れるようになります。
比例ゲインを大きくしすぎると、ロボットが曲がりすぎるので不安定になります。
逆に小さくしすぎると、振れは小さくなりますが、カーブなどが曲がりきれなくなります。
適切な比例ゲインと速度は、コースやロボットによって異なるので、試験走行をして適切な値を探りましょう。
このプログラムはスタートから停止までのジグザクの回数（turn_countの値）をLCDに表時するようにしています。 限界感度法で PID ゲインを決定する際にはジグザクの周期（振動周期）を与える必要がありますが、それを求めるときこの値を利用することが出来ます。