ラジコンをArduinoで自動運転！その22(追伸)

メビウスKさん

ラジコンの自動運転プログラムで色々アイディアを頂きましてありがとうございました

遅くなりましたがいくつかご助言頂きました内容で実験してみました

 

改良前のプログラム

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

図1：プログラム例

※ArduinoIDEリファレンス

 

改良後のプログラム　その①

ご助言

こんにちは。
割り込み処理ルーチンについてですが、unsigned long Tdmy;なる変数を定義しておいて、
void LpIn(){Tdmy=micros(); if(digitalRead(1)==LOW){DownOld[0]=DownNew[0];DownNew[0]=Tdmy;}else{UpOld[0]=UpNew[0];UpNew[0]=Tdmy;}}
のようにすると、時間を計る精度が、ちょっとだけ良くなるかもしれません。
サーボのブルブルとは関係ないかもしれませんが。。。

 

 

結果

以下の感じでArduinoに書き込んで動かしてみました。

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

図1：プログラム例

※ArduinoIDEリファレンス

時間を測る精度は測れていませんが、見た感じサーボのブルブル同じぐらいでした。

 

改良後のプログラム　その②

ご助言

void LpIn(){
     if(digitalRead(1)==LOW){DownOld[0]=DownNew[0];DownNew[0]=micros();  LastIR[0]=0;}
      else{UpOld[0]=UpNew[0];UpNew[0]=micros();  LastIR[0]=1;}} //Lpの信号を保存

void RsIn(){
      if(digitalRead(0)==LOW){DownOld[1]=DownNew[1];DownNew[1]=micros();  LastIR[1]=0;}
      else{UpOld[1]=UpNew[1];UpNew[1]=micros();  LastIR[1]=1;}} //Rsの信号を保存

void ThIn(){
      if(digitalRead(2)==LOW){DownOld[2]=DownNew[2];DownNew[2]=micros();  LastIR[2]=0;}
      else{UpOld[2]=UpNew[2];UpNew[2]=micros();  LastIR[2]=1;}} //Thの信号を保存

void FsIn(){
      if(digitalRead(3)==LOW){DownOld[3]=DownNew[3];DownNew[3]=micros();  LastIR[3]=0;}
      else{UpOld[3]=UpNew[3];UpNew[3]=micros();  LastIR[3]=1;}} //Fsの信号を保存

Angle()関数の書き直し、（ここ重要！）

int Angle(byte i) {  //受信した信号からサーボ角度を計算する関数
    const unsigned long cntLim =  4294967295;   //３２ビットの限界の値
    
    unsigned long PW0 , PW1;

         if(UpNew[i] >= UpOld[i]){   //  通常の状態なら
　　　　　　　PW0 = UpNew[i] - UpOld[i];
　　　　｝
　　　　else{
               PW0 = cntLim- UpOld[i]; PW0 += UpNew[i]+1; 
         }
         if( LastIR[i] ==0 ){      // DownNewが最も新しいなら
                if(DownNew[i] >= UpNew[i]){   //  通常の状態なら
　　　　　　　         PW1 = DownNew[i] - UpNew[i];
                 }
                 else{
                        PW1 = cntLim- UpNew[i]; PW1 += DownNew[i]+1; 
                 }
         else{                         // UpNewが最も新しいなら
                 if(DownNew[i] >= UpOld[i]){   //  通常の状態なら
　　　　　　　         PW1 = DownNew[i] - UpOld[i];
                 }
                 else{
                        PW1 = cntLim- UpOld[i]; PW1 += DownNew[i]+1; 
                 }
　　　　｝
        PW1 *= 10000;
        Val[i]=(PW1/PW0-600)*2225/10000;
        return Val[i];
   }

 

結果

以下の感じでArduinoに書き込んで動かしてみました。

void LpIn() {
  Tdmy = micros();  //○
  if (digitalRead(1) == LOW) {
    DownOld[0] = DownNew[0];
    DownNew[0] = Tdmy;
    LastIR[0] = 0;
  } else {
    UpOld[0] = UpNew[0];
    UpNew[0] = Tdmy;
    LastIR[0] = 1;
  }
}
void RsIn() {
  Tdmy = micros();  //○
  if (digitalRead(0) == LOW) {
    DownOld[1] = DownNew[1];
    DownNew[1] = Tdmy;
    LastIR[1] = 0;
  } else {
    UpOld[1] = UpNew[1];
    UpNew[1] = Tdmy;
    LastIR[1] = 1;
  }
}
void ThIn() {
  Tdmy = micros();  //○
  if (digitalRead(2) == LOW) {
    DownOld[2] = DownNew[2];
    DownNew[2] = Tdmy;
    LastIR[2] = 0;
  } else {
    UpOld[2] = UpNew[2];
    UpNew[2] = Tdmy;
    LastIR[2] = 1;
  }
}
void FsIn() {
  Tdmy = micros();  //○
  if (digitalRead(3) == LOW) {
    DownOld[3] = DownNew[3];
    DownNew[3] = Tdmy;
    LastIR[3] = 0;
  } else {
    UpOld[3] = UpNew[3];
    UpNew[3] = Tdmy;
    LastIR[3] = 1;
  }
}

int Angle(byte i) {  //受信した信号からサーボ角度を計算する関数  //○
  const unsigned long cntLim =  4294967295;   //３２ビットの限界の値  //○
  unsigned long PW0 , PW1;  //○
  if (UpNew[i] >= UpOld[i]) { //  通常の状態なら  //○
    PW0 = UpNew[i] - UpOld[i];  //○
  } else { //○
    PW0 = cntLim - UpOld[i]; PW0 += UpNew[i] + 1; //○
  }  //○
  if ( LastIR[i] == 0 ) {   // DownNewが最も新しいなら  //○
    if (DownNew[i] >= UpNew[i]) { //  通常の状態なら  //○
      PW1 = DownNew[i] - UpNew[i];  //○
    } else { //○
      PW1 = cntLim - UpNew[i]; PW1 += DownNew[i] + 1; //○
    }  //○
  } else {                        // UpNewが最も新しいなら  //○
    if (DownNew[i] >= UpOld[i]) { //  通常の状態なら  //○
      PW1 = DownNew[i] - UpOld[i];  //○
    } else { //○
      PW1 = cntLim - UpOld[i]; PW1 += DownNew[i] + 1; //○
    }  //○
  }  //○
  PW1 *= 10000;  //○
  Val[i] = (PW1 / PW0 - 600) * 2225 / 10000; //○
  return Val[i];  //○
}  //○

図1：プログラム例

※ArduinoIDEリファレンス

 

原因は良く分かりませんがフロントサーボのみ右折しようとすると左折に反転します。残念ながらサーボのプル付きに改善は見られませんでした。

 

改良後のプログラム　その③

ご助言

Angle()関数の、さらに、書き直し、（ここも重要！）

int Angle(byte i) {  //受信した信号からサーボ角度を計算する関数
    const unsigned long cntLim =  4294967295;   //３２ビットの限界の値
    
    unsigned long PW0 , PW1;

         if(UpNew[i] >= UpOld[i]){   //  通常の状態なら
　　　　　　　PW0 = UpNew[i] - UpOld[i];
　　　　｝
　　　　else{
               PW0 = cntLim- UpOld[i]; PW0 += UpNew[i]+1; 
         }
         if( LastIR[i] ==0 ){      // DownNewが最も新しいなら
                if(DownNew[i] >= UpNew[i]){   //  通常の状態なら
　　　　　　　         PW1 = DownNew[i] - UpNew[i];
                 }
                 else{
                        PW1 = cntLim- UpNew[i]; PW1 += DownNew[i]+1; 
                 }
                 PW1 *= 10000;
                 Val[i]=(PW1/PW0-600)*2225/10000;
         else{                         // UpNewが最も新しいなら
                 // Angleの値は更新せずに、１つ前のVal[i]を戻り値にする。
　　　　｝
       
        return Val[i];
   }

 

結果

以下の感じでArduinoに書き込んで動かしてみました。

void LpIn() {
  Tdmy = micros();  //○
  if (digitalRead(1) == LOW) {
    DownOld[0] = DownNew[0];
    DownNew[0] = Tdmy;
    LastIR[0] = 0;
  } else {
    UpOld[0] = UpNew[0];
    UpNew[0] = Tdmy;
    LastIR[0] = 1;
  }
}
void RsIn() {
  Tdmy = micros();  //○
  if (digitalRead(0) == LOW) {
    DownOld[1] = DownNew[1];
    DownNew[1] = Tdmy;
    LastIR[1] = 0;
  } else {
    UpOld[1] = UpNew[1];
    UpNew[1] = Tdmy;
    LastIR[1] = 1;
  }
}
void ThIn() {
  Tdmy = micros();  //○
  if (digitalRead(2) == LOW) {
    DownOld[2] = DownNew[2];
    DownNew[2] = Tdmy;
    LastIR[2] = 0;
  } else {
    UpOld[2] = UpNew[2];
    UpNew[2] = Tdmy;
    LastIR[2] = 1;
  }
}
void FsIn() {
  Tdmy = micros();  //○
  if (digitalRead(3) == LOW) {
    DownOld[3] = DownNew[3];
    DownNew[3] = Tdmy;
    LastIR[3] = 0;
  } else {
    UpOld[3] = UpNew[3];
    UpNew[3] = Tdmy;
    LastIR[3] = 1;
  }
}

int Angle(byte i) {  //受信した信号からサーボ角度を計算する関数  //○○
  const unsigned long cntLim =  4294967295;   //３２ビットの限界の値  //○○
  unsigned long PW0 , PW1;  //○○
  if (UpNew[i] >= UpOld[i]) { //  通常の状態なら  //○○
    PW0 = UpNew[i] - UpOld[i];  //○○
  } else { //○○
    PW0 = cntLim - UpOld[i]; PW0 += UpNew[i] + 1; //○○
  }  //○○
  if ( LastIR[i] == 0 ) {   // DownNewが最も新しいなら  //○○
    if (DownNew[i] >= UpNew[i]) { //  通常の状態なら  //○○
      PW1 = DownNew[i] - UpNew[i];  //○○
    } else { //○○
      PW1 = cntLim - UpNew[i]; PW1 += DownNew[i] + 1; //○○
    }  //○○
    PW1 *= 10000;  //○○
    Val[i] = (PW1 / PW0 - 600) * 2225 / 10000; //○○
  } else {                        // UpNewが最も新しいなら  //○○
    // Angleの値は更新せずに、１つ前のVal[i]を戻り値にする。  //○○
  }       //○○
  return Val[i];  //○○
}  //○○

図1：プログラム例

※ArduinoIDEリファレンス

 半分無視して計算式を統一する方法が一番効くかと思いましたが、これも残念ながら改良前と同じ位でした。

改良後のプログラム　その④(2016.08.01追記)

ご助言

 

また提案ですが、Ａｎｇｅｌ（ ）；関数を実行している間、割り込みを禁止することはできないでしょうか？
計算している最中にもとになる値が更新されてしまうとマズいことになりそうな気がします。

 

 

結果

Angle();関数内に割り込みを禁止・解除する命令(太字部)を追記してArduinoに書き込んで動かしてみました。

int Angle(byte i) {  //受信した信号からサーボ角度を計算する関数
  noInterrupts();//〇
  if (UpNew[3] <= DownNew[i]) { //もしDownNewの更新が新しかったら{}内を実行
    Val[i] = ((((DownNew[i] - UpNew[3]) * 10000) / ((UpNew[3] - 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;
  } interrupts(); return Val[i];
}

図1：プログラム例

※ArduinoIDEリファレンス

 

結果は残念ながら変化は見られませんでした

 

まとめ

 今更ですがPWM信号を測定する際に必要な時間の精度を計算してみました。

○周期：15ms

○Duty：0.07～0.13

最小パルス幅：15ms×0.07=1.05ms=1050μs

最大パルス幅：15ms×0.13=1.95ms=1950μs

パルス幅の変位差：1950μs-1050μs=900μs

○PWM出力のDuty比分解能：180

PWM出力の時間分解能：900μs/180=5μs

 ざっくり0μs(サーボ角0°)か5μs(サーボ角1°)を判別するためには、その差の4倍程度の精度は必要なので1μs程度の精度は欲しいです。しかしArduinoMicroのmicros()による時刻測定精度は4μsしかなく、1°位サーボがぶるぶるするのは当然な気がしてきました。もちろん割り込みの計算負荷による遅れもモロ影響して来ます。

 メビウスKさんが計画されているPICの時間測定精度は0.8μsと1μsを切る精度との事なので上手く行く可能性が高いですね！

 今から結果が楽しみです

 

励みになりますのでよければクリック下さい(^o^)/

↩【ラジコンをArduinoで自動運転！】目次に戻る

---