自動走行車にBluetoothの機能を追加してラジコン化したい

スケッチ

#define PIN_IN_A1 9
#define PIN_IN_A2 10
#define PIN_IN_B1 15
#define PIN_IN_B2 11
#define TRG_PIN 2
#define ECHO_PIN 3
long randomNum = 0;
double Duration = 0;
double Distance = 0;
#include <SoftwareSerial.h>
SoftwareSerial mySerial(16, 14);//RX, TX

void setup() {
  pinMode(PIN_IN_A1, OUTPUT);
  pinMode(PIN_IN_A2, OUTPUT);
  pinMode(PIN_IN_B1, OUTPUT);
  pinMode(PIN_IN_B2, OUTPUT);
  pinMode(TRG_PIN, OUTPUT);
  digitalWrite(TRG_PIN, LOW);
  pinMode(ECHO_PIN, INPUT);
  randomSeed(analogRead(0));
  mySerial.begin(9600);
}

void forward() {
  digitalWrite(PIN_IN_A1, HIGH);
  digitalWrite(PIN_IN_A2, LOW);
  digitalWrite(PIN_IN_B1, HIGH);
  digitalWrite(PIN_IN_B2, LOW);
}

void brake() {
  digitalWrite(PIN_IN_A1, LOW);
  digitalWrite(PIN_IN_A2, LOW);
  digitalWrite(PIN_IN_B1, LOW);
  digitalWrite(PIN_IN_B2, LOW);
}

void reverse() {
  digitalWrite(PIN_IN_A1, LOW);
  digitalWrite(PIN_IN_A2, HIGH);
  digitalWrite(PIN_IN_B1, LOW);
  digitalWrite(PIN_IN_B2, HIGH);
}

void coast() {
  digitalWrite(PIN_IN_A1, HIGH);
  digitalWrite(PIN_IN_A2, HIGH);
  digitalWrite(PIN_IN_B1, HIGH);
  digitalWrite(PIN_IN_B2, HIGH);
}

void turnLeft() {
  digitalWrite(PIN_IN_A1, LOW);
  digitalWrite(PIN_IN_A2, LOW);
  digitalWrite(PIN_IN_B1, HIGH);
  digitalWrite(PIN_IN_B2, LOW);
}

void turnRight() {
  digitalWrite(PIN_IN_A1, HIGH);
  digitalWrite(PIN_IN_A2, LOW);
  digitalWrite(PIN_IN_B1, LOW);
  digitalWrite(PIN_IN_B2, LOW);
}

void rotateLeft() {
  digitalWrite(PIN_IN_A1, LOW);
  digitalWrite(PIN_IN_A2, HIGH);
  digitalWrite(PIN_IN_B1, HIGH);
  digitalWrite(PIN_IN_B2, LOW);
}

void rotateRight() {
  digitalWrite(PIN_IN_A1, HIGH);
  digitalWrite(PIN_IN_A2, LOW);
  digitalWrite(PIN_IN_B1, LOW);
  digitalWrite(PIN_IN_B2, HIGH);
}

double measureDistance() {
  digitalWrite(TRG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRG_PIN, LOW);
  Duration = pulseIn(ECHO_PIN, HIGH);
  if (Duration > 0) {
    Duration = Duration / 2; //往復距離を半分にする
    Distance = Duration * 340 * 100 / 1000000; // 音速を340m/sに設定
  }
  return Distance;
}

void avoid() {
  reverse();
  delay(1000);
  coast();
  rotateRight();
  delay(500);
  coast();
}

void basicMotion(char c) {

  String s = String(c);

  mySerial.println(s);
  
  if (s == "8") {
    forward();
  } else if (s == "2") {
    reverse();
  } else if (s == "7") {
    turnLeft();
  } else if (s == "9") {
    turnRight();
  } else if (s == "4") {
    rotateLeft();
  } else if (s == "6") {
    rotateRight();
  } else if (s == "5") {
    brake();
  }
}

boolean withinDistance() {
  double d = 0;
  d = measureDistance();
  if (d < 10) {
    return true;
  } else {
    return false;
  }
}

void loop() {

  if (mySerial.available() > 0) {
    char c = mySerial.read();
    basicMotion(c);
  }
  if (withinDistance()) {
    avoid();
  }
}