1. バグ

④ラジコンがバックしない⁉(半分解決！)

症状：モーターアンプでモーターが逆転しない

原因：恐らくサーボライブラリの.attach関数の初期設定が微妙(調査中)

　　　もしくはモーターアンプ内で急にバック出来ない様に制御されている？

対策：バックする前にスロットルをニュートラルにし「deray(500);」した後バックした所バック可能になりました！・・・手動運転でバックしません、恐らくニュートラル時もデータがばらついて止まっているとみなされないのでしょう・・・。

⑤電源ONでラジコンが走る⁉(調整中)

症状：電源を入れると全部のサーボが思いっきり動く

原因：volatile int Val = {0,0,0,0};・・・初期値全部”0”

対策： volatile int Val = {94,94,94,94};にしましたがうまくいきません。

⑥荷台がラジコン本体に食い込む(調整中)

症状：荷台サーボの動作角度が100°位と想定の90°より多い

原因：そういうもの・・・

⑧サーボが激しくブルブルする時が有る(解決！)

症状：センサーから距離30±1cmに障害物が有る時ブルブルが発生する。

原因：測距センサーの値は30cmの時に±1cm程度のバラつきが有り、制御が切り替わるポイント(30cm)に障害物が有るとデータのバラつきで超高速にサーボの角度が切り替わりサーボがブルブルする。

対策：制御が切り替わる特定距離±3cm位の時に制御を変更させない様にする。if文沢山使った10行以上のプログラムになりそう・・・。

対策：反応が早すぎたのが原因でしたのでメインループ内で「deray(200);」した所サーボのブルブルは解決しました！

2. プログラム

作り途中ですがプログラムはこんな感じになってます。

// ラジコンをArduinoで自動運転！(20160718 NOBのArduino日記)
#include <Servo.h>                   //サーボライブラリを呼び出す
Servo LpOut;                            //接続するサーボの名前をLpOutにする
Servo RsOut;                            //接続するサーボの名前をRS Outにする
Servo ThOut;                            //接続するサーボの名前をThOutにする
Servo FsOut;                            //接続するサーボの名前をFsOutにする
volatile unsigned long UpNew[] = {0, 0, 0, 0};
volatile unsigned long UpOld[] = {0, 0, 0, 0};
volatile unsigned long DownNew[] = {0, 0, 0, 0};
volatile unsigned long DownOld[] = {0, 0, 0, 0};
volatile byte Val[] = {27, 94, 94, 94};                      //測定した開度
byte FSOut[27] = {94, 62, 30, 127, 62, 30, 160, 30, 30, 127, 94, 62, 127, 94, 94, 160, 94, 94, 160, 127, 94, 160, 94, 94, 160, 94, 94}; //左30,中94,右160
byte THOut[27] = {30, 30, 62, 62, 62, 62, 62, 62, 62, 30, 30, 62, 62, 127, 127, 62, 127, 127, 62, 62, 127, 62, 127, 127, 62, 127, 127}; //前進30,停止94,バック160
byte RSOut[27] = {94, 127, 160, 62, 127, 160, 30, 160, 160, 62, 94, 127, 62, 94, 94, 30, 94, 94, 30, 62, 94, 30, 94, 94, 30, 94, 94}; //右30,中94,左160
byte Speed = 4; //自動運転のスピードを1/Speedにする(Speed=1が一番早い、Speed=3だと1の1/3のスピードにダウン)
byte MinimumSpeed = 70; //自動運転時の最低スロットル開度
byte THOutAj = 0;
byte Auto1 = 40; //1:27 2:40 3:53 4:67 5:80 6:94 7:107 8:120 9:134 10:147 11:160 (範囲：各値±6)
byte Auto2 = 53;
byte Auto3 = 67;
byte Auto4 = 80;

void setup() {
  for (int j = 0; j < 4; j++) {   //iが0から1ずつ増加し{}内を4回繰り返す
    pinMode(j, INPUT_PULLUP);//割込監視0～3ピンをプルアップし入力に設定
    pinMode(j + 4, OUTPUT); //モーターに接続された4～7ピンを出力に設定
  }
  attachInterrupt(digitalPinToInterrupt(1), LpIn, CHANGE);//各ピン割込時実行
  attachInterrupt(digitalPinToInterrupt(0), RsIn, CHANGE);
  attachInterrupt(digitalPinToInterrupt(2), ThIn, CHANGE);
  attachInterrupt(digitalPinToInterrupt(3), FsIn, CHANGE);
  LpOut.attach(4, 1910, 2190); //各ピンにサーボ名を割当て出力ピンに設定
  RsOut.attach(5, 1910, 2190);
  ThOut.attach(6, 1910, 2190);
  FsOut.attach(7, 1910, 2190);
}

