Arduinoでロボットを作ってみました！【15】テーブルから落ちないロボットPart2
テーブル端を検出した時の回避動作
落下防止センサでテーブル端を検出した時の回避動作について検討してみます。
＜左側の落下防止センサが検出した時＞
左側の落下防止センサの入力値がしきい値以下になったら、14章で説明した急停止（200msec後進→停止）→500msec後進→右旋回で回避します。
＜右側の落下防止センサが検出した時＞
右側の落下防止センサの入力値がしきい値以下になったら、急停止（200msec後進→停止）→500msec後進→左旋回で回避します。
＜左右両方の落下防止センサが検出した時 Pattern 1＞
左右両方の落下防止センサがしきい値以下になったら、急停止（200msec後進→停止）→500msec後進→停止して、その位置で再度落下防止センサの入力値を確認し、左側の落下防止センサの入力値がしきい値以下であれば右旋回で回避します。
＜左右両方の落下防止センサが検出した時 Pattern 2＞
左右両方の落下防止センサがしきい値以下になったら、急停止（200msec後進→停止）→500msec後進→停止、その位置で再度落下防止センサの入力値を確認し、右側の落下防止センサの入力値がしきい値以下であれば左旋回で回避します。
＜左右両方の落下防止センサが検出した時 Pattern 3＞
左右両方の落下防止センサがしきい値以下になったら、急停止（200msec後進→停止）→500msec後進→停止して、その位置で再度落下防止センサの入力値を確認します。左右両方の入力値がしきい値より大きい時は、さらに500msec後進→停止して、この位置でスコープを左下に向けて障害物検出センサの入力値value0Lを取得します。次に、スコープを右下に向けて障害物検出センサの入力値value0Rを取得します。value0Lとvalue0Rとの大きさを比較して、
value0L ＞ value0R → 左旋回
value0L ≦ value0R → 右旋回
の動作で回避します。これは、テーブルのより広い方へ回避動作を行わせるために、障害物検出センサを用いて、テーブルの状況を確認する動作です。
角部での回避動作
ロボットがテーブルの角部に行ってしまった時の回避動作について、検討してみます。
左図のように、ロボットがテーブルの角部近くで入射角度 60°で進入してきたとします。右側の落下防止センサでテーブル端を検出しますので、急停止→500msec後進→左旋回の回避動作をします。図の112mmは500msec後進動作時の移動量（電源電圧：5.06V時の実測値）です。ここで旋回角度を50°とすると、左旋回後に直進し、図の縦のテーブル端を左側の落下防止センサで検出して、急停止→500msec後進→右旋回の回避動作をして直進、図の横のテーブル端を右側の落下防止センサで検出。この動きの繰り返しで、角部から脱出できなくなります。
右図では、角部での入射状況は左図と同じですが、旋回角度が70°の時の動きです。この場合は角部から脱出することができます。
角部から脱出するためには、旋回角度が入射角度よりも大きい必要があります。したがいまして、回避動作における旋回角度は90°よりも大きな100°程度に設定しようと思います。
駆動時間と旋回角度との関係測定
回避動作における旋回動作は、左旋回ならば、左トラックを後進回転、右トラックを前進回転、右旋回はその逆で行います。そのための関数を定義します。
// 前進(左急速旋回)
void move_left_rapid(int ms)
{
digitalWrite(11, LOW);
digitalWrite(10, HIGH);
digitalWrite(6, HIGH);
digitalWrite(9, LOW);
delay(ms);
}
// 前進(右急速旋回)
void move_right_rapid(int ms)
{
digitalWrite(11, HIGH);
digitalWrite(10, LOW);
digitalWrite(6, LOW);
digitalWrite(9, HIGH);
delay(ms);
}
この関数で動かした際の動作時間と旋回角度との関係を測定します。
測定結果は、
この結果から、左旋回動作時間を1000msec, 右旋回動作時間を900msecと設定します。
動作確認
上記の検討結果をスケッチに反映させて、実際に動かしてみます。
// 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; // 障害物検知センサ入力値
int value0L = 0; // 左下方を向いた時の障害物検出センサ入力値
int value0R = 0; // 右下方を向いた時の障害物検出センサ入力値
int value1 = 0; // 落下防止センサＲ入力値
int value2 = 0; // 落下防止センサＬ入力値
int threshold = 500; // テーブル端検出しきい値
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_lighting(0, 0, 500);
// スコープ中立位置
scope_np(500);
// PBS1 ON待ち
s1 = digitalRead(PBS1);
while(s1!=0)
{
s1 = digitalRead(PBS1);
}
// 始業点検足元確認（測距モジュールの入力値確認）
// 左側確認
while(value2 < threshold)
{
scope_drive(133, 130, 90, 1000);
value2 = analogRead(analogPin2);
}
scope_drive(88, 45, 90, 1000);
led_lighting(10, 0, 1000);
// 右側確認
while(value1 < threshold)
{
scope_drive(43, 130, 90, 1000);
value1 = analogRead(analogPin1);
}
scope_drive(88, 45, 90, 1000);
led_lighting(10, 10, 1000);
// スコープ中立位置
scope_np(500);
// PBS1 ON (s1=0)の時
if(s1==0)
{
while(1) // breakするまで繰り返す
{
move_straight_i(); // 何もなければひたすら直進
// PBS2 ONの時 while(1)ループを抜ける
s2 = digitalRead(PBS2);
if(s2==0)
{
move_stop(1000);
break;
}
// 測距モジュール入力値
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);
// 落下防止センサＬがテーブル端を検出した時の回避動作
if(value1 > threshold && value2 <= threshold)
{
scope_drive(133, 130, 90, 1000); // スコープ左下方
move_back(500); // 500msec後進
move_right_rapid(900); // 右急速旋回
move_stop(1000); // 1000msec停止
scope_np(1000); // スコープ中立位置
}
// 落下防止センサＲがテーブル端を検出した時の回避動作
else if(value1 <= threshold && value2 > threshold)
{
scope_drive(43, 130, 90, 1000); // スコープ右下方
move_back(500); // 500msec後進
move_left_rapid(1000); // 左急速旋回
move_stop(1000); // 1000msec停止
scope_np(1000); // スコープ中立位置
}
// 落下防止センサＬＲ両方がテーブル端を検出した時の回避動作
else if(value1 <= threshold && value2 <= threshold)
{
// 少し後進して状況を確認する
scope_drive(88, 130, 90, 1000); // スコープ下方
move_back(500); // 500msec後進
move_stop(1000); // 1000msec停止
value1 = analogRead(analogPin1);
value2 = analogRead(analogPin2);
// 落下防止センサＬがテーブル端を検出した時
if(value1 > threshold && value2 <= threshold)
{
move_right_rapid(900); // 右急速旋回
move_stop(1000); // 1000msec停止
scope_np(1000); // スコープ中立位置
}
// 落下防止センサＲがテーブル端を検出した時
else if(value1 <= threshold && value2 > threshold)
{
move_left_rapid(1000); // 左急速旋回
move_stop(1000); // 1000msec停止
scope_np(1000); // スコープ中立位置
}
// 落下防止センサＬＲ共にテーブル端でないと判断した時
else if(value1 > threshold && value2 > threshold)
{
// 障害物検出センサで回りの状況を確認する
move_back(500); // 500msec後進
move_stop(1000); // 1000msec停止
scope_drive(133, 130, 90,1000); // スコープ左下方
value0L = analogRead(analogPin0); // 障害物検出センサ入力値取得
scope_drive(43, 130, 90, 1000); // スコープ右下方
value0R = analogRead(analogPin0); // 障害物検出センサ入力値取得
scope_np(1000); // スコープ中立位置
// 左右の領域が広い方へ旋回
if(value0L > value0R) // 左側の領域が広いと判断
{
move_left_rapid(1000); // 左急速旋回
}
if(value0L <= value0R) // 右側の領域が広いと判断
{
move_right_rapid(900); // 右急速旋回
}
}
}
}
}
}
}
// 関数の定義
// スコープ中立位置移動
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_right_rapid(int ms)
{
digitalWrite(11, HIGH);
digitalWrite(10, LOW);
digitalWrite(6, LOW);
digitalWrite(9, HIGH);
delay(ms);
}
// 前進(左急速旋回)
void move_left_rapid(int ms)
{
digitalWrite(11, LOW);
digitalWrite(10, HIGH);
digitalWrite(6, HIGH);
digitalWrite(9, LOW);
delay(ms);
}
// 後進(直進)
void move_back(int ms)
{
digitalWrite(11, LOW);
digitalWrite(10, HIGH);
digitalWrite(6, LOW);
digitalWrite(9, HIGH);
delay(ms);
}
// 停止(時間指定)
void move_stop(int ms)
{
digitalWrite(11, LOW);
digitalWrite(10, LOW);
digitalWrite(6, LOW);
digitalWrite(9, LOW);
delay(ms);
}
検討通りの動きができているようです。
