以前に紹介した自律走行ロボット「TURTLE ONE」の自立走行性を改良したものを作ってみようかと考えています。
初号機では一応衝突回避運動までは出来ますが袋小路に入るとその図体の大きさからか脱出できなかったりしていました。
小回りのきく機体をということで初号機を一旦分解して部品どりから始めます。作ったものはコレクションとして残しておきたいのですが、予算上と置き場所の関係で再生利用します。
・走行部の構成
初号機の二つのクローラを分解し補助クローラ部とミッションを利用することにします。
ミッションとクローラ部品
その他に部品箱にモータードライバーTA7291Pが2個、マイコンもPromicroの中華版、超音波距離センサーGP2Y0A21YK、サーボモーターSG90がありましたのでほとんど走行系の車両を作れる部品は揃っています。
ともかく車体部の構想図を三次元CADで描いてみました。
・衝突回避センサ部の構想
初号機では衝突回避のための超音波センサーはサーボに取り付けて首振りさせるので比較的高い位置しか検知できませんでした。さらに初号機ではテーブル端のような場所では落下してしまう欠点がありました。それで補助的に前方と前方下部だけを監視するセンサーを二個追加します。赤外線式のKKHMF LM393というセンサーがアマゾンにありましたので追加購入します。送料無料で一個110円ほどでした。
これを車体の出来るだけ低い位置に前方監視と斜め下監視のためにつけます。
メインのGP2Y0A21YKは左右首振りだけでは芸がないので二関節追加して動きの表情を豊かにしてみました。当初の構想では前下方をこのセンサで監視させようとしたのですが上手くいきませんでした。タイミングのずれで衝突してしまうのです。完成後に追加改良が必要な項目です。
三次元CAD上で動きを確認しておきます。
フリーソフトでもここまで出来るとはいい時代です。私の現役時代にこんなものがあればもっと仕事の効率が上がったのにと思ってみたりします。
カバーをつけてみました。
・回路図
回路上気になるのはサーボモータの消費電流です。SG-90の消費電流について気になって色々調べていますが一説には500mAという説もあって３台同時操作はしないとしても制御系に影響を与えそうです。５V電源の独立回路を作っておいたほうが良さそうなので追加します。ついでにバッテリー出口にダイオードも追加します。これは電池を逆挿入した際に機器の損傷を防ごうというものです。これまでこういった事故はありませんでしたが念のため。
その後の改良で電源はCR123Aというリチウムイオン電池2個の予定でしたが電池寿命を考えるとDCモータとセンサの駆動電源は別にしたほうが良さそうなのでさらに二本を追加しました。
・全体構想図
・走行部の製作
・センサ部の製作
カバーを取り付ける前にセンサ部の動きを確認してみました。
・基板の製作
・機体完成
前面
背部
・試運転
三つの測距センサーによって障害を検出した時には停止、後退して右旋回、その他では前進という簡単なロジックのプログラムとして試験運転です。
まずは今回追加したテーブル端を検知し回避するという場面での試運転です。
うまく作動しているようです。この後は試験運転を続けて不具合を調整していくことになります。心配していた電池の寿命ですがフル充電した2300mAh のCR232電池二個で約8分間の連続走行は可能ですが、最終的には二個追加しました。
・スケッチの完成
試運転を繰り返して問題点を洗い出しプログラムを修正し一応の完成状態となりました。プログラムの流れは以下のようなものです。
１．固定の測距センサー二個にて前方と斜め下方を検知し、障害物があれば停止、後進の後右旋回
２．首振りする測距センサーにて障害物との距離が10㎝以下となれば一旦停止少しバック障害物と反対方向に旋回，二回繰り返してもだめなら大きく右旋回
３．首振りする測距センサーにて障害物との距離が20㎝以下となれば障害物と反対方向に旋回
４．なにもなければひたすら前進
長くなりますがプログラムを掲載しておきます。
//Self Driving Vehicle ver.2 2017.12.30
//赤外線距離センサ（GP2YOA21）周囲の障害物検知
//赤外線近接センサ（LM393） 障害物検知の補助として二個、斜め下方と低位置前方を測距
#include <VarSpeedServo.h>
// サーボを定義
VarSpeedServo servo1,servo2,servo3;
int servo1_min,servo1_max,servo1_mean; //首振り
int servo2_min,servo2_max,servo2_mean; //首傾げ
int servo3_min,servo3_max,servo3_mean; //首上下
int position_of_servo1[] ={85,30,85,140};
int len_1 = 4;
int position_of_servo2[] ={60,80};
int len_2 = 2;
int position_of_servo3[] ={60,100};
int len_3 = 2;
// モーターを定義
const int motorA_1 = 7;
const int motorA_2 = 8;
const int PWM_motorA = 9;
const int motorB_1 = 14;
const int motorB_2 = 16;
const int PWM_motorB =10;
//赤外線距離センサ（GP2YOA21）を定義
const int value = A2;
int distance = 0;
//赤外線近接センサ(LM393)を定義
const int LM393_1 = A0;
const int LM393_2 = A1;
int LM393_1value;
int LM393_2value;
//シグナルランプを定義
const int SignalLump = A3;
void setup() {
servo1.attach(3);
servo2.attach(5);
servo3.attach(6);
servo1_min=30,servo1_max=140,servo1_mean=85;
servo2_min=60,servo2_max=100,servo2_mean=80;
servo3_min=60,servo3_max=100,servo3_mean=90;
servo1.write(servo1_mean);
servo2.write(servo2_mean);
servo3.write(servo3_mean);
Serial.begin(9600);
}
void loop(){
for (int i = 0; i < len_1; i++){
servo1.write(position_of_servo1[i], 100, true); //サーボ位置決め
distance = (6787/(analogRead(value)-3))-4;
Serial.print(distance);
Serial.println(“cm”);
LM393_1value = digitalRead(LM393_1);
LM393_2value = digitalRead(LM393_2);
Serial.print (“LM393_1 = “);
Serial.println(LM393_1value);
Serial.print (“LM393_2 = “);
Serial.println(LM393_2value);
if (LM393_1value == 1 || LM393_2value == 0) { //落下または衝突回避のため停止、後進の後、右旋回
return_head();
SignalLump_ONOFF();
stop();
shake_head();
delay(300);
return_head();
updown_head();
delay(300);
return_head();
back(50);
delay(500);
turn_right(50);
delay(600);
}
else if (distance < 10) { //GP2YOA21と障害物との距離が10cm未満の場合、一旦停止少しバック障害物と反対方向に旋回，二回繰り返してもだめなら大きく右旋回
for (int n = 0; n< 2; n++){
if (distance > 85) { //障害物が左の場合
stop();
delay(100);
back(50);
delay(300);
turn_right(50);
delay(600);
}
else { //障害物が右の場合
stop();
delay(100);
back(50);
delay(300);
turn_left(50);
delay(600);
}
turn_right(50);
delay(600);
}
}
else if (distance < 20) { //GP2YOA21と障害物との距離が20cm未満の場合：障害物と反対方向に旋回
if (distance > 85) { //障害物が右の場合
turn_left(50);
delay(300);
}
else { //障害物が左の場合
turn_right(50);
delay(300);
}
}
else { //前方に障害物ない場合は前進
forward(75);
delay(100);
}
}
}
void forward(int x) {
Serial.println(“forward”);
digitalWrite(SignalLump, HIGH);
analogWrite(PWM_motorA, x);
analogWrite(PWM_motorB, x);
digitalWrite(motorA_1, HIGH);
digitalWrite(motorA_2, LOW);
digitalWrite(motorB_1, HIGH);
digitalWrite(motorB_2, LOW);
}
void turn_right(int x){
Serial.println(“turn right”);
digitalWrite(SignalLump, LOW);
analogWrite(PWM_motorA, x);
analogWrite(PWM_motorB, x);
digitalWrite(motorA_1, LOW);
digitalWrite(motorA_2, HIGH);
digitalWrite(motorB_1, HIGH);
digitalWrite(motorB_2, LOW);
delay(300);
}
void turn_left(int x){
Serial.println(“turn left”);
digitalWrite(SignalLump, LOW);
analogWrite(PWM_motorA, x);
analogWrite(PWM_motorB, x);
digitalWrite(motorA_1, HIGH);
digitalWrite(motorA_2, LOW);
digitalWrite(motorB_1, LOW);
digitalWrite(motorB_2, HIGH);
delay(300);
}
void stop(){
Serial.println(“stop”);
digitalWrite(SignalLump, HIGH);
digitalWrite(motorA_1, LOW);
digitalWrite(motorA_2, LOW);
digitalWrite(motorB_1, LOW);
digitalWrite(motorB_2, LOW);
}
void back(int x){
Serial.println(“back”);
digitalWrite(SignalLump, HIGH);
analogWrite(PWM_motorA, x);
analogWrite(PWM_motorB, x);
digitalWrite(motorA_1, LOW);
digitalWrite(motorA_2, HIGH);
digitalWrite(motorB_1, LOW);
digitalWrite(motorB_2, HIGH);
delay(300);
}
void shake_head(){
for (int l = 0; l < len_2; l++){
servo2.write(position_of_servo2[l], 100, true);
delay(300);
}
}
void updown_head(){
for (int m = 0; m < len_3; m++){
servo3.write(position_of_servo3[m], 100, true);
delay(300);
}
}
void return_head(){
servo1.write(servo1_mean);
servo2.write(servo2_mean);
servo3.write(servo3_mean);
}
void SignalLump_ONOFF() {
if(millis() % 500 > 250) {
analogWrite(SignalLump, 255);
}
else {
analogWrite(SignalLump, 0);
}
}
・完成試験
テーブル端を検知して落下を防ごうと設けた斜め下を測距するためのセンサーですが思わぬ効用があります。前方下部を監視するために追加したセンサーでも発見できない地面付近の背の低い障害物にはこれまでは乗り上げてしまって動きがとれなくなっていましたが、この斜め下を検知するセンサーによりテーブル端と同じく地面がないと判断して停止、後退、右旋回で脱出できるようになりました。