ラジコンの自動運転化装置をラジコンに載せる前に最終動作チェックを行ってみました。取り敢えず動きますが、やっちゃった系から良く分からない系も含めて6個程バグを発見しました

※発見しなかったバグもあると思います！

1.プログラム

前に作っておいた以下二つのプログラムをくっつけてみました。

// ラジコンをArduinoで自動運転！(20160716 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 int Val[] = {0, 0, 0, 0};                      //測定した開度
byte FSOut[27] = {94, 94, 62, 62, 62, 30, 160, 30, 30, 94, 94, 62, 127, 94, 94, 160, 94, 94, 127, 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, 94, 127, 127, 127, 160, 30, 160, 160, 94, 94, 127, 62, 94, 94, 30, 94, 94, 62, 62, 94, 30, 94, 94, 30, 94, 94}; //右30,中94,左160


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

void loop() {
  byte Pattern = State();            //State関数を実行し動作ﾊﾟﾀｰﾝ番号を取得
  //LpOut.write(Angle(0));  //各ピンにサーボ制御信号を出力
  //RsOut.write(RSOut[Pattern]);
  //ThOut.write(THOut[Pattern]);
  //FsOut.write(FSOut[Pattern]);
  LpOut.write(Angle(0));  //各ピンにサーボ制御信号を出力
  RsOut.write(Angle(1));
  ThOut.write(Angle(2));
  FsOut.write(Angle(3));
  //Serial.println(Angle(0));  //iの値をシリアル出力します
  //delay(1000);                           //1000ms(1秒)待ちます
}

void LpIn() {
  if (digitalRead(0) == LOW) {
    DownOld[0] = DownNew[0];  //Lpの信号を保存
    DownNew[0] = micros();
  } else {
    UpOld[0] = UpNew[0];
    UpNew[0] = micros();
  }
}
void RsIn() {
  if (digitalRead(1) == 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, A = 115, B = 175, C = 176;
  for (int i = 0; i < 9; i++) {           //測距ｾﾝｻｰの値を10回読み平均値を出す
    L = L + analogRead(0); S = S + analogRead(1); R = R + analogRead(2);
  }
  L = L / 10, S = S / 10, R = R / 10, 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;
}