Arduinoで距離センサを3つ用いたプログラム

Arduinoを使用し行き止まりが近づくと左右に距離を測り左右どちらに進むべきかをブザーで知らせ、前に一定距離以上の空間が開くと左右の距離の測定をやめるというのを繰り返すプログラム（一本道を障害物に当たらずに音のガイドで人が進むことが目的です）を作りたいのですが、下記のプログラムだと前の距離センサのみが動き、左右の距離はシリアルモニタにずっと0，00で表示されます。プログラムでどこかおかしいところはありますでしょうか？教えて下さると幸いです。（距離センサとして超音波モジュールを用いています。）

#define trigF 8
#define echoR 9
#define trigR 10
#define echoL 13
#define trigL 12
#define buza 7

double DurF=0;
double DisF=0;
double DurR=0;
double DisR=0;
double DurL=0;
double DisL=0;

void setup() {
  Serial.begin(9600);
  pinMode(echoF,INPUT);
  pinMode(trigF,OUTPUT);
  pinMode(echoR,INPUT);
  pinMode(trigR,OUTPUT);
  pinMode(echoL,INPUT);
  pinMode(trigL,OUTPUT);
  pinMode(buza,OUTPUT);
}


void DistanceForword(){
  digitalWrite(trigF,LOW);
  delayMicroseconds(2);
  digitalWrite(trigF,HIGH);
  delayMicroseconds(10);
  digitalWrite(trigF,LOW);
  DurF=pulseIn(echoF,HIGH);
  if(DurF>0){
    DurF=DurF/2;
    DisF=DurF*340*100/1000000;
  }
  Serial.print("DisF:");
  Serial.println(DisF);
  delay(20);
}

void DistanceRight(){
  digitalWrite(trigR,LOW);
  delayMicroseconds(2);
  digitalWrite(trigR,HIGH);
  delayMicroseconds(10);
  digitalWrite(trigR,LOW);
  DurF=pulseIn(echoR,HIGH);
  if(DurR>0){
    DurR=DurR/2;
    DisR=DurR*340*100/1000000;
  }
  Serial.print("DisR:");
  Serial.println(DisR);
  delay(20);
}

void DistanceLeft(){
  digitalWrite(trigL,LOW);
  delayMicroseconds(2);
  digitalWrite(trigL,HIGH);
  delayMicroseconds(10);
  digitalWrite(trigL,LOW);
  DurF=pulseIn(echoL,HIGH);
  if(DurL>0){
    DurL=DurL/2;
    DisL=DurL*340*100/1000000;
  }
  Serial.print("DisL:");
  Serial.println(DisL);
  delay(20);
}

  void TurnRight(){
    tone(buza,262,200);
    delay(200);
  }

  void TurnLeft(){
tone(buza,262,200);
    delay(100);
    tone(buza,262,200);
    delay(100);
    tone(buza,262,200);
    delay(200);
  }

