Arduino簡単二足歩行ロボットで超音波センサーを使って障害物を回避する。

こんにちはRoboTakaoです。ようこそ「極力ローコスト ロボット製作 ブログ」にお越しくださいました。

今回は久々に自分のロボットネタです。

以前紹介した超音波距離計を使って、障害物を回避したいと思います。

以前の記事はこちらを参照してください。

センサーを取り付けた状態のロボット




ビニールテープ固定で無理やりです。

ロボット詳細はこちらを参照してください。

ロジック

ロジックは簡単

１）前進
２）前方２０cm以内に障害物を検知
３）右に向いて前方の障害物の距離を確認
４）左に向いて前方の障害物の距離を確認
５）後ろに下がる
６）左右の障害物のまでの距離を比較して遠い方に方向転換する。

結果の動画

狙い通りに動いているようです。



ただし、どうも斜め壁を図るの難しいみたいです。超音波の反射が元の位置まで戻って来にくい体と推定されます。

スケッチ

#include

Servo myservo2;
Servo myservo3;
Servo myservo4;
Servo myservo5;

#define echo 9 // Echo Pin
#define trig 8 // Trig Pin
double Duration = 0; //受信した間隔
double Distance = 0; //距離
double Right_D;
double Left_D;

// RU,RB,LU,LB
int ang0[4]={90,89,85,87};
int ang[16][4]={
{0,0,0,0},
{0,0,0,0},
{0,0,0,0},
{0,0,0,0},
{0,0,0,0},
{0,0,0,0},
{0,0,0,0},
{0,0,0,0},
{0,0,0,0},
{0,0,0,0},
{0,0,0,0},
{0,0,0,0},
{0,0,0,0},
{0,0,0,0},
{0,0,0,0},
{0,0,0,0}};
int stp[2][4]={{0,0,0,0},{0,0,0,0}};
int fw[12][4]={
{0,0,0,0},
{0,-15,0,-15},
{-15,-15,-15,-15},
{-15,0,-15,0},
{-15,15,-15,15},
{15,15,15,15},
{15,0,15,0},
{15,-15,15,-15},
{-15,-15,-15,-15},
{-15,0,-15,0},
{-15,15,-15,15},
{0,15,0,15}};
int rh[12][4]={
{0,0,0,0},
{0,-15,0,-20},
{-15,-15,-10,-20},
{-15,0,-10,0},
{-15,20,-10,15},
{0,20,-10,15},
{0,0,-10,0},
{0,-15,-10,-20},
{-10,-15,-5,-20},
{-10,0,5,0},
{-10,20,-5,15},
{0,20,-5,15}};
int lh[12][4]={
{0,0,0,0},
{0,20,0,15},
{10,20,15,15},
{10,0,15,0},
{10,-15,15,-20},
{10,-15,0,-20},
{10,0,0,0},
{10,20,0,15},
{5,20,10,15},
{5,0,10,0},
{5,-15,10,-20},
{5,-15,0,-20}};
int bw[12][4]={
{0,0,0,0},
{0,-15,0,-20},
{15,-15,15,-20},
{15,0,15,0},
{15,25,15,15},
{-15,25,-15,15},
{-15,0,-15,0},
{-15,-15,-15,-20},
{15,-15,15,-20},
{15,0,15,0},
{15,25,15,15},
{0,25,0,15}};
int delection = 0;
int ts=300;
int td=10;
int a[4],b[4];
int m=0;

void setup()
{
myservo2.attach(2);
myservo3.attach(3);
myservo4.attach(4);
myservo5.attach(5);
Serial.begin(9600);
pinMode(echo,INPUT);
pinMode(trig,OUTPUT);
}

void loop()
{
// delection = Serial.read();
delection = 70;

measure_distance();
Serial.println(Distance);

if(Distance<20){
right();
measure_distance();
Right_D = Distance;
left();
left();
measure_distance();
Left_D = Distance;
if(Right_D > Left_D){
back();
right();
right();
}
else{
back();
left();
left();
}
}
Serial.print("Right:");
Serial.print(Right_D);
Serial.print(" Left:");
Serial.println(Left_D);

switch (delection) {
case 70: // F FWD
Serial.println("FWD") ;
forward();
break;
case 76: // L Left
Serial.println("Left") ;
left();
break;
case 82: // R Right
Serial.println("Right") ;
right();
break;
case 66: // B back
Serial.println("Back") ;
back();
break;
default: // S Stay
Serial.println("Stay") ;
stay();
break;
}

}

void forward()
{
for (int k=0; k <=3; k++){
ang[0][k] = ang[m][k];
}
for (int i=0; i <=11; i++){
for (int j=0; j <=3; j++){
ang[i+1][j] = fw[i][j];
}
}
walk(12);
}

void left()
{
for (int k=0; k <=3; k++){
ang[0][k] = ang[m][k];
}
for (int i=0; i <=11; i++){
for (int j=0; j <=3; j++){
ang[i+1][j] = lh[i][j];
}
}
walk(12);
}

void right()
{
for (int k=0; k <=3; k++){
ang[0][k] = ang[m][k];
}
for (int i=0; i <=11; i++){
for (int j=0; j <=3; j++){
ang[i+1][j] = rh[i][j];
}
}
walk(12);
}

void back()
{
for (int k=0; k <=3; k++){
ang[0][k] = ang[m][k];
}
for (int i=0; i <=11; i++){
for (int j=0; j <=3; j++){
ang[i+1][j] = bw[i][j];
}
}
walk(12);
}

void stay()
{
for (int k=0; k <=3; k++){
ang[0][k] = ang[m][k];
}
for (int i=0; i <=2; i++){
for (int j=0; j <=3; j++){
ang[i+1][j] = stp[i][j];
}
}
walk(2);
}

void walk(int st)
{
m = st;
for (int i=1; i <=st; i++){
for (int j=0; j <=3 ; j++){
a[j] = ang[i][j] - ang[i-1][j];
b[j] = ang[i-1][j];
}
for (int k=0; k <=td-1 ; k++){
myservo2.write(ang0[0]+a[0]*k/td+b[0]);
myservo3.write(ang0[1]+a[1]*k/td+b[1]);
myservo4.write(ang0[2]+a[2]*k/td+b[2]);
myservo5.write(ang0[3]+a[3]*k/td+b[3]);
delay(ts/td);
}
}
}

void measure_distance()
{
digitalWrite(trig,HIGH); //超音波を出力
delayMicroseconds(10); //10μsパルスを出す
digitalWrite(trig,LOW);
Duration = pulseIn(echo,HIGH); //センサからパルス間隔を取得
if (Duration > 0) {
Distance = Duration/57.7; // パルス間隔から距離を計算
}
}


それでは今回はこの辺で失礼します！ありがとうございました。

Tweet

スポンサードリンク