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】いろいろな動き
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);
}
検討通りの動きができているようです。
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＞程度が良い値ではないかと思います。
以上で、テーブル端を検出した際の停止動作と最適なしきい値が決まりましたので、次の章で連続して動くようにプログラムを検討していきます。
Arduinoでロボットを作ってみました！【13】動作試験Part3：測距モジュールの特性確認
今回のロボットには、スコープユニット部に障害物検出用１個、前方下部左右に落下防止用２個の測距モジュールを搭載しています。それぞれの測距モジュールからの入力値の特性を測定します。測定時の電源電圧は 5.29Vでした。
落下防止用測距モジュール
入力値の測定は、ArduinoとPCとをUSBでつないで、シリアルモニタで行います。
＜高さの変化vs入力値＞
ロボットの下にスペーサを挿入して、ロボットをテーブル面から離していきます。高さの増加分と左右の測距モジュールからの入力値との関係を測定しました。
A1は右側の測距モジュールからの入力値、A2は左側の測距モジュールからの入力値です。
＜テーブル端からの距離vs入力値＞
テーブル端からトラックまでの距離を変えて、左右の測距モジュールからの入力値を測定します。床からテーブルまでの距離は760mmです。
左右の測距モジュールで特性に違いが出た原因は、取付姿勢の違いによると考えます。
Light emiter（発光側）がトラックから遠い側にある左側の測距モジュールの方が、より早くテーブルの端を検出できそうです。
＜テーブルの端ぎりぎりに置いた場合の入力値＞
トラックとテーブルの端が一致するように置き、測距モジュールからの入力値を測定します。
障害物検出用測距モジュール
テーブル面から障害物検出用測距モジュールまでの設計上の高さは 130.3mmですので、ターゲットとして、80mm x 170mmの厚紙（板目表紙）を容易し、テーブル面に対して垂直に立て、障害物検出用測距モジュールからの距離を変えて、A0入力値を測定します。
以上で、落下防止用および障害物検出用測距モジュールの特性が分かりましたので、この結果を以降の動作プログラムに使っていきます。
Arduinoでロボットを作ってみました！【12】動作試験Part2：走行試験
直進走行試験
前進と後進を行って、真直ぐに走行できるかを確認してみます。動作に使用したスケッチは、以下の通りで、首関節をホームポジションに移動→前進4秒→停止5秒→後進5秒→停止5秒を繰り返します。DCモータの駆動はdigitalWriteで行います。
//ARD-R01動作確認：digitalWriteによる直線走行
#include <Servo.h> // ライブラリの読み込み<Servo.h>
Servo servo0,servo1,servo2; // Servo型の変数は「servo0」～「servo2」とする
void setup() //初期設定
{
pinMode(11, OUTPUT); //ピン11を出力に設定（LモータIN1）
pinMode(10, OUTPUT); //ピン10を出力に設定（LモータIN2）
pinMode(9, OUTPUT); //ピン9を出力に設定（RモータIN1）
pinMode(8, OUTPUT); //ピン8を出力に設定（RモータIN2）
servo0.attach(4); // Servo型の変数は「servo0(θ)」をピン4に割り当てる
servo1.attach(5); // Servo型の変数は「servo1(α)」をピン5に割り当てる
servo2.attach(6); // Servo型の変数は「servo2(β)」をピン6に割り当てる
}
void loop() //メインの処理
{
// スコープを中立位置に移動
servo0.write(90); // サーボ出力。θ角度は90°
servo1.write(80); // サーボ出力。α角度は80°
servo2.write(100); // サーボ出力。β角度は100°
delay(1000); // タイマ(1秒)
//DCモータ駆動
//前進
//Lモータ前進回転
digitalWrite(11, HIGH); //ピン11にHIGH(1)を出力
digitalWrite(10, LOW); //ピン10にLOW(0)を出力
//Rモータ前進回転
digitalWrite(9, HIGH); //ピン9にHIGH(1)を出力
digitalWrite(8, LOW); //ピン8にLOW(0)を出力
delay(4000);
//停止
digitalWrite(11, LOW);
digitalWrite(10, LOW);
digitalWrite(9, LOW);
digitalWrite(8, LOW);
delay(5000);
//後進
//Lモータ後進回転
digitalWrite(11, LOW);
digitalWrite(10, HIGH);
//Rモータ後進回転
digitalWrite(9, LOW);
digitalWrite(8, HIGH);
delay(4000);
//停止
digitalWrite(11, LOW);
digitalWrite(10, LOW);
digitalWrite(9, LOW);
digitalWrite(8, LOW);
delay(5000);
}
あまり直進性は良くないようです。
analogWriteによるDCモータ駆動を試す
analogWriteでDCモータを動かしてみて、直進性が改善するかを試してみます。DCモータ駆動のdigitalWriteをanalogWriteに変更してみます。
：
//DCモータ駆動
//前進
//Lモータ前進回転
analogWrite(11, 230); //ピン11にアナログ値 230を出力
analogWrite(10, 0); //ピン10にアナログ値 0を出力
//Rモータ前進回転
analogWrite(9, 255); //ピン9にアナログ値 255を出力
analogWrite(8, 0); //ピン8にアナログ値 0を出力
delay(4000);
：
//後進
//Lモータ後進回転
analogWrite(11, 0);
analogWrite(10, 255);
//Rモータ後進回転
analogWrite(9, 0);
analogWrite(8, 255);
delay(4000);
：
前進の直進性が改善されたために、後進の際に左側に曲がるのが目立ってきました。そこで、後進の際のRモータの回転を少し遅くしようと思い、
analogWrite(8, X);
のXの値を小さくしていったのですが、動きに変化がありません。いろいろとXの値を変化していくと、
X=128～255ではX=255と同じ動き
X≦127ではモータ停止
原因は、Arduino UNOにおいて、アナログ出力（PWM出力）ができるピンは
ピン番号：3, 5, 6, 9, 10, 11
のみ（ピン番号の後ろに～がついているピン）で、PWM出力に対応していない8番ピンにanalogWriteを設定しても、扱いはdigitalWriteと同等になってしまうためのようです。
analogWrite(8, 128～255) → digitalWrite(8, HIGH)
analogWrite(8, 0～127) → digitalWrite(8, LOW)
回路の修正
PWM出力が可能なピン番号を意識しないで回路設計をしてしまいましたので、接続ピン番号を下表のように修正します。
走行速度と瞳LEDの明るさとを可変にしたいと思いますので、モータードライバーとLEDとをPWM出力ピンにつなぎます。また、0, 1番ピンはシリアル通信用のピンなので、空けておくことにします。
analogWriteによるDCモータ駆動を試す Part2
上記の回路修正を行い、再度走行試験を行います。
//ARD-R01動作確認：analogWriteによる直線走行
#include <Servo.h> // ライブラリの読み込み<Servo.h>
Servo servo0,servo1,servo2; // Servo型の変数は「servo0」～「servo2」とする
void setup() //初期設定
{
pinMode(11, OUTPUT); //ピン11を出力に設定（LモータIN1）
pinMode(10, OUTPUT); //ピン10を出力に設定（LモータIN2）
pinMode(6, OUTPUT); //ピン6を出力に設定（RモータIN1）
pinMode(5, OUTPUT); //ピン5を出力に設定（RモータIN2）
servo0.attach(2); // Servo型の変数は「servo0(θ)」をピン2に割り当てる
servo1.attach(4); // Servo型の変数は「servo1(α)」をピン4に割り当てる
servo2.attach(7); // Servo型の変数は「servo2(β)」をピン7に割り当てる
}
void loop() //メインの処理
{
// スコープを中立位置に移動
servo0.write(90); // サーボ出力。θ角度は90°
servo1.write(80); // サーボ出力。α角度は80°
servo2.write(100); // サーボ出力。β角度は100°
delay(1000); // タイマ(1秒)
//DCモータ駆動
//前進
//Lモータ前進回転
analogWrite(11, 233); //ピン11にアナログ値 233を出力
analogWrite(10, 0); //ピン10にアナログ値 0を出力
//Rモータ前進回転
analogWrite(6, 255); //ピン6にアナログ値 255を出力
analogWrite(5, 0); //ピン5にアナログ値 0を出力
delay(4000);
//停止
digitalWrite(11, LOW);
digitalWrite(10, LOW);
digitalWrite(6, LOW);
digitalWrite(5, LOW);
delay(5000);
//後進
//Lモータ後進回転
analogWrite(11, 0);
analogWrite(10, 255);
//Rモータ後進回転
analogWrite(6, 0);
analogWrite(5, 252);
delay(4000);
//停止
digitalWrite(11, LOW);
digitalWrite(10, LOW);
digitalWrite(6, LOW);
digitalWrite(5, LOW);
delay(5000);
}
後進に対してもanalogWriteが有効となり、前進、後進共に直進性が向上しました。
瞳LEDの明るさコントロール試験
瞳LEDの配線も PWM出力ピンに配線変更を行いましたので、明るさのコントロールができるか確認してみます。首を傾けて、瞳LEDの明るさを徐々に明るくし（0→255）、徐々に暗くする（255→0）動作を行ってみます。
//ARD-R01動作確認：LED明るさ調整
#include <Servo.h> // ライブラリの読み込み<Servo.h>
Servo servo0,servo1,servo2; // Servo型の変数は「servo0」～「servo2」とする
void setup() //初期設定
{
pinMode(3, OUTPUT); //ピン3(LED R)に出力設定
pinMode(9, OUTPUT); //ピン9(LED L)に出力設定
servo0.attach(2); // Servo型の変数は「servo0(θ)」をピン2に割り当てる
servo1.attach(4); // Servo型の変数は「servo1(α)」をピン4に割り当てる
servo2.attach(7); // Servo型の変数は「servo2(β)」をピン7に割り当てる
}
void loop() //ループ
{
// スコープを中立位置に移動
servo0.write(90);
servo1.write(80);
servo2.write(100);
delay(2000);
// 上を向いて首を右に傾け、右目をゆるやかに明るくした後暗くする
servo1.write(20);
servo2.write(30);
delay(1000);
int i; //iはint型変数
for(i=0; i<=255; i=i+1)
{
analogWrite(3, i); //ピン3にアナログ値(0～255)を出力
delay(10); //タイマ(10msec)
}
for(i=255; i>=0; i=i-1)
{
analogWrite(3, i); //ピン3にアナログ値(255～0)を出力
delay(10); //タイマ(10msec)
}
// 上を向いて首を左に傾け、左目をゆるやかに明るくした後暗くする
servo1.write(20);
servo2.write(170);
delay(1000);
for(i=0; i<=255; i=i+1)
{
analogWrite(9, i); //ピン9にアナログ値(0～255)を出力
delay(10); //タイマ(10msec)
}
for(i=255; i>=0; i=i-1)
{
analogWrite(9, i); //ピン9にアナログ値(255～0)を出力
delay(10); //タイマ(10msec)
}
}
上を向いて首を右に傾け、右目をゆるやかに明るくした後暗くする動作は期待通りに実行されているのですが、上を向いて首を左に傾けた後の、左目の明るさコントロールができていません。原因を調べたところ、
”いずれかのピンがattach()されると9番,10番ピンからのPWM出力ができなくなる”
のが仕様であることが分かりました。つまり、9番ピンに接続したLED LへはPWM出力ができないためにこのような動きになってしまいました。
再度回路の修正
RCサーボも動かそうとすると、PWM出力が可能なピンは、3, 5, 6, 11の４ピンのみとなります。瞳LEDの明るさコントロールは外せませんから、走行速度制御は後進時はあきらめて前進時のみとして、接続ピン番号を下表のように修正します。
修正した回路図は下図のようになります。
スケッチも修正して、
//ARD-R01動作確認：LED明るさ調整
#include <Servo.h> // ライブラリの読み込み<Servo.h>
Servo servo0,servo1,servo2; // Servo型の変数は「servo0」～「servo2」とする
void setup() //初期設定
{
pinMode(3, OUTPUT); //ピン3(LED R)に出力設定
pinMode(5, OUTPUT); //ピン5(LED L)に出力設定
servo0.attach(2); // Servo型の変数は「servo0(θ)」をピン2に割り当てる
servo1.attach(4); // Servo型の変数は「servo1(α)」をピン4に割り当てる
servo2.attach(7); // Servo型の変数は「servo2(β)」をピン7に割り当てる
}
void loop() //ループ
{
// スコープを中立位置に移動
servo0.write(90);
servo1.write(80);
servo2.write(100);
delay(2000);
// 上を向いて首を右に傾け、右目をゆるやかに明るくした後暗くする
servo1.write(20);
servo2.write(30);
delay(1000);
int i; //iはint型変数
for(i=0; i<=255; i=i+1)
{
analogWrite(3, i); //ピン3にアナログ値(0～255)を出力
delay(10); //タイマ(10msec)
}
for(i=255; i>=0; i=i-1)
{
analogWrite(3, i); //ピン3にアナログ値(255～0)を出力
delay(10); //タイマ(10msec)
}
// 上を向いて首を左に傾け、左目をゆるやかに明るくした後暗くする
servo1.write(20);
servo2.write(170);
delay(1000);
for(i=0; i<=255; i=i+1)
{
analogWrite(5, i); //ピン5にアナログ値(0～255)を出力
delay(10); //タイマ(10msec)
}
for(i=255; i>=0; i=i-1)
{
analogWrite(5, i); //ピン5にアナログ値(255～0)を出力
delay(10); //タイマ(10msec)
}
}
これでようやく期待通りの動きができるようになりました。
８の字走行試験
DCモータの速度制御とLEDの明るさ制御をトータルで確認するために、８の字走行試験を行います。今回のスケッチでは、関数の定義やRCサーボを駆動する際に、動かしたらservo.detach()することで、停止時に「ジジ」とか「カリ」と鳴いたり、時々震えるような現象を抑えることも試みます。
時計回りに一定の半径を保って走り、中間位置で停止、首を振って瞳LEDの明るさを変化させ、反時計回りに一定の半径を保って走るという動作を行います。
//ARD-R01動作確認：８の字走行試験
#include <Servo.h> // ライブラリの読み込み<Servo.h>
Servo servo0,servo1,servo2; // Servo型の変数は「servo0」～「servo2」とする
void setup() //初期設定
{
pinMode(3, OUTPUT); //ピン3に出力設定(LED R)
pinMode(5, OUTPUT); //ピン5に出力設定(LED L)
pinMode(11, OUTPUT); //ピン11に出力設定（LモータIN1）
pinMode(10, OUTPUT); //ピン10に出力設定（LモータIN2）
pinMode(6, OUTPUT); //ピン6に出力設定（RモータIN1）
pinMode(9, OUTPUT); //ピン9に出力設定（RモータIN2）
servo0.attach(2); // Servo型の変数は「servo0(θ)」をピン2に割り当てる
servo1.attach(4); // Servo型の変数は「servo1(α)」をピン4に割り当てる
servo2.attach(7); // Servo型の変数は「servo2(β)」をピン7に割り当てる
}
void loop() //ループ
{
//停止
move_stop(5000);
// スコープを中立位置に移動
scope_np(2000);
// 時計回り走行
move_straight(500);
move_curve(255, 50, 8000);
move_straight(500);
move_stop(2000);
// 上を向いて首を右に傾け、右目をゆるやかに明るくした後暗くする
scope_drive(80, 20, 30, 1000);
int i; //iはint型変数
for(i=0; i<=255; i=i+1)
{
analogWrite(3, i); //ピン3にアナログ値(0～255)を出力
delay(10); //タイマ(10msec)
}
for(i=255; i>=0; i=i-1)
{
analogWrite(3, i); //ピン3にアナログ値(255～0)を出力
delay(10); //タイマ(10msec)
}
// 上を向いて首を左に傾け、左目をゆるやかに明るくした後暗くする
scope_drive(110, 20, 170, 1000);
for(i=0; i<=255; i=i+1)
{
analogWrite(5, i); //ピン5にアナログ値(0～255)を出力
delay(10); //タイマ(10msec)
}
for(i=255; i>=0; i=i-1)
{
analogWrite(5, i); //ピン5にアナログ値(255～0)を出力
delay(10); //タイマ(10msec)
}
//反時計回り走行
move_straight(500);
move_curve(65, 255, 9000);
move_straight(500);
move_stop(2000);
}
//関数の定義
//前進（直進）
void move_straight(int ms)
{
analogWrite(11, 233);
digitalWrite(10, LOW);
analogWrite(6, 255);
digitalWrite(9, LOW);
delay(ms);
}
//前進（曲線走行）
void move_curve(int MLS, int MRS, int ms)
{
analogWrite(11, MLS);
digitalWrite(10, LOW);
analogWrite(6, MRS);
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);
}
//スコープ中立位置移動
void scope_np(int ms)
{
servo0.attach(2);
servo1.attach(4);
servo2.attach(7);
servo0.write(90);
servo1.write(80);
servo2.write(100);
delay(ms);
servo0.detach();
servo1.detach();
servo2.detach();
}
//スコープ駆動
void scope_drive(int deg0, int deg1, int deg2, int ms)
//deg0(θ)設定範囲：45～135°
//deg1(α)設定範囲：20～140°
//deg1(β)設定範囲：30～170°
{
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();
}
反時計回りの動きの安定性が少々悪いですが、DCモータの速度制御とLEDの明るさ制御ができていることを確認できました。
Arduinoでロボットを作ってみました！【11】首関節の構造見直し
RCサーボの制動性能
首痙攣の治療法を検討するに当たって、先ずは首を動かしているMicro Servo 9g SG90の制動性能を確認します。
SG90に円盤を取付け、おもりを載せることで慣性モーメントを変化させ、SG90を設定した角度まで動かした際に円盤を停止させることができるかを観察します。
実際の動きを動画で示します。
この結果から、
SG90で制動可能な慣性モーメント ＜ 2.36E-5 kgm^2
を設計仕様として対策を検討していきます。
スコープユニットの慣性モーメント
スコープユニットの重心回りの慣性モーメントは
α軸方向：3.9E-5 kgm^2 ＞ 2.36E-5 kgm^2
β軸方向：3.3E-5 kgm^2 ＞ 2.36E-5 kgm^2
であり、αステージ、βステージ共に回転軸を重心位置と一致させたとしても、SG90での直接駆動（ダイレクトドライブ）は不可ということが分かります。従って、SG90に減速機構を追加して、増力することを考えます。
αβステージ構造
αステージ、βステージそれぞれの駆動特性を見ていきます。
αステージ
βステージ
以上の結果からαステージ、βステージ共に駆動軸換算等価慣性モーメントはSG90で制動可能な慣性モーメント2.36E-5 kgm^2よりも十分に小さく、首が痙攣する問題を解決できそうです。
固有振動数の確認
今回の改良設計でのスコープ取付テーブルの固有振動数を確認しておきます。
前回と同様に両端固定梁の固有振動数計算に置き換えて計算すると、
スコープ取付テーブルの固有振動数： f = 68.9Hz
となり、振動源の周波数 50Hzから 18.9Hz離れていますので、こちらも問題にならないかと思います。
首関節の動作試験
中立位置の設定
αステージ、βステージ駆動用のRCサーボ SG90の設定角度を90°とすると、少し傾いているようです。
そこで、αステージ、βステージが水平になるように、SG90の設定角度を調整してみました。
αステージ駆動用SG90設定角度：80°
βステージ駆動用SG90設定角度：100°
この角度を中立位置として、αステージ、βステージ駆動用のSG90の可動範囲を以下のように設定します。
αステージ駆動用SG90：20°~140°
βステージ駆動用SG90：30°~170°
首振り動作試験
以下のスケッチで首振り動作試験を行ってみました。
#include <Servo.h> // ライブラリの読み込み<Servo.h>
Servo servo0,servo1,servo2; // Servo型の変数は「servo0」～「servo2」とする
void setup() // 初期設定
{
servo0.attach(4); // Servo型の変数は「servo0(θ)」をピン4に割り当てる
servo1.attach(5); // Servo型の変数は「servo1(α)」をピン5に割り当てる
servo2.attach(6); // Servo型の変数は「servo2(β)」をピン6に割り当てる
}
void loop() // メインの処理
{
// 中立位置に戻す
servo0.write(90); // θ中立位置90°
delay(1000);
servo1.write(80); // α中立位置80°
delay(1000);
servo2.write(100); // β中立位置100°
delay(1000);
// θステージの首振り
servo0.write(45);
delay(1000);
servo0.write(135);
delay(1000);
servo0.write(45);
delay(1000);
servo0.write(135);
delay(1000);
servo0.write(90);
delay(1000);
// αステージの首振り
servo1.write(20);
delay(1000);
servo1.write(140);
delay(1000);
servo1.write(20);
delay(1000);
servo1.write(140);
delay(1000);
servo1.write(80);
delay(1000);
// βステージの首振り
servo2.write(30);
delay(1000);
servo2.write(170);
delay(1000);
servo2.write(30);
delay(1000);
servo2.write(170);
delay(1000);
servo2.write(100);
delay(1000);
}
今回首関節の構造を見直すことにより、以前のように首が痙攣することなく、滑らかに動かすことができるようになりました。
Arduinoでロボットを作ってみました！【10】動作試験Part1：首関節が痙攣してる!!
スコープユニット動作確認
最初にθαβステージを中心位置にするため下記のスケッチを実行しました。
#include <Servo.h> // ライブラリの読み込み<Servo.h>
Servo servo0,servo1,servo2; // Servo型の変数は「servo0」～「servo2」とする
void setup() // 初期設定
{
servo0.attach(4); // Servo型の変数は「servo0」をピン4(θ軸)に割り当てる
servo1.attach(5); // Servo型の変数は「servo1」をピン5(α軸)に割り当てる
servo2.attach(6); // Servo型の変数は「servo2」をピン6(β軸)に割り当てる
}
void loop() // メインの処理
{
servo0.write(90); // サーボ出力。Servo0角度は90°
delay(1000); // タイマ(1秒)
servo1.write(80); // サーボ出力。Servo1角度は80°
delay(1000); // タイマ(1秒)
servo2.write(82); // サーボ出力。Servo2角度は82°
delay(1000); // タイマ(1秒)
}
ところが、いきなり首関節がはげしく痙攣し始めました。
当初、電気的なノイズによる誤動作かと思い、RCサーボの配線経路変更など行いましたが、現象は収まりません。そこで、スコープユニットの関節部の固有振動数が振動源の周波数と一致する共振現象によるものと推定して、スコープユニットの上におもりを載せてみました。おもりは、呼び6x30x1.5のステンレス製平座金を８個計61.5gです。
対策の効果をより分かりやすくするために、αテーブルに積極的に加振力を与えるように90°→80°→90°→・・・のように動かしています。
：
void loop() // メインの処理
{
servo0.write(90); // サーボ出力。Servo0角度は90°
delay(1000); // タイマ(1秒)
servo1.write(90); // サーボ出力。Servo1角度は90°
delay(500); // タイマ(0.5秒)
servo1.write(80); // サーボ出力。Servo1角度は80°
delay(1000); // タイマ(1秒)
servo2.write(82); // サーボ出力。Servo2角度は82°
delay(1000); // タイマ(1秒)
}
おもりを載せることで、αテーブルの振動は収まりますので、共振が原因とも考えられます。
それでは、振動源はなんでしょうか。αテーブルに使用しているRCサーボSG90のPWM周期が20ms (50Hz)ですので、これが振動源の候補となります。そこで、αステージから上で、固有振動数が50Hz近傍になる箇所を探していきます。
先ず、テーブルの回転軸に対して剛体振り子と考えるとどうでしょうか。剛体振り子の固有振動数は、
この式を使ってαテーブル、βテーブルについて計算すると、
αテーブルの回転軸に対する固有振動数は2.09Hz、βテーブルの回転軸に対する固有振動数は1.77Hzです。振動源の50Hzから大分離れていますので、これが共振の要因とは考えずらいようです。
次にαステージテーブルの固有振動数を計算してみます。
αステージテーブルはこのような形をしていますが、このままでは固有振動数が計算しずらいので、両端固定はりの固有振動数計算に置き換えて検討します。
材質、幅b、高さhをαステージテーブルと同じにして、中央に同一の集中荷重＜ここではβステージ＋スコープユニットの合計質量＝60.5gより0.593N＞を負荷した際にたわみが同一となるように両端固定はりの長さLを決め、これをαステージテーブルの等価モデルとして固有振動数を計算します。
上記の式に
材質：板目表紙（縦目）
縦弾性係数：3.89 GPa
密度：749.1 kg/m^3
はりの長さ L：44.94mm = 0.04494m
はりの幅 b：24mm = 0.024m
はりの高さ h：0.7mm = 0.0007m
負荷質量 m：60.5g = 0.0605kg（βステージ＋スコープユニット合計質量）
を入れて計算すると
αステージテーブルの固有振動数 fa = 48.5Hz
αステージテーブルが共振して首関節が痙攣!!しているようです。
βステージテーブルの固有振動数は負荷質量 mをスコープユニットのみの質量 45gとして fb = 56.2Hzとなります。
共振対策
αステージテーブルの固有振動数を振動源の周波数50Hzからずらすために、スコープユニット内におもりを追加します。
おもりは、呼び6x30x1.5（実測値：内径φ6.7 x 外径φ29.7 x 厚み1.5）のステンレス製平座金８個計61.5gです。
重心位置を計算すると、αステージに関しては、おもりがない場合よりも重量バランスも良くなります。
実際におもりを追加して、振動がどのようになるかを確認しました。おもりは左右に１枚ずつ増やしていき、＜おもりなし＞＜左右各１個計２個＞＜左右各２個計４個＞＜左右各３個計6個＞＜左右各４個計８個＞の５通りをαステージ、βステージの振動に着目して試験を行いました。
αステージの振動試験は下記のスケッチで動かします。
#include <Servo.h> // ライブラリの読み込み<Servo.h>
Servo servo0,servo1,servo2; // Servo型の変数は「servo0」～「servo2」とする
void setup() // 初期設定
{
servo0.attach(4); // 「servo0」→ピン4(θ軸)
servo1.attach(5); // 「servo1」→ピン5(α軸)
//servo2.attach(6); // 「servo2」→ピン6(β軸)：無効
}
void loop() // メインの処理
{
servo0.write(90); // Servo0角度：90°
delay(1000); // タイマ(1秒)
servo1.write(80); // Servo1角度は80°
delay(1000); // タイマ(1秒)
}
βステージの振動試験は下記のスケッチで動かします。
#include <Servo.h> // ライブラリの読み込み<Servo.h>
Servo servo0,servo1,servo2; // Servo型の変数は「servo0」～「servo2」とする
void setup() // 初期設定
{
servo0.attach(4); // 「servo0」→ピン4(θ軸)
//servo1.attach(5); // 「servo1」→ピン5(α軸)：無効
servo2.attach(6); // 「servo2」→ピン6(β軸)
}
void loop() // メインの処理
{
servo0.write(90); // Servo0角度：90°
delay(1000); // タイマ(1秒)
servo2.write(87); // Servo2角度：82°+5°
delay(1000); // タイマ(1秒)
servo2.write(77); // Servo2角度：82°-5°
delay(1000); // タイマ(1秒)
}
βステージ中心位置±5°首振り運動です。
試験結果をまとめると、
×：振動が収束しない △：振動するが収束する
このように、固有振動数を共振周波数から離しただけでは、振動を抑えることができません。この要因はRCサーボSG90の非力な発生トルクに対して、慣性モーメントが大き過ぎて制動しきれていないためではと考えられます。
αテーブル回転軸回りの慣性モーメントがβテーブルに比べて、大分大きくなっています。
βステージの振動試験の結果を見ると、おもりの個数が少なくて慣性モーメントが小さいところで振動が収束せず、おもりを増やしていくと揺れながらも振動が収束しています。この要因はおもり0個と２個では固有振動数が振動源の周波数に近いために共振が発生しおり、おもりを増やしていくと固有振動数が振動源の周波数から離れていくために、共振が発生しずらくなって、SG90がなんとか頑張って制動していると考えて良いかと思います。
αテーブルでは固有振動数を下げるためにおもりを追加すると、慣性モーメントが巨大になり、SG90が制動できなくなっているものと考えられます。
以上の検討から、首関節の痙攣を収めるためには、固有振動数を振動源の周波数50Hzから上または下に10～15Hz以上離す、また回転軸回りの慣性モーメントをできるだけ小さく5E-5 (kg･m^2)以下程度にする必要がありそうです。
首関節の構造を大幅に見直さなければならないようです。
