Arduinoを用いたロボットカーの制御プログラムの記述方法

Question

私は、Arduinoを使って道を走る自立走行型のロボットカーを作っています。

動きの流れは、
1.車の前後と壁までの距離を測る
2.距離の長い方向へ進む(前の方が長ければ前進、後ろの方が長ければ後進)
3.進んだ方向の距離が3cm以下になったら停止(前進したなら前の距離、後進したなら後ろの距離)
4.その場で左周りに回転
5.1〜4を繰り返す
です。

しかし、このプログラムでは、前後の壁の真ん中で前進と後進をし、最終的には真ん中から動かなくなってしまいます。
コンパイルしてもエラーは出ないので、書き方が間違っているのだと思いますが、どこがどのように間違っているのかわからないので教えていただきたいです。

以下がコードです。

#define echo 2
#define trig 3
#define ECHO 8
#define TRIG 9

double DurF = 0;
double DisF = 0;
double DurB = 0;
double DisB = 0;

double SF = 0;
double SB = 0;

void setup() {
  Serial.begin(9600);
  pinMode(echo, INPUT);
  pinMode(trig, OUTPUT);
  pinMode(ECHO, INPUT);
  pinMode(TRIG, OUTPUT);
  for (int P = 30; P <= 37; P++) {
    pinMode(P, OUTPUT);
  }
}

void loop() {
  Distance();
  if (DisF > DisB) {
    SF = DisF / 2;
    Serial.print("SF");
    Serial.println(SF);
    for (SF > 0; SB--;) {
      GoForward();
    }
  }
  else if (DisB > DisF) {
    SB = DisB / 8;
    Serial.print("SB");
    Serial.println(SB);
    for (SB > 0; SB--;) {
      GoBack();
    }
  }
  Stop();
  TurnLeft();
}

void Distance() {
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  DurF = pulseIn(echo, HIGH);
  if (DurF > 0) {
    DurF = DurF / 2;
    DisF = DurF * 340 * 100 / 1000000;
  }
  delay(20);

  digitalWrite(TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG, LOW);
  DurB = pulseIn(ECHO, HIGH);
  if (DurB > 0) {
    DurB = DurB / 2;
    DisB = DurB * 340 * 100 / 1000000;
  }
  delay(50);

  Serial.print(DisF);
  Serial.print("    ");
  Serial.println(DisB);
}

void GoForward() {
  HiL(1, 1, 0, 0, 0, 0, 1, 1);
  HiL(0, 1, 1, 0, 0, 1, 1, 0);
  HiL(0, 0, 1, 1, 1, 1, 0, 0);
  HiL(1, 0, 0, 1, 1, 0, 0, 1);
}

void GoBack() {
  HiL(0, 0, 1, 1, 1, 1, 0, 0);
  HiL(1, 0, 0, 1, 1, 0, 0, 1);
  HiL(1, 1, 0, 0, 0, 0, 1, 1);
  HiL(0, 1, 1, 0, 0, 1, 1, 0);
}

void TurnLeft() {
  for (int t = 0; t <= 100; t++) {
    HiL(1, 1, 0, 0, 1, 1, 0, 0);
    HiL(0, 1, 1, 0, 0, 1, 1, 0);
    HiL(0, 0, 1, 1, 0, 0, 1, 1);
    HiL(1, 0, 0, 1, 1, 0, 0, 1);
  }
}

void Stop() {
  HiL(0, 0, 0, 0, 0, 0, 0, 0);
  delay(6);
}

void HiL(int A, int B, int C, int D, int E, int F, int G, int H) {
  digitalWrite(30, A);
  digitalWrite(31, B);
  digitalWrite(32, C);
  digitalWrite(33, D);
  digitalWrite(34, E);
  digitalWrite(35, F);
  digitalWrite(36, G);
  digitalWrite(37, H);
  delay(3);
}

モーターには28BYJ-48のステッピングモーターを使用しています。
よろしくお願い致します。

Comments

[jimbe]

Arduino は分からないのですが
> for (SF > 0; SB--;) {
> GoForward();
> }
や
> for (SB > 0; SB--;) {
> GoBack();
> }
の for 文は間違っていませんか?

Answer 1

そりゃloopの中でいつも距離比較してればそうなるでしょう。
いちど比較して、この方向、と決まったら比較するのはやめなくちゃ

Comments

[Tadateru]

なるほど、その通りだと思います。
それはどうやったら実現できますか？

[y_waiwai]

変数に数字入れてそれで動作を切り替えればよろしい

１．距離計測＆比較、方向を決める
２．決められた方向へ走る
３．距離が3cmになるまで待つ
４．停止する
５．繰り返し

などなど