Arduinoでロボットを作ってみました！【14】テーブルから落ちないロボットPart1
測距モジュールの特性が分かりましたので、落下防止用測距モジュールがテーブル端を検出したら、回避動作をするプログラムを検討してみます。
測距モジュールの取付位置を見直す
13章までの測距モジュールは下図のような取付状態です。
測距モジュールがテーブル端を検出する位置でのテーブル端とトラックとの距離 SFは、右側で10mm、左側で20mmといったところです。ざっくり動作を確認してみたところ、テーブル端を検出してから停止するまでに、トラックがテーブル端からはみ出して、ロボットが落下してしまいそうでした。測距モジュールとトラックとの位置が近過ぎるようです（特に右側）。
トラックから測距モジュールを離すように取付位置を見直すことにしました。
測距モジュールのLight emiterから真直ぐに出た光線が、左右でほぼ対称の位置に到達するように、左右で測距モジュールの取付角度を変えています。測距モジュールからテーブルまでの距離は59.2mmで、入力値が最大になる距離です。この取付位置の測距モジュールで、テーブル端－トラック間距離と入力値との関係を再度測定します。
測距モジュールがテーブル端を検出する位置でのテーブル端とトラックとの距離は、左右共に40mmは取れそうです。また、左右の測距モジュールの特性をほぼ合わせることができています。
テーブル端と平行な姿勢では、左右共にテーブル端から25mmでテーブル端を検出できそうです。
テーブル端検出から停止動作の確認
見直しを行った落下防止用測距モジュールの特性が分かりましたので、テーブル端を検出してから、ロボットが停止するまでの動作を確認してみます。動作確認に使用したスケッチは以下の通りです。
初期設定でServoライブラリの読込、タクトスイッチ、測距モジュールの定義、各ピンの入出力設定を行います。
// 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" 落下防止 R)
#define analogPin2 2 // 置き換え (analogPin2→"2" 落下防止 L)
int value0 = 0; // 変数 value0はint型, value0をｸﾘｱ(0)
int value1 = 0; // 変数 value1はint型, value1をｸﾘｱ(0)
int value2 = 0; // 変数 value2はint型, value2をｸﾘｱ(0)
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(β)
}
ここからメインの処理
void loop() // メインの処理
{
LEDを消灯し、スコープを少し振ってから中立位置に移動します。
// LED点灯
led_lighting(0, 0, 500);
// スコープを中立位置に移動
scope_drive(70, 50, 70, 500);
scope_np(500);
タクトスイッチ1（PBS1）が押されるのを待ちます。
// PBS1 ON待ち
s1 = digitalRead(PBS1);
while(s1!=0)
{
s1 = digitalRead(PBS1);
}
PBS1 ONを検出するとwhileループに入り込んで、直進動作を繰り返します。
// PBS1 ON (s1=0)の時
if(s1==0)
{
while(1) // breakするまで繰り返す
{
move_straight_i; // 何もなければひたすら直進
測距モジュールの入力値を監視しLRのどちらかの入力値がしきい値以下になったらモーター急停止、LRのLED点灯し、breakでwhileループを抜けます。
// 測距モジュール入力値
value0 = analogRead(analogPin0);
value1 = analogRead(analogPin1);
value2 = analogRead(analogPin2);
// 落下防止センサがテーブル端を検出した時に停止
if(value1<=600 || value2<=600)
{
// モーター急停止
digitalWrite(11, HIGH);
digitalWrite(10, HIGH);
digitalWrite(6, HIGH);
digitalWrite(9, HIGH);
// LED LR点灯
digitalWrite(5, HIGH);
digitalWrite(3, HIGH);
delay(2000);
break;
}
}
}
}
以下は関数を定義しています。
// 関数の定義
// スコープ中立位置移動
void scope_np(int ms)
{
servo0.attach(2);
servo1.attach(4);
servo2.attach(7);
servo0.write(88);
servo1.write(70);
servo2.write(90);
delay(ms);
servo0.detach();
servo1.detach();
servo2.detach();
}
// スコープ駆動
void scope_drive(int deg0, int deg1, int deg2, int ms)
// deg0(θ)設定範囲：88°±45°(43°～133°)
// deg1(α)設定範囲：70°±60°(10°～130°)
// deg1(β)設定範囲：90°±70°(20°～160°)
{
servo0.attach(2);
servo1.attach(4);
servo2.attach(7);
servo0.write(deg0);
servo1.write(deg1);
servo2.write(deg2);
delay(ms);
servo0.detach();
servo1.detach();
servo2.detach();
}
// LEDコントロール
void led_lighting(int LED_L, int LED_R, int ms)
{
analogWrite(5, LED_L);
analogWrite(3, LED_R);
delay(ms);
}
// 前進(無限)
void move_straight_i()
{
analogWrite(11, 233);
digitalWrite(10, LOW);
analogWrite(6, 255);
digitalWrite(9, LOW);
}
// 停止(時間指定)
void move_stop(int ms)
{
digitalWrite(11, LOW);
digitalWrite(10, LOW);
digitalWrite(6, LOW);
digitalWrite(9, LOW);
delay(ms);
}
このスケッチで動かしてみると、
テーブル端から40mm程度の位置では、落下防止用測距モジュールがテーブル端を検出して停止動作に入るものの、モータードライバーのブレーキモードでも、ロボットが停止するまでに、テーブル端からはみ出すほどまで、動いてしまいます。
停止位置がここまできてしまうと、なかなか危なっかしいので、停止させる方法を見直してみます。落下防止測距モジュールがテーブル端を検出したら、いきなり後進動作を200msec行ってから停止する動作にしてみます。
// 落下防止センサがテーブル端を検出した時に停止
if(value1<=600 || value2<=600)
{
move_back(200);
move_stop(2000);
break;
}
ここで、move_back()は後進動作を行う関数で以下のように定義します。
// 後進(直進)
void move_back(int ms)
{
digitalWrite(11, LOW);
digitalWrite(10, HIGH);
digitalWrite(6, LOW);
digitalWrite(9, HIGH);
delay(ms);
}
このように変更したスケッチで動かしてみると、
テーブル端から余裕を持って停止させることができます。
テーブル端検出用しきい値の最適値を決める
上記の試験では、テーブル端を検出用のしきい値を＜600＞としています。これは、テーブル端－トラック間距離と入力値との関係を測定した結果から、できるだけ安全を見て設定した値ですが、この設定で繰り返し動かしていると、前進せずにいきなり停止動作を始めることが時々あります。当初、PBS1を押した際にロボットの前方が沈んで、測距モジュールの値に影響しているのかと考えて、PBS1を押した後にLEDを2000msec点灯する動作を追加したのですが、状況は変わりません。しきい値を＜500＞にすると期待通りの動きとなります。
原因を探るために、シリアルモニタで測距モジュールの入力値を取得する機能をスケッチに追加して調べてみます。
初期設定は前述のスケッチと同じですが、シリアルモニタの通信速度設定を追加します。
：
void setup() //初期設定
{
：
Serial.begin(9600); // シリアルモニタの通信速度を設定する
}
void loop() // メインの処理
{
// LED点灯
led_lighting(0, 0, 500);
// スコープを中立位置に移動
scope_drive(70, 50, 70, 500);
scope_np(500);
whileループを抜けた時の測距モジュールの入力値をシリアルモニタへ出力します。
// 測距モジュール入力値
Serial.print("A1 =");
Serial.println(value1);
Serial.print("A2 =");
Serial.println(value2);
delay(1000);
PBS1が押されるまで待機します。
// PBS1 ON待ち
s1 = digitalRead(PBS1);
while(s1!=0)
{
s1 = digitalRead(PBS1);
}
PBS1を押したことが分かり易いように、またロボットの姿勢を落ち着かせるために、LEDを2000msec点灯します。
// LED点灯
led_lighting(10, 10, 2000);
// PBS1 ON (s1=0)の時
if(s1==0)
{
while(1) // breakするまで繰り返す
{
move_straight_i(); // 何もなければひたすら直進
// 測距モジュール入力値
value0 = analogRead(analogPin0);
value1 = analogRead(analogPin1);
value2 = analogRead(analogPin2);
// 落下防止センサがテーブル端を検出した時に停止
if(value1<=600 || value2<=600)
{
move_back(200);
move_stop(2000);
break;
}
}
}
}
繰り返し動かしてみると、落下防止用測距モジュールの入力値がしきい値の＜600＞以下となる場合があることが分かります。この場合にはロボットは前進することなく停止動作に入ります。また、しきい値を＜500＞にし、テーブル端を検出して停止した際の測距モジュールの入力値も調べてみました。この結果から、入力値が538は前進可、457はテーブル端検出と判断して停止と考えると、しきい値として＜500＞程度が良い値ではないかと思います。
以上で、テーブル端を検出した際の停止動作と最適なしきい値が決まりましたので、次の章で連続して動くようにプログラムを検討していきます。
