Arduinoでロボットを作ってみました！【16】障害物検出センサを活かした動き
テーブルから落ちない動作はできましたので、さらにスコープに取り付けた測距モジュールを活かして、障害物を検出したらぶつからないように回避する動作を追加したいと思います。
テーブル端を検出した際の動きがおかしい
これまで大分動作試験をしてきて、電源電圧が落ちてきたようなので、電源用のエネループを充電しました。充電直後の電圧は5.6Vほどになったのですが、この状態で動かしたところ、テーブル端を検出した際の動きがおかしいです。
落下防止センサがテーブル端を検出した際の動きは14章で説明したように、測距モジュールの入力値value1またはvalue2の値がしきい値以下となったら、急停止のための後退動作をします。その後再度測距モジュールの入力値を取得して、その値によって回避動作パターンを選択しているのですが、再取得値value1,2が両方共しきい値より大きい場合には、直進動作を行います。エネループの電圧が低い時には、move_back(200)が急停止となっていたのですが、今回は充電した直後で電圧が高いためか、move_back(200)で実際に後退してしまい、後退した後の落下防止センサ入力値の値がテーブル端ではないと判断されて、前進動作に移行、前進したとたんにテーブル端を検出、この繰り返しで異常な動きとなっているようです。
while(1) // breakするまで繰り返す
{
move_straight_i(); // 何もなければひたすら前進
// 測距モジュール入力値
value0 = analogRead(analogPin0);
value1 = analogRead(analogPin1);
value2 = analogRead(analogPin2);
// 落下防止センサがテーブル端を検出した時に停止
if(value1 <= threshold || value2 <= threshold)
{
move_back(200);
move_stop(2000);
// 落下防止センサ入力値再取得
value1 = analogRead(analogPin1);
value2 = analogRead(analogPin2);
※以下value1, 2の再取得値によって回避動作パターンを選択
左のトラックが突然回転しなくなった
動作試験を行っていたら、突然左側のトラックが回転しなくなってしまいました。ギヤボックスを調べてみると、左モータのピニオンギヤがずれてクラウンギヤとのかみ合いが外れていました。
DCモータには異常がありませんでした。テーブル端検出時には前進動作からいきなり後退動作となり、ギヤボックスは慣性で前進回転、モータは後退回転しようとしている状況となるため、繰り返しているうちにピニオンギヤが緩んできたのではないかと推測します。ピニオンギヤに接着剤を塗布して再度モータ軸に挿入して復旧しました。
落下防止用測距モジュールの取付位置をまたまた見直す
テーブル端を検出した際のおかしな動き、ピニオンギヤの緩み発生を受けて、落下防止用測距モジュールの取付位置を、またまた見直すことにします。
測距モジュールでもっと遠くを検出するような配置にして、テーブル端を検出してからモータ停止（move_stop）を行っても、余裕をもって止まれるようにすると共に、できるだけモータにも優しい動きにしたいと思います。
テーブル端－トラック間距離と入力値の関係を測定します。
この結果から、テーブル端検出しきい値を200にしておくと、テーブル端から少なくとも110mmの位置ではテーブル端と検出して、そこからmove_stopで停止して制動距離が60mmとしても、テーブル端から50mm手前では停止できそうです。
今回、落下防止用測距モジュールの配置を変更することで、落下防止用測距モジュールを障害物検出用としても使えそうです。障害物検出用と考えた時の特性を測定してみます。
障害物検出しきい値を400にしておくと、障害物から約120mmの位置で障害物と検出できそうです。
以上の結果から、落下防止用測距モジュールについては、
200 ≦ 入力値 ≦ 400：通常動作
入力値 ＜ 200：テーブル端検出
400 ＜ 入力値：障害物検出
として動作プログラムを考えていきます。
なぜか障害物もなくテーブル端でもない平坦な箇所で停止動作が発生
障害物もなく、テーブル端でもない平坦な箇所で、突然停止動作（move_stop）が発生し、その後直進動作を続ける現象が見られました。スケッチを見てみますと、
while(1) // breakするまで繰り返す
{
move_straight_i(); // 何もなければひたすら直進
// 測距モジュール入力値
value0 = analogRead(analogPin0);
value1 = analogRead(analogPin1);
value2 = analogRead(analogPin2);
// 障害物またはテーブル端を検出時の動作
if(value0 >= ob0 || value1 <= ta1 || value1 >= ob1 || value2 <= ta2 || value2 >= ob2)
{
move_stop(500);
// 測距モジュール入力値再取得
value0 = analogRead(analogPin0);
value1 = analogRead(analogPin1);
value2 = analogRead(analogPin2);
※以下value0, 1, 2の再取得値によって回避動作パターンを選択
ここで、障害物、テーブル端を判断するしきい値は
ob0：DMS0（障害物検出用測距モジュール）障害物検出しきい値 400に設定
ob1：DMS1（落下防止用測距モジュールR）障害物検出しきい値 400に設定
ob2：DMS2（落下防止用測距モジュールL）障害物検出しきい値 400に設定
ta1：DMS1（落下防止用測距モジュールR）テーブル端検出しきい値 200に設定
ta1：DMS2（落下防止用測距モジュールL）テーブル端検出しきい値 200に設定
平坦箇所を走行中のあるタイミングで、３つの測距モジュールのいずれかがしきい値を超えて、停止動作 move_stop(500)が発生し、その後入力値を再取得すると、すべて通常動作の範囲内と判断して直進を続けるという現象が発生しているのではと考えます。
そこで、直進動作中の測距モジュールの入力値をモニターしてみます。
A0 = 85
A1 = 299
A2 = 308
A0 = 153
A1 = 304
A2 = 311
A0 = 149
A1 = 307
A2 = 310
A0 = 9
A1 = 307
A2 = 314
A0 = 55
A1 = 521
A2 = 310
A0 = 33
A1 = 306
A2 = 314
A0 = 34
A1 = 306
A2 = 313
A0 = 36
A1 = 308
A2 = 315
A0 = 38
A1 = 304
A2 = 312
平坦箇所を走行時の落下防止用測距モジュールの入力値A1, A2は通常300近辺なのですが、あるタイミングで521のようにぶっ飛んだ値となり、次にはまた300近辺に戻る現象が見られました。この場合、DMS1の入力値A1が障害物検出しきい値400を超えたために、move_stop(500)が発生し、その後再取得したら300近辺なので、通常の直進動作に戻ります。
測距モジュールの入力値が突然飛んでしまう原因は分からないのですが、このような現象が発生しても無駄に変な動きをしないように、測距モジュールの入力値を5回平均した値を判定値に使うようにスケッチを改良してみます。
// 測距モジュール入力値
int sum_v0 = 0;
int sum_v1 = 0;
int sum_v2 = 0;
int v_sum_n = 0;
while(v_sum_n < 5)
{
sum_v0 = sum_v0 + analogRead(analogPin0);
sum_v1 = sum_v1 + analogRead(analogPin1);
sum_v2 = sum_v2 + analogRead(analogPin2);
v_sum_n = v_sum_n + 1;
}
value0 = sum_v0 / 5;
value1 = sum_v1 / 5;
value2 = sum_v2 / 5;
障害物を避けつつテーブルからも落ちない動作
以上の検討を盛り込んだスケッチで実際に動作確認を行ってみました。
// ARD-R01：障害物＆落下回避走行
// RCサーボの設定
#include <Servo.h> // ライブラリの読み込み<Servo.h>
Servo servo0,servo1,servo2; // Servo型の変数は「servo0」～「servo2」
// タクトスイッチPBS1,PBS2の定義
#define PBS1 8 // 置き換え (PBS1→"8")
#define PBS2 12 // 置き換え (PBS2→"12")
int s1, s2; // 変数 s1, s2はint型
// 測距モジュールの定義
#define analogPin0 0 // 置き換え (analogPin0→"0" 障害物検知)
#define analogPin1 1 // 置き換え (analogPin1→"1" 落下防止Ｒ)
#define analogPin2 2 // 置き換え (analogPin2→"2" 落下防止Ｌ)
// 測距モジュール入力値初期値設定
int value0 = 0; // 障害物検出センサ(DMS0)入力値
int value1 = 0; // 落下防止センサＲ(DMS1)入力値
int value2 = 0; // 落下防止センサＬ(DMS2)入力値
int value0L = 0; // テーブル端検出時：左下方を向いた時のDMS0入力値
int value0R = 0; // テーブル端検出時：右下方を向いた時のDMS0入力値
// 障害物検出・テーブル端検出判断しきい値
int ob0 = 400; // DMS0:障害物検出しきい値
int ob1 = 400; // DMS1:障害物検出しきい値
int ob2 = 400; // DMS2:障害物検出しきい値
int ta1 = 200; // DMS1:テーブル端検出しきい値
int ta2 = 200; // DMS1:テーブル端検出しきい値
void setup() //初期設定
{
// タクトスイッチ用ピン入力設定
pinMode(PBS1, INPUT); // 対面して左側(黄色)のスイッチ
pinMode(PBS2, INPUT); // 対面して右側(白色)のスイッチ
// LED用ピン出力設定
pinMode(3, OUTPUT); // LED R
pinMode(5, OUTPUT); // LED L
// モータードライバー用ピン出力設定
pinMode(11, OUTPUT); // ML IN1
pinMode(10, OUTPUT); // ML IN2
pinMode( 6, OUTPUT); // MR IN1
pinMode( 9, OUTPUT); // MR IN2
//Servo型変数ピン割り当て
servo0.attach(2); // servo0(θ)
servo1.attach(4); // servo1(α)
servo2.attach(7); // servo2(β)
// シリアルモニタの通信速度を設定する
Serial.begin(9600);
}
void loop() // メインの処理
{
電源SWをONにすると、LEDを消灯し、始業点検を行います。今回始業点検は関数にしてみました。
led_lighting(0, 0, 500); // LED消灯
startup_inspection(); // 始業点検
PBS1が押されるまで待機します。
// PBS1 ON待ち
s1 = digitalRead(PBS1);
while(s1!=0)
{
s1 = digitalRead(PBS1);
}
PBS1が押されたらLEDを消灯します。
// PBS1 ON (s1=0)の時
if(s1==0)
{
led_lighting(0, 0, 1000); // LED消灯
以降走行モードに入ります。
while(1) // breakするまで繰り返す
{
move_straight_i(); // 何もなければひたすら直進
直進走行時にPBS2を押すと停止し、走行モード（whileループ）を抜けます。
// PBS2 ONの時 while(1)ループを抜ける
s2 = digitalRead(PBS2);
if(s2==0)
{
move_stop(1000);
break;
}
測距モジュールの入力値を5回分平均した値をvalue0, 1, 2とします。
// 測距モジュール入力値
int sum_v0 = 0;
int sum_v1 = 0;
int sum_v2 = 0;
int v_sum_n = 0;
while(v_sum_n < 5)
{
sum_v0 = sum_v0 + analogRead(analogPin0);
sum_v1 = sum_v1 + analogRead(analogPin1);
sum_v2 = sum_v2 + analogRead(analogPin2);
v_sum_n = v_sum_n + 1;
}
value0 = sum_v0 / 5;
value1 = sum_v1 / 5;
value2 = sum_v2 / 5;
value0, 1, 2の値をシリアルモニタに表示します。
// 測距モジュール入力値出力
Serial.print("A0 = ");
Serial.println(value0);
Serial.print("A1 = ");
Serial.println(value1);
Serial.print("A2 = ");
Serial.println(value2);
// 障害物検出時の動作
DMS1のみが障害物を検出した時には、LED Rを点灯して 左旋回して回避、その後LED消灯します。
// DMS1(R側センサ)のみが障害物検出と判断した際の動き
if(value0 < ob0 && value1 >= ob1 && value2 < ob2)
{
led_lighting(0, 10, 100); // LED R点灯
move_left_rapid(500); // 左急速旋回
led_lighting(0, 0, 10); // LED消灯
}
DMS2のみが障害物を検出した時には、LED Lを点灯して 右旋回して回避、その後LED消灯します。
// DMS2(L側センサ)のみが障害物検出と判断した際の動き
if(value0 < ob0 && value1 < ob1 && value2 >= ob2)
{
led_lighting(10, 0, 100); // LED L点灯
move_right_rapid(500); // 右急速旋回
led_lighting(0, 0, 10); // LED消灯
}
DMS0が障害物を検出した時には、LED LRを点灯し、500ms後退した後、大きめに右旋回して回避します。
// DMS0(障害物検出センサ)が障害物検出と判断した際の動き
if(value0 >= ob0)
{
led_lighting(10, 10, 100); // LED LR点灯
move_back(500); // 後退
move_right_rapid(1000); // 右急速旋回
}
DMS1またはDMS2がテーブル端を検出した時には、停止動作を行います。
// テーブル端を検出時の動作
if(value1 <= ta1 || value2 <= ta2)
{
move_stop(500);
停止した位置で測距モジュールの入力値を再取得します。
// 測距モジュール入力値再取得
value0 = analogRead(analogPin0);
value1 = analogRead(analogPin1);
value2 = analogRead(analogPin2);
DMS1がテーブル端を検出した時には、LED Rを点灯→スコープを左向き→左旋回→停止→スコープを中立位置→LED消灯します。
// DMS1(R側センサ)がテーブル端を検出した時の回避動作
if(value1 <= ta1 && value2 > ta2)
{
led_lighting(0, 10, 100); // LED R点灯
scope_drive(108, 70, 80, 200); // スコープ左向き
move_left_rapid(600); // 急速左旋回
move_stop(500); // 停止
scope_np(200); // スコープ中立位置
led_lighting(0, 0, 10); // LED消灯
}
DMS2がテーブル端を検出した時には、LED Lを点灯→スコープを右向き→右旋回→停止→スコープを中立位置→LED消灯します。
// DMS2(L側センサ)がテーブル端を検出した時の回避動作
if(value1 > ta1 && value2 <= ta2)
{
led_lighting(10, 0, 100); // LED L点灯
scope_drive(68, 70, 80, 200); // スコープ右向き
move_right_rapid(500); // 急速右旋回
move_stop(500); // 停止
scope_np(200); // スコープ中立位置
led_lighting(0, 0, 10); // LED消灯
}
DMS1,2両方がテーブル端を検出した時には、先ずLED LRを点灯→スコープを下向き→後退→停止→LED消灯を行います。
// DMS1,DMS2両方がテーブル端を検出した時の回避動作
if(value1 <= ta1 && value2 <= ta2)
{
// 少し後退して状況を確認する
led_lighting(10, 10, 100); // LED L,R点灯
scope_drive(88, 130, 80, 200); // スコープ下向き
move_back(500); // 後退
move_stop(500); // 停止
led_lighting(0, 0, 10); // LED消灯
後退した位置で測距モジュールの入力値を取得します。
// 測距モジュール入力値取得
value1 = analogRead(analogPin1);
value2 = analogRead(analogPin2);
DMS1がテーブル端を検出した時には、LED Rを点灯→左旋回→停止→スコープを中立位置→LED消灯します。
// DMS1(R側センサ)がテーブル端を検出した時
if(value1 <= ta1 && value2 > ta2)
{
led_lighting(0, 10, 100); // LED R点灯
move_left_rapid(1100); // 急速左旋回
move_stop(500); // 停止
scope_np(500); // スコープ中立位置
led_lighting(0, 0, 10); // LED消灯
}
DMS2がテーブル端を検出した時には、LED Lを点灯→右旋回→停止→スコープを中立位置→LED消灯します。
// DMS2(L側センサ)がテーブル端を検出した時
if(value1 > ta1 && value2 <= ta2)
{
led_lighting(10, 0, 100); // LED L点灯
move_right_rapid(1000); // 急速右旋回
move_stop(500); // 停止
scope_np(500); // スコープ中立位置
led_lighting(0, 0, 10); // LED消灯
}
DMS1, DMS2共にテーブル端ではないと判断したときには、スコープを左下方および右下方へ向けて、それぞれのDMS0の値を取得し、DMS0の値が大きい方が走行領域が広いと判断して、走行領域の広い方へ旋回します。
// DMS1, DMS2共にテーブル端ではないと判断した時
if(value1 > ta1 && value2 > ta2)
{
// DMS0(障害物検出センサ)で回りの状況を確認する
move_back(380); // 後退
move_stop(500); // 停止
scope_drive(133, 130, 80,1000); // スコープ左下方
value0L = analogRead(analogPin0); // 障害物検知センサ入力値取得
scope_drive(43, 130, 80, 1000); // スコープ右下方
value0R = analogRead(analogPin0); // 障害物検知センサ入力値取得
scope_np(500); // スコープ中立位置
// 左右の領域が広い方へ旋回
if(value0L > value0R)
{
move_left_rapid(1100);
}
if(value0L <= value0R)
{
move_right_rapid(1000);
}
}
}
}
}
}
}
// 関数の定義
// 複合動作関数
今回始業点検を関数としてみました。スコープを中立位置にし、DMS2の入力値をテーブル端検出しきい値と比較して大きければLED Lを点灯、次にDMS1の入力値をテーブル端検出しきい値と比較して大きければLED LRを点灯、スコープを上向きにしてから中立位置へ動かして、OKのようなうなずきの動作を行います。
// 始業点検足元確認（測距モジュールの入力値確認）
void startup_inspection()
{
scope_np(1000); // スコープ中立位置
// 左側確認
while(value2 <= ta2)
{
value2 = analogRead(analogPin2); // DMS2(L側センサ)入力値取込
}
led_lighting(10, 0, 1000); // LED L点灯
// 右側確認
while(value1 <= ta1)
{
value1 = analogRead(analogPin1); // DMS1(R側センサ)入力値取込
}
// 始業点検完了合図
led_lighting(10, 10, 1000); // LED L, R点灯
scope_drive(88, 50, 80, 1000);
scope_np(1000);
}
ほぼ期待通りの動きになっているようです。
次は【17】いろいろな動き