void loop() {
  DistanceForword();
    if(DisF<10){
      
      digitalWrite(trigF,LOW);
      DistanceLeft();
      DistanceRight();
      while(DisF>10){
        if(DisR>20){
          TurnRight();
        }
     else if(DisL>20){
        TurnLeft();
      }
       else if((DisR<20)&&(DisL<20)){
       digitalWrite(trigF,HIGH);
       digitalWrite(trigR,LOW);
       digitalWrite(trigL,LOW);
       DistanceForword();
    }
   
｝
    }
}

intをvoidの代わりに用いても動きませんでした。距離センサの不具合でもないです。単体ならきちんと作動するので。

y_waiwai

2021/01/24 12:31

このままではコードが読みづらいので、質問を編集し、<code>ボタンを押し、出てくる’’’の枠の中にコードを貼り付けてください

行動規範の内容に同意します

回答3件

基本的にはDisXが初期値を0にしている事とpulseIn関数のタイムアウト時の戻り値が0なのにその処理をしていない、という事が原因のような気がします。
※デフォルトではタイムアウトは1秒なので、障害物が無ければその間何も変わりません。

なので、初期化の際

double DisX=30;

のように大き目の値を入れておいて、、パルスの取得も

DurX=pulseIn(echoX,HIGH,2000);//2msでタイムアウト

のようにタイムアウトを入れて

if(DurX>0) {
DurX=DurX/2;
DisX=DurX340100/1000000;
}else DisX=30;

のような処理をいれれば改善するかも。

投稿2021/01/25 01:14

nac_tnk

総合スコア463

他人が見ると簡単に見つかったりするんですけどね。

Arduino
1void DistanceRight(){
2  digitalWrite(trigR,LOW);
3  delayMicroseconds(2);
4  digitalWrite(trigR,HIGH);
5  delayMicroseconds(10);
6  digitalWrite(trigR,LOW);
7  DurF=pulseIn(echoR,HIGH);//// <-
8  if(DurR>0){
9    DurR=DurR/2;
10    DisR=DurR*340*100/1000000;
11  }
12  Serial.print("DisR:");
13  Serial.println(DisR);
14  delay(20);
15}
16
17void DistanceLeft(){
18  digitalWrite(trigL,LOW);
19  delayMicroseconds(2);
20  digitalWrite(trigL,HIGH);
21  delayMicroseconds(10);
22  digitalWrite(trigL,LOW);
23  DurF=pulseIn(echoL,HIGH);//// <-
24  if(DurL>0){
25    DurL=DurL/2;
26    DisL=DurL*340*100/1000000;
27  }
28  Serial.print("DisL:");
29  Serial.println(DisL);
30  delay(20);
31}

「同じ処理でパラメータが違うところ」をコピペで処理をしたことがプログラミングテクニックとしてはまずいところ。「コピペして違うところをなおす」行為は、間違いが紛れ込むモトです。
せっかく中身C++なので距離センサをクラスにまとめるのがいいかと思います。

以下、コンパイルは通りましたが動作チェックはしていません
(一部修正 HC-SR4は一つしか持ってないから順次で試したけど一応動くみたい 1/25)

Arduino
1typedef int Pin;
2//#defineよりは型チェックの入るconstをおススメします
3const Pin echoF = 6;
4const Pin trigF = 8;
5const Pin echoR = 9;
6const Pin trigR = 10;
7const Pin echoL = 13;
8const Pin trigL = 12;
9const Pin buza = 7;
10
11//距離センサクラス
12class DistSens {
13    Pin echo, trig;
14    String label;
15  public:
16    DistSens(Pin echo, Pin trig, String label):
17      echo(echo), trig(trig), label(label) {
18      pinMode(echo, INPUT);
19      pinMode(trig, OUTPUT);
20    }
21    double getDist() {
22      double dist=0;
23      digitalWrite(trig, LOW);
24      delayMicroseconds(2);
25      digitalWrite(trig, HIGH);
26      delayMicroseconds(10);
27      digitalWrite(trig, LOW);
28      unsigned long dur = pulseIn(echo, HIGH);
29      if (dur > 0) {
30        dist = dur / 2. * 340 * 100 / 1000000;
31      }
32      Serial.print("Dis");
33      Serial.print(label);
34      Serial.print(':');
35      Serial.println(dist);
36      delay(20);
37      return dist;
38    }
39};
40
41DistSens distF(echoF, trigF, "F");
42DistSens distR(echoR, trigR, "R");
43DistSens distL(echoL, trigL, "L");
44
45void setup() {
46  Serial.begin(9600);
47  pinMode(buza, OUTPUT);
48}
49
50void TurnRight() {
51  tone(buza, 262, 200);
52  delay(200);
53}
54
55void TurnLeft() {
56  tone(buza, 262, 200);
57  delay(100);
58  tone(buza, 262, 200);
59  delay(100);
60  tone(buza, 262, 200);
61  delay(200);
62}
63
64void loop() {
65  double DisF = distF.getDist();
66  if (DisF < 10) {
67    digitalWrite(trigF, LOW);
68    double DisL = distL.getDist();
69    double DisR = distR.getDist();
70    while (DisF > 10) {
71      if (DisR > 20) {
72        TurnRight();
73      }
74      else if (DisL > 20) {
75        TurnLeft();
76      }
77      //以下のelseに到達するのはDisR<=20かつDisL<=20だったときだけ。
78      //(DisR<20)&&(disL<20)だとDisR==20またはDisL==20のときどうなる?
79      //そもそもここでifで検査する意味はあるのか?
80      else if ((DisR < 20) && (DisL < 20)) {
81        //これ意味のある動作?
82        digitalWrite(trigF, HIGH);
83        digitalWrite(trigR, LOW);
84        digitalWrite(trigL, LOW);
85        distF.getDist();
86      }
87    }
88  }
89}