1. プログラム

プログラムまだ調整中ですが、とりあえず良い感じで動いています

このプログラムはサーボモーターの制御信号(PWM信号)をArduinoで読み取り、読み取ったPWM信号からサーボ角度を計算します。

計算したサーボ角度からArduinoでサーボ制御信号を再び生成し出力しています。

一度読み取ってから同じものを作ると言う一見意味無い事をしていますが、これが出来る事によってラジコンの遊びの幅がグッと広がります

例えば子供は(私も

)ラジコンをすぐにぶつけて壊してしまいますが、Arduino等によって制御されたラジコンでは、危険を察知するとぶつかる前に避けたり止まったりする事も出来ます！

うちの二歳児に丁度良い機能です

PWM信号をスルーするプログラムは以下の通りです。

// ラジコンをArduinoで自動運転！(20160712 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};                      //測定した開度

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

void loop() {
  LpOut.write(Angle(0));  //各ピンにサーボ制御信号を出力
  RsOut.write(Angle(1));
  ThOut.write(Angle(2));
  FsOut.write(Angle(3));
}

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