0.始めに

この記事では、spike-rtで、
・ON-OFF制御
・P制御
・PID制御
の三つでライントレースをする方法について解説します。
通常の直進、回転についてはこちらをご覧ください。

1.ロボットの用意

まず、車を組み立ててください。
カラーセンサーが真ん中にあればOKです。
例）

レゴspike primeの車の組み立て方 by @keikyufun

2.ON-OFF制御

仕組み

ON-OFF制御は一番簡単で、黒には行ったら、白に行く、白に行ったら、黒に行くというのを繰り返す簡単なライントレースです。

メリットは、簡単で、失敗が少ないことですが、動きがガタガタしていて、毎回停まる向きがランダムというデメリットもあります。

コード

次に実際のコードです。
まず、実際にvoid Mainの前に書く、定義を乗せます。

definition.c

void line_trace_onoff(pup_motor_t *right_motor, pup_motor_t *left_motor, pup_device_t *color_sensor){
	if (pup_color_sensor_reflection(color_sensor) < しきいち){
		pup_motor_set_speed(right_motor, 500);
		pup_motor_set_speed(left_motor, 1000);
	}else{
		pup_motor_set_speed(right_motor, 1000);
		pup_motor_set_speed(left_motor, 500);
	}
}

警告　絶対にしきいちははかるようにしてください。
しきいちは、（黒の値＋白の値）÷2で求めることが出来ます。

次に使い方です。

syntax.c

line_trace_onoff(右モーター,左モーター,カラーセンサー)

ここで注意点です。
このプログラムは、一回だけのプログラムになっているので、これだけを書いても、「ピクッ」と動くだけで、進みません。なので、以下の例文のようにwhile文に入れて使用します。

example

pup_motor_t *motorA;
pup_motor_t *motorB;
pup_device_t *colorC;
void Main(intptr_t exinf)
{
  // ここからプログラムを書く
  motorA = pup_motor_get_device('A');
  motorB = pup_motor_get_device('B');
  colorC = pup_color_sensor_get_device('C');
  pup_motor_setup(motorA, PUP_DIRECTION_COUNTERCLOCKWISE, true);
  pup_motor_setup(motorB, PUP_DIRECTION_CLOCKWISE, true);

  while (1){
	line_trace_onoff(motorB, motorA, colorC);
	dly_tsk(10);
  }
  exit(0);
}

この文は、ライントレースをずっと続ける物です。
voidの中の、角度を調整することで、早さや、ガタつきを調整することが出来ます。

3.P制御

仕組み

P制御は、明るさに応じて、ON‐OFFのように、曲がり具合を固定するのではなく、曲がり具合を自分で変えてくれるライントレースです。
具体的に言うと、黒と白を実際に計測し、その中間の値との「ずれ」にあわせて、曲がり具合を調整します。

メリットとしては、プログラムが、中学生程度でも分かるのと、かなり正確なところです。Pは、比例を意味していて、反射率に比例して、曲がり具合を調整すると言うことです。

コード

まず、定義です。

definition

void line_trace_p(pup_motor_t *right_motor, pup_motor_t *left_motor, pup_device_t *color_sensor){

    int reflected = pup_color_sensor_reflection(color_sensor);
    float kp = 3.0f
    float direction = しきいち - (reflected * kp);
    int base = 200;
    int right_speed = base + direction;
    int left_speed  = base - direction;

    pup_motor_set_speed(right_motor, right_speed);
    pup_motor_set_speed(left_motor, left_speed);
}

警告　絶対にしきいちははかるようにしてください。
しきいちは、（黒の値＋白の値）÷2で求めることが出来ます。

次に、呼び出し例です。

syntax.c

line_trace_p(右モーター,左モーター,カラーセンサー);

これも同じように、一回分しかないので、whileで繰り返させます。

example

pup_motor_t *motorA;
pup_motor_t *motorB;
pup_device_t *colorC;
void Main(intptr_t exinf)
{
  // ここからプログラムを書く
  motorA = pup_motor_get_device('A');
  motorB = pup_motor_get_device('B');
  colorC = pup_color_sensor_get_device('C');
  pup_motor_setup(motorA, PUP_DIRECTION_COUNTERCLOCKWISE, true);
  pup_motor_setup(motorB, PUP_DIRECTION_CLOCKWISE, true);

  while (1){
	line_trace_p(motorB, motorA, colorC);
	dly_tsk(10);
  }
  exit(0);
}

kpの調整