void loop() {
  byte Pattern = State();            //State関数を実行し動作ﾊﾟﾀｰﾝ番号を取得
  if ((Angle(0) < Auto1 - 6) || (Auto4 + 6 < Angle(0))) { //手動運転
    LpOut.write(Angle(0));  //各ピンにサーボ制御信号を出力
    RsOut.write(Angle(1));
    ThOut.write(Angle(2));
    if (90 <= THOut[Pattern] && THOut[Pattern] <= 94) {
      THOut[94];
    }
    FsOut.write(Angle(3));
  } else {                                      //自動運転
    if (Auto1 - 6 <= Angle(0) && Angle(0) <= Auto1 + 6) {
      Speed = 4;
    }
    if (Auto2 - 6 <= Angle(0) && Angle(0) <= Auto2 + 6) {
      Speed = 3;
    }
    if (Auto3 - 6 <= Angle(0) && Angle(0) <= Auto3 + 6) {
      Speed = 2;
    }
    if (Auto4 - 6 <= Angle(0) && Angle(0) <= Auto4 + 6) {
      Speed = 1;
    }
    LpOut.write(Angle(0));  //各ピンにサーボ制御信号を出力
    RsOut.write(RSOut[Pattern]);
    if (THOut[Pattern] == 127) {
      Back();
    } else {
      THOutAj = ((THOut[Pattern]) - 94) / Speed + 94;
      if (THOutAj > MinimumSpeed) {
        THOutAj = MinimumSpeed;
      } ThOut.write(THOutAj);
    }
    FsOut.write(FSOut[Pattern]);
    delay(100);                           //100ms(1秒)待ちます
  }
}

void LpIn() {
  if (digitalRead(1) == LOW) {
    DownOld[0] = DownNew[0];  //Lpの信号を保存
    DownNew[0] = micros();
  } else {
    UpOld[0] = UpNew[0];
    UpNew[0] = micros();
  }
}
void RsIn() {
  if (digitalRead(0) == LOW) {
    DownOld[1] = DownNew[1];  //Rsの信号を保存
    DownNew[1] = micros();
  } else {
    UpOld[1] = UpNew[1];
    UpNew[1] = micros();
  }
}
void ThIn() {
  if (digitalRead(2) == LOW) {
    DownOld[2] = DownNew[2];  //Thの信号を保存
    DownNew[2] = micros();
  } else {
    UpOld[2] = UpNew[2];
    UpNew[2] = micros();
  }
}
void FsIn() {
  if (digitalRead(3) == LOW) {
    DownOld[3] = DownNew[3];  //Fsの信号を保存
    DownNew[3] = micros();
  } else {
    UpOld[3] = UpNew[3];
    UpNew[3] = micros();
  }
}
int Angle(byte i) {  //受信した信号からサーボ角度を計算する関数
  if (UpNew[i] <= DownNew[i]) { //もしDownNewの更新が新しかったら{}内を実行
    Val[i] = ((((DownNew[i] - UpNew[i]) * 10000) / ((UpNew[i] - UpOld[i])) * 0.01) - 6) * 22.25;
  } else {                              //もしUpNewの更新が新しかったら{}内を実行
    Val[i] = ((((DownNew[i] - UpOld[i]) * 10000) / (UpNew[i] - UpOld[i]) * 0.01) - 6) * 22.25;
  } return Val[i];
}

byte State() { //3つのセンサーの状態から動作パターン番号を返す関数
  int L = 0, S = 0, R = 0, i = 0;
  byte A = 115, B = 175, C = 176;
  for (byte i = 0; i < 50; i++) {           //測距ｾﾝｻｰの値を50回読み平均値を出す
    L = L + analogRead(0); S = S + analogRead(1); R = R + analogRead(2);
  }
  L = L / 50, S = S / 50, R = R / 50, i = 0;
  if (A <= L && L <= B) {
    i = i + 9; //左ｾﾝｻｰの状況判断
  } if (C <= L) {
    i = i + 18;
  }
  if (A <= S && S <= B) {
    i = i + 3; //正面ｾﾝｻｰの状況判断
  } if (C <= S) {
    i = i + 6;
  }
  if (A <= R && R <= B) {
    i = i + 1; //右ｾﾝｻｰの状況判断
  } if (C <= R) {
    i = i + 2;
  }
  return i;
}

void Back() {
  ThOut.write(127);//急停止
  delay(500);
  ThOut.write(94);//一度停止
  delay(500);//0.5秒間待つ(必要！)
  FsOut.write(94);
  RsOut.write(94);
  ThOut.write(127);
  delay(500);  //0.5秒間バックする
  ThOut.write(94);
  delay(500);  //0.5秒間待つ
  if (analogRead(0) > analogRead(2)) {
    FsOut.write(160); RsOut.write(30);
  } else {
    FsOut.write(30); RsOut.write(160);
  }
  ThOut.write(70);  //広い方に逃げる
  delay(500);  //0.5秒間前進する
}