8
2

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

@keikyufun

【SPIKE-RT】ライントレースのやり方を三種類紹介！

8
Posted at

0.始めに

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

1.ロボットの用意

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

2.ON-OFF制御

仕組み

ON-OFF制御は一番簡単で、黒には行ったら、白に行く、白に行ったら、黒に行くというのを繰り返す簡単なライントレースです。
image.png
メリットは、簡単で、失敗が少ないことですが、動きがガタガタしていて、毎回停まる向きがランダムというデメリットもあります。

コード

次に実際のコードです。
まず、実際に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制御.drawio.png
メリットとしては、プログラムが、中学生程度でも分かるのと、かなり正確なところです。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制御に、ＩとＤを加えた物です。
※文系の場合選択です。
ライントレース-PID制御.drawio.png

Ｄ制御とは

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

Ｉ制御とは

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

コード

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

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制御については、倒立振子など、ほかにも活用できますので、是非覚えると、ロボットプログラミングがもっと楽しくなるかと思います。
ここまでお疲れ様でした！

8
2
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
8
2

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?