また、うまくいかない場合は、void中の、「kp」を調整しなければいけません。
kpを上げると、もっと内側に曲がり、早く移動できます。
逆に下げると、もっと外側を曲がり、遅くなります。
これは、コースの曲線によって違うので、コースごとに調整しなければいけません。
このようにすると、なめらかなライントレースをすることが出来ます。私は、P制御だけで十分だと思うのですが、いろいろなコースに対応させるには、PIDが必要になってきます。

4.PID制御

これはめっちゃ難しいライントレースです。
I、Dとは、微分、積分のことで、高校3で習います。
P制御に、ＩとＤを加えた物です。
※文系の場合選択です。

Ｄ制御とは

まず、Ｄ制御について説明します。
Ｄ制御は、前回はかったずれと、今回はかったずれを計算し、今回起こるであろうずれを、Ｐ制御の出力に加えるものです。
簡単に言うと、Ｐ制御のずれを、瞬間的に直すブレーキのような物です。

Ｉ制御とは

次にＩ制御です。
Ｉ制御は、今までのずれを積み立てていって、それをもとに、いままでの癖のようなずれを算出します。
これが、-と、＋で打ち消せばいいのですが、大きくなるほど、Ｉが暴走し出すので、ストッパーを書けるのが一般的です。
簡単に言うと、Ｉ制御は、Ｐ制御の癖直しのような物です。

コード

まず、定義です。
わかりやすいように、//でコメントを入れているので、是非見てみてください。

definition

void line_trace_pid(pup_motor_t *right_motor, pup_motor_t *left_motor, pup_device_t *color_sensor){
	// I制御用
	static int i_2 = 0;
	// D制御用
    static int last_error = 0;
	// 調整用の定数
	float kp = 4.0f;
	float ki = 0.0f;
	float kd = 0.0f;
	// 反射率
	int reflected = pup_color_sensor_reflection(color_sensor);
	if (reflected < しきいち){
		hub_light_on_color(PBIO_COLOR_BLACK);
	}else{
		hub_light_on_color(PBIO_COLOR_WHITE);
	}
	hub_display_number(reflected);
	// P制御
    int target = しきいち;
    float new_error = reflected - target;
    float turn = new_error * kp;
    int base = 200;
	// I制御
	i_2 += new_error;
	if (i_2 > 1000){
		i_2 = 1000;
	} else if (i_2 < -1000){
		i_2 = -1000;
	}
	int i = i_2 * ki;
	// D制御
	int d_2 = new_error - last_error;
	int d = d_2 * kd;
	last_error = new_error;
	// 移動速度の計算
    int right_speed = base + turn + i + d;
    int left_speed  = base - turn + i + d;
	// 移動
    pup_motor_set_speed(right_motor, right_speed);
    pup_motor_set_speed(left_motor, left_speed);
}

int right_speed = base + turn + i + d;
int left_speed = base - turn + i + d;
では、iと、dを、pの影響させる値であるturnに足すことで、モーターのスピードをPIDによって調整させています。
:::note alert
警告　絶対にしきいちははかるようにしてください。
しきいちは、（黒の値＋白の値）÷2で求めることが出来ます。
:::
次に、呼び出し例です。

syntax.c

line_tarce_pid(右モーター,左モーター,カラーセンサー);

こちらも、一回だけでは動作しないので、whileで繰り返す必要があります。

example.c

pup_motor_t *motorA;
pup_motor_t *motorB;
pup_device_t *colorC;
void Main(intptr_t exinf)
{
  // ここからプログラムを書く
  motorA = pup_motor_get_device('A');
  motorB = pup_motor_get_device('B');
  colorC = pup_color_sensor_get_device('C');
  pup_motor_setup(motorA, PUP_DIRECTION_COUNTERCLOCKWISE, true);
  pup_motor_setup(motorB, PUP_DIRECTION_CLOCKWISE, true);

  while (1){
	line_trace_pid(motorB, motorA, colorC);
	dly_tsk(10);
  }
  exit(0);
}

調整方法

こちらもＰ制御同様、調整が必要です。以下に表を載せますので。調整してください。
かなり面倒くさいと思いますが、辛抱強く調整してください。

パラメータ	上げると	下げると	役割
kp	曲がり強い・反応速い・蛇行しやすい	曲がり弱い・外にふくらむ	今のズレを直す力
ki	偏りを補正・暴れやすい	安定する	ズレの蓄積を直す力
kd	曲がりすぎ防止・滑らか	反応が鈍い	ズレ変化のブレーキ

5.終わりに

ここまでで三種類のライントレースを紹介してきました。

と合わせることで、だいたいのコースは走れると思います。
PID制御については、倒立振子など、ほかにも活用できますので、是非覚えると、ロボットプログラミングがもっと楽しくなるかと思います。
ここまでお疲れ様でした！