1. バグ

残りのバグ④⑤⑥について、以下の様に解決できました！

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

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

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

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

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

対策2：お騒がせ致しました・・・プロポ送信機4PLSの取説(64ページ)を見ているとATL(ブレーキ)設定が100にしていないといけないところ私の送信機は0になっていました

これではモチロンバックしません、100にした所バックするようになりました失礼いたしました

⑤電源ONでラジコンが走る⁉(解決！)

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

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

対策： volatile int Val = {37,94,94,94};に

Setup中で

LpOut.write(37);
RsOut.write(94);
ThOut.write(94);
FsOut.write(94);

した後、割り込みの初期値を無視する為に

deray(1500);

した所うまくいきました！

前回までのderay(1000)ではプロポの送信機と受信機がペアリングが完了する前にloop関数に移行してしまった為全部のサーボがめちゃくちゃに動いていましたが、deray(1500)にした所電源オン時に勝手に動くことは無くなりました。

⑥荷台がラジコン本体に食い込む(解決！)

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

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

対策：loop関数内でLpOut.write(Angle(0));を、前回の記事でメビウスKさんに教えて頂いた式を少しアレンジした所解決いたしました！

byte A0=Angle(0);

if(20<=A0 && A0<=32){LpOut.write(37);}else{LpOut.write(A0);}

2. プログラム

と言う事で最終的にバグを除去し終わったキレイな？ラジコンの自動運転プログラムはコチラ！

// ラジコンをArduinoで自動運転！(20160720 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[] = {37, 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; //自動運転時の最低スロットル開度、実走70、実験78
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); //①初期設定：1860,2140　②調整後:1910,2190　③旧：1050,2800
  LpOut.write(37);  //各ピンにサーボ制御信号を出力
  RsOut.write(94);
  ThOut.write(94);
  FsOut.write(94);
  delay(1500);        //プロポペアリング時間1500ms(1.5秒)待ち
}

void loop() {
  byte Pattern = State(), A0 = Angle(0); //State関数を実行し動作ﾊﾟﾀｰﾝ番号を取得
  if ((Angle(0) < Auto1 - 6) || (Auto4 + 6 < Angle(0))) { //手動運転
    if (20 <= A0 && A0 <= 32) {
      LpOut.write(37);
    } else {
      LpOut.write(A0);
    }
    RsOut.write(Angle(1));  //各ピンにサーボ制御信号を出力
    ThOut.write(Angle(2));
    FsOut.write(Angle(3));
  } else {                                      //自動運転
    if (Auto1 - 6 <= A0 && A0 <= Auto1 + 6) {
      Speed = 4;
    }
    if (Auto2 - 6 <= A0 && A0 <= Auto2 + 6) {
      Speed = 3;
    }
    if (Auto3 - 6 <= A0 && A0 <= Auto3 + 6) {
      Speed = 2;
    }
    if (Auto4 - 6 <= A0 && A0 <= Auto4 + 6) {
      Speed = 1;
    }
    LpOut.write(37);  //各ピンにサーボ制御信号を出力
    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]);
  }
}

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];
}

volatile byte Lval, Sval, Rval;
byte State() { //3つのセンサーの状態から動作パターン番号を返す関数
  int L = 0, S = 0, R = 0 ;
  byte A = 115, B = 175, C = 176, D = 510; //A:50cm BC:30cm D:20cm
  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;
  if (A - 5 <= L && L <= A + 5) {} else {
    if (B - 5 <= L && L <= C + 5) {} else {
      Lval = 0;  //左ｾﾝｻｰの状況判断
      if (A <= L && L <= B) {
        Lval = Lval + 9;
      } if (C <= L) {
        Lval = Lval + 18;
      }
    }
  }
  if (A - 5 <= S && S <= A + 5) {} else {
    if (B - 5 <= S && S <= C + 5) {} else {
      Sval = 0;  //正面ｾﾝｻｰの状況判断
      if (A <= S && S <= B) {
        Sval = Sval + 3;
      } if (C <= S) {
        Sval = Sval + 6;
      }
    }
  }
  if (A - 5 <= R && R <= A + 5) {} else {
    if (B - 5 <= R && R <= C + 5) {} else {
      Rval = 0;  //右ｾﾝｻｰの状況判断
      if (A <= R && R <= B) {
        Rval = Rval + 1;
      } if (C <= R) {
        Rval = Rval + 2;
      }
    }
  }
  if (D < L || D < S || D < R) {
    return 26;
  } else {
    return Lval + Sval + Rval;
  }
}

void Back() {
  ThOut.write(127);//急停止
  delay(500);//制動時間
  ThOut.write(94);//スロットルをニュートラルにする
  delay(500);//しばし待つ(重要！これをしないとバックできません！)
  FsOut.write(94); //フロントステアリングをニュートラルにする
  RsOut.write(94); //リアステアリングをニュートラルにする
  ThOut.write(127);//スロットルをバックにする
  delay(500);  //0.5秒間バックする
  ThOut.write(94);//スロットルをニュートラルにする
  delay(500);  //しばし待つ！
  if (analogRead(0) > analogRead(2)) {
    FsOut.write(160); RsOut.write(30);
  } else {
    FsOut.write(30);  //左右を確認し広い方に移動する
    RsOut.write(160);
  }
  ThOut.write(70);//スロットルをON
  delay(500);  //0.5秒間進む
}

図1：プログラム例

※ArduinoIDEリファレンス

3. まとめ

完成した感が自分の中で漂っています！がッ一つ大事なことを思い出しました。

そもそもの目的は家の二歳児がラジコンを運転しても衝突を回避する事であって、自動運転はおまけ機能である事をすっかり忘れていました

まだプログラム作成は終わりません

20160815追記

// ラジコンをArduinoで自動運転！(20160720 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[] = {37, 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; //自動運転時の最低スロットル開度、実走70、実験78
byte MinimumDistance = 150; //車を停止させる最小距離(200:25cm)//※200から150に変更
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); //①初期設定：1860,2140　②調整後:1910,2190　③旧：1050,2800
  LpOut.write(37);  //各ピンにサーボ制御信号を出力
  RsOut.write(94);
  ThOut.write(94);
  FsOut.write(94);
  delay(1500);        //プロポペアリング時間1500ms(1.5秒)待ち
}

void loop() {
  byte Pattern = State(), A0 = Angle(0); //State関数を実行し動作ﾊﾟﾀｰﾝ番号を取得
  if ((A0 < Auto2 - 6) || (Auto3 + 6 < A0)) { //手動運転　※Auto1～4からAuto2～3に修正
    if (20 <= A0 && A0 <= 40) {
      LpOut.write(37); //Lpのフラツキを止める※A0<=32から40に修正
    } else {
      LpOut.write(A0);
    }
    RsOut.write(Angle(1));  //各ピンにサーボ制御信号を出力
    if (Auto1 - 6 <= A0 && A0 <= Auto1 + 6) {
      if (analogRead(0) >= 300 || analogRead(1) >= MinimumDistance || analogRead(2) >= 300 && Angle(2) < 94) {
        Back2(); //※MinimumDistanceを300に変更
      } else {
        ThOut.write(Angle(2));
      }
    } else { //未使用
      ThOut.write(Angle(2));
    }
    FsOut.write(Angle(3));
  } else {                                      //自動運転
    if (Auto1 - 6 <= A0 && A0 <= Auto1 + 6) {
      Speed = 4; //※未使用
    }
    if (Auto2 - 6 <= A0 && A0 <= Auto2 + 6) {
      Speed = 3; //Speed=3から2に変更//使用
    }
    if (Auto3 - 6 <= A0 && A0 <= Auto3 + 6) {
      Speed = 2; //Speed=2から1に変更//使用
    }
    if (Auto4 - 6 <= A0 && A0 <= Auto4 + 6) {
      Speed = 1; //※未使用
    }
    LpOut.write(37);  //各ピンにサーボ制御信号を出力
    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]);
  }
}

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];
}

volatile byte Lval, Sval, Rval;
byte State() { //3つのセンサーの状態から動作パターン番号を返す関数
  int L = 0, S = 0, R = 0 ;
  byte A = 115, B = 175, C = 176, D = 510; //A:50cm BC:30cm D:20cm ※A=115, B=175, C=176, D=510から変更
  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;
  if (A - 5 <= L && L <= A + 5) {} else {
    if (B - 5 <= L && L <= C + 5) {} else {
      Lval = 0;  //左ｾﾝｻｰの状況判断
      if (A <= L && L <= B) {
        Lval = Lval + 9;
      } if (C <= L) {
        Lval = Lval + 18;
      }
    }
  }
  if (A - 5 <= S && S <= A + 5) {} else {
    if (B - 5 <= S && S <= C + 5) {} else {
      Sval = 0;  //正面ｾﾝｻｰの状況判断
      if (A <= S && S <= B) {
        Sval = Sval + 3;
      } if (C <= S) {
        Sval = Sval + 6;
      }
    }
  }
  if (A - 5 <= R && R <= A + 5) {} else {
    if (B - 5 <= R && R <= C + 5) {} else {
      Rval = 0;  //右ｾﾝｻｰの状況判断
      if (A <= R && R <= B) {
        Rval = Rval + 1;
      } if (C <= R) {
        Rval = Rval + 2;
      }
    }
  }
  //if(D<L || D<S || D<R){return 26;}else{return Lval+ Sval+ Rval;}
  return Lval + Sval + Rval; //※一時保留
}

void Back() {
  ThOut.write(127);//急停止
  delay(500);//制動時間
  ThOut.write(94);//スロットルをニュートラルにする
  delay(500);//しばし待つ(重要！これをしないとバックできません！)
  FsOut.write(94); //フロントステアリングをニュートラルにする
  RsOut.write(94); //リアステアリングをニュートラルにする
  ThOut.write(127);//スロットルをバックにする
  delay(500);  //0.5秒間バックする
  ThOut.write(94);//スロットルをニュートラルにする
  delay(500);  //しばし待つ！
  if (analogRead(0) > analogRead(2)) {
    FsOut.write(160); RsOut.write(30);
  } else {
    FsOut.write(30);  //左右を確認し広い方に移動する
    RsOut.write(160);
  }
  ThOut.write(70);//スロットルをON
  delay(500);  //0.5秒間進む
}

void Back2() {
  ThOut.write(127);//急停止
  delay(500);//制動時間
  ThOut.write(94);//スロットルをニュートラルにする
  delay(500);//しばし待つ(重要！これをしないとバックできません！)
  FsOut.write(94); //フロントステアリングをニュートラルにする
  RsOut.write(94); //リアステアリングをニュートラルにする
  ThOut.write(127);//スロットルをバックにする
  delay(500);  //0.5秒間バックする
  ThOut.write(94);//スロットルをニュートラルにする
}