16軸ラジコンサーボ制御 (タイマーW)

以前、すこし多数のパルス発生を考えてみましたが、SISOさんがタイマーWを使ったH8-3694Fのラジコンサーボ16軸制御プログラムを作られています


3664F用のタイマーWを使った16軸ラジコンサーボ制御ソフトです。

タイマーWで多数のパルス発生

SISOさんが作られた3664F用のタイマーWを使った16軸サーボ制御ソフトがあります。

タイマーWのPWM機能はピンが固定されていますが、その機能ではなく、タイマーWを時間測定だけに使い、それを切り替えてI/O出力する方法です。
この方法があったんだ と気づきました!


外部回路を使わず、タイマーWを使い、タイマーなら時間もより正確で、この方法なら多数パルスの発生が可能です。

これは以前、考えてみた2番目の
PWMパルス発生を切り替える   のような方法で、タイマーWを時間計測だけに使用し、それをI/O出力切り替えでパルスにする方法になります。

 

SISOさんにこちらで解説、公開してもよいとの許可がいただけたので、こちらでも掲載します。
SISOさんがH8-3664F用で、きれいにプログラムにしているのでそのまま使わせてもらいましょう。

優れた方法とソースファイルも簡潔でいながら、まとまっています。sisoさん、すごい人です

SiSO

 

H8-3664、3694もロボットに活用されていますので、3664、3694でも情報豊富なHPです。リンクに追加しました。
加工関連でも、大きな機械を使わず(最近 かなりCNCフライス盤も活用されています)、主にハンドツールなど身近なもので工夫のあるいろんな方法や材料など加工方法にも興味深い内容です。

 


このプログラムは 上記HPのSISOさんが作成されたもので、同様のことを手がけているときに偶然出会い、SISOさんの厚意により当HPで公開許可いただけたものです

内容はよくわからないが実用に改良したい等、一番問い合わせが多いのがここの多軸RCサーボ制御のところですが、これは多軸RCサーボ制御方法の1つとして加えました。sisoさんのソースもきれいに簡潔に書かれていて、下に説明も加えましたので処理内容を理解し発展させる種として活用してください。



多数のパルス発生を考える であるように、出力を出す、落とすのON、OFFを、I/O出力でするのですが、そのONからOFFまで時間を計るのにタイマーWの機能を使います。

割り込み関数実行と 各タイマーWのA-D で時間を設定すると4つの時間を測れます。 その時間でI/OピンをON、OFFするのです。

タイマーWは一度に4つまで時間測定が出来るので、それを
PWMパルス発生を切り替える
のように、IOピンを切り替えると、周期10msなら 最大パルス長2.5msで  周期10ms内で4回

一度にタイマーWで4個 × 毎回切り替えて4回で 16通りのパルス発生が可能です。(周期を20msにすると原理では32通りのパルス発生が可能)

以前に少し調べたように、タイマーではより正確な幅のパルスができるので外部回路を持たず、ソフトだけでできるいい方法ですね。。

 

回路

パルスの出力は

 

  • IO.PDR1.BYTE    上位4ビット
  • IO.PDR5.BYTE    下位4ビット
  • IO.PDR8.BYTE    8ビット全部


での各ピンに出力されます。


周期10msの多軸ラジコンサーボ用

 

SISOさんのオリジナルプログラムは全体周期20msとなっているので
本来の2.5msごとに切り替え、全体周期10msにしてみました。

そのために少しばかりの変更をしました。(最初の何もしないオーバーフロー時間設定部分を取り除く程度ですが)
簡単な変更でしたが、SISOさんのオリジナルプログラムはロボット制御用に作られた様子で、ほんの一部、そのなごりと思われるような、多分他の処理用の時間部分を変更し 私がラジコンサーボ用に変更したものです。

また、わかりやすいよう出力ポートも書き出しました
(だいぶ前に作成した記事の変更になります)

 

プログラム

 

* 2010/7/4  このサンプルファイルはGDLの3664FとGDLのVer1.5.0.33で作成したサンプルファイルです。
これ以降のGDLのバージョンでは動作しないこともあります。GDLのVer 2.0.2.9 でビルドしたものでは全く動作しませんでした。
2010/4/28原因はコンパイラのビットアクセスの不良のようです。GDLのVer 2.4.xxでは動作しました。

 

SISO_sarvos10ms


 #include <3664.h>

#define TIMERW_10MSEC 45535  // 10msec待つためのタイマW初期値
#define TIMERW_2M5SEC 60535  // 2.5msec待つためのタイマW初期値

_BYTE   bStep;            // サーボ制御ステップ
_WORD   awServoPos[16];   // サーボ制御位置


//
// タイマW割り込みルーチン
//
void int_timerw( void )
{
  _BYTE bSrvA, bSrvB, bSrvC, bSrvD;   // サーボ制御信号ON作業用

  if( TW.TSRW.BIT.OVF == 1 ){   // オーバーフロー発生
    // オーバーフロー発生時の処理
    TW.TSRW.BIT.OVF = 0;    // オーバーフローフラグクリア

    TW.TMRW.BIT.CTS = 0;    // タイマストップ(他のビットはデフォルト)


      TW.TCNT = TIMERW_2M5SEC;    // 2.5msec後にOVFする値設定

      // GRレジスタへの制御信号OFF時間の設定
      TW.GRA = TIMERW_2M5SEC + awServoPos[(bStep-1)+0];
      TW.GRB = TIMERW_2M5SEC + awServoPos[(bStep-1)+4];
      TW.GRC = TIMERW_2M5SEC + awServoPos[(bStep-1)+8];
      TW.GRD = TIMERW_2M5SEC + awServoPos[(bStep-1)+12];

      bSrvA = 1 << (( bStep-1 )+4 );  // 元ネタの計算
      bSrvB = 1 << (( bStep-1 ));     // 元ネタの計算
      bSrvC = 1 << (( bStep-1 ));     // 元ネタの計算
      bSrvD = 1 << (( bStep-1 )+4 );  // 元ネタの計算

      IO.PDR1.BYTE |= bSrvA;          // 信号ON!!
      IO.PDR5.BYTE |= bSrvB;          // 信号ON!!
      IO.PDR8.BYTE |= bSrvC;          // 信号ON!!
      IO.PDR8.BYTE |= bSrvD;          // 信号ON!!
    

    // ステップ情報の操作
    //   1~4:サーボ制御信号出力なので、5になっていたら
    //   数値を0に戻す。
    bStep++;
    if( bStep == 5 ){
      bStep = 1;
    }

    TW.TMRW.BIT.CTS = 1;    // タイマスタート
  }
  
  if(TW.TSRW.BIT.IMFA  == 1){   // GRA コンペアマッチ
    // GRAコンペアマッチ発生時の処理
    IO.PDR1.BYTE &= 0x0F;
    TW.TSRW.BIT.IMFA = 0;       // 割り込みフラグクリア
  }
  if(TW.TSRW.BIT.IMFB  == 1){   // GRB コンペアマッチ
    // GRBコンペアマッチ発生時の処理
    IO.PDR5.BYTE &= 0xF0;
    TW.TSRW.BIT.IMFB = 0;       // 割り込みフラグクリア
  }
  if(TW.TSRW.BIT.IMFC  == 1){   // GRC コンペアマッチ
    // GRCコンペアマッチ発生時の処理
    IO.PDR8.BYTE &= 0xF0;
    TW.TSRW.BIT.IMFC = 0;       // 割り込みフラグクリア
  }
  if(TW.TSRW.BIT.IMFD  == 1){   // GRD コンペアマッチ
    // GRDコンペアマッチ発生時の処理
    IO.PDR8.BYTE &= 0x0F;
    TW.TSRW.BIT.IMFD = 0;       // 割り込みフラグクリア
  }
}

int main()
{
  //
  // 初期化
  //
  // タイマモードレジスタ
  TW.TMRW.BIT.CTS = 0;    // タイマストップ(他のビットはデフォルト)

  // タイマコントロールレジスタ
  TW.TCRW.BIT.CKS = 3;    // クロックセレクト
                          //   内部クロックをΦ8でカウントアップ

  // タイマインタラプトイネーブルレジスタ
  TW.TIERW.BIT.OVIE = 1;  // オーバーフロー割り込み有効
  TW.TIERW.BIT.IMIEA = 1; // GRA割り込み有効
  TW.TIERW.BIT.IMIEB = 1; // GRB割り込み有効
  TW.TIERW.BIT.IMIEC = 1; // GRC割り込み有効
  TW.TIERW.BIT.IMIED = 1; // GRD割り込み有効

  IO.PCR1 = 0xF0;         // ポート1のP14-17を出力設定
  IO.PCR5 = 0x0F;         // ポート5のP50-53を出力設定
  IO.PCR8 = 0xFF;         // ポート8は全部出力設定

  bStep = 1;              // ステップを0に初期化

  // データの初期化
  awServoPos[0] = 3000;  // 1.5msec   P14
  awServoPos[1] = 3200;  // 1.6msec   P15
  awServoPos[2] = 3400;  // 1.7msec   P16
  awServoPos[3] = 3600;  // 1.8msec   P17

  awServoPos[4] = 3800;  // 1.9msec   P50
  awServoPos[5] = 4000;  // 2.0msec   P51
  awServoPos[6] = 3000;  // 1.5msec   P52
  awServoPos[7] = 3200;  // 1.6msec   P53

  awServoPos[8] = 3400;  // 1.7msec   P80
  awServoPos[9] = 3600;  // 1.8msec   P81
  awServoPos[10] = 3800;  // 1.9msec   P82
  awServoPos[11] = 4000;  // 2.0msec   P83

  awServoPos[12] = 3000;  // 1.5msec   P84
  awServoPos[13] = 3200;  // 1.6msec   P85
  awServoPos[14] = 3400;  // 1.7msec   P86
  awServoPos[15] = 3600;  // 1.8msec   P87
  

  EI;                     // 割り込み許可

  TW.TCNT = TIMERW_2M5SEC;// タイマカウンタに2.5msec後にOVFする値設定
  TW.TMRW.BIT.CTS = 1;    // タイマスタート

  while( 1 ){
    // せっかくなのでパソコンからキーボード入力でサーボを
    // 動かすプログラムを書いてみましょ。
  }
}

       

 

内容は、SISOさんのHPの説明内にあるように、タイマーWの割り込みでピンをON、コンペアマッチでOFFにする方法です。
処理もコメントも簡潔にまとまっていて見やすいです。

 

いろんな積み重ねで作られたかもしれないプログラムの内容を、結果のソースだけみて説明なんてすると失礼になるかもしれないものです。だから説明はSISOさんのHPをみましょう、としたですが
でもHPで公開、解説は多くはしてないようなので、細かいとろを考えるには私が理解したような方法で、少しわかりにくいところとプログラムの流れ説明します。

 

変数

_BYTE bStep; // サーボ制御ステップ
_WORD awServoPos[16]; // サーボ制御位置

 

これは見慣れない変数宣言ですが、3664ヘッダでtypedef定義されていて
_BYTE → unsigned char
_WORD → unsigned short
と同じことです。

 

処理の流れ

処理の大きなポイントは

割り込み関数は オーバーフローやコンペアマッチで実行されタイマーW割り込みのオーバーフローで ピン出力ON、 コンペアマッチA~DでOFFになります。


まずTW.TCNTで周期を設定し、割り込み関数は オーバーフロー、コンペアマッチで呼び出されます。
割り込み関数は オーバーフロー、コンペアマッチで共に同じ割り込み関数が実行されるので、 内部で判断してオーバーフロー、コンペアマッチで各処理をします。

 

・オーバーフロー割り込み

if( TW.TSRW.BIT.OVF == 1 ){    // オーバーフロー発生
else{(){

 

・コンペアマッチ割り込み

if(TW.TSRW.BIT.IMFA == 1){ // GRA コンペアマッチ
 ・
 ・
if(TW.TSRW.BIT.IMFD == 1){ // GRD コンペアマッチ

 

割り込み関数内部で オーバーフロー割り込みかコンペアマッチ割り込みかをレジスタのフラグで判断して各処理をします。

 

周 期

周期10msは 
else{ TW.TCNT = TIMERW_2M5SEC; // 2.5msec後にOVFする値設定  で
2.5msごとに4回実行を繰り返します。

 

そこで4つの
// GRレジスタへの制御信号OFF時間の設定
がされます(この時間がくると、割り込みとは独立に、コンペアマッチGRA~Dで設定時間が来たことがわかります)

 

そして、 bSrvA = のビットシフトで呼び出しのたびに
出力ピンを切り替えて
IO.PDR1.BYTE |= bSrvA; // 信号ON!!
で各切り替えピンにON信号が出力される流れです。

 

各コンペアマッチ

GRA~Dので
// GRA コンペアマッチ
で 設定時間が来たときの処理になります。設定時間がくるとこれを実行するのでこの処理ではピン出力がOFFになります。

 

割り込み関数

bStep のカウントで区切り

 

周期10msで 2.5msごとに呼ばれ、また始めにもどるを繰り返す。
また、コンペアマッチA~Dでも呼ばれる


内部での処理

・ // GRレジスタへの制御信号OFF時間の設定 →OFFにする時間を設定

TW.GRA = TIMERW_2M5SEC + awServoPos[(bStep-1)+0];
ここで GRA~Dに パルス長をタイマ用にした数値を設定

 

一度にタイマーWで4個 × 毎回切り替えて4回で 16通りのパルス発生が可能です。 (周期を20msにすると原理では32通りのパルス発生が可能)

 

*赤は出力ポートP
1回目 2回目 3回目 4回目
OVF 2.5ms OVF 2.5ms OVF 2.5ms OVF 2.5ms
bStep == 1 bStep == 2 bStep == 3 bStep == 4
awServoPos[0] P14  awServoPos[1] P15  awServoPos[2] P16  awServoPos[3] P17 
awServoPos[4] P50 awServoPos[5] P51 awServoPos[6] P52 awServoPos[7] P53
awServoPos[8] P80 awServoPos[9] P81 awServoPos[10] P82 awServoPos[11] P83
awServoPos[12]P84 awServoPos[13]P85 awServoPos[14] P86 awServoPos[15] P87
全部で周期10ms

 

 

各出力ピンをONにする設定

bSrvA = 1 << (( bStep-1 )+4 ); // 元ネタの計算 
bSrvB = 1 << (( bStep-1 )); // 元ネタの計算
bSrvC = 1 << (( bStep-1 )); // 元ネタの計算
bSrvD = 1 << (( bStep-1 )+4 ); // 元ネタの計算

 

1 << (( bStep-1 )+4 );  で 上位ビット4bitから→5→6bit・・と割り込み呼び出しのたびに出力ピンを変える
( bStep-1 )+4 とあるのは出力するピン IO.PDR1.BYTE PDR8  などにより、上位ビット、下位ビットだけ、を出力ピンに使っているので

 

IO.PDR1.BYTE |= bSrvA; // 信号ON!!
IO.PDR5.BYTE |= bSrvB; // 信号ON!!
IO.PDR8.BYTE |= bSrvC; // 信号ON!!
IO.PDR8.BYTE |= bSrvD; // 信号ON!!

 

ORをとって目的ビットだけを1に。これで設定したbSrvA ~B を ピンにON出力でON信号が出力される流れです。

処理を簡潔に、スムーズに書かかれたきれいなソースですねぇ、いつみても、見習いたいと思います

 

●各コンペアマッチ

各出力ピンをコンペアマッチ時間が来てOFFにする設定

// GRAコンペアマッチ発生時の処理  → 設定時間がくると、これを実行するので、ピンOFFにするのを実行
各GRA ~D  アンドでマスクで他のビットに影響が出ず、目的ビットだけを0に。

 

IO.PDR1.BYTE &= 0x0F;
IO.PDR5.BYTE &= 0xF0;
IO.PDR8.BYTE &= 0x0F;
IO.PDR8.BYTE &= 0xF0;

 

実行

ラジコンサーボをいっぱい持ってないので16軸も確認できないですが。。
そのまま実行してみてどんな波形か見てみました。

オシロでの測定です。

パルス観測波形1

P83の波形です。きれいな波形です。

 

SISOさんのHPではコンペアマッチ割り込みが重なるタイミングでジッタが起こるそうです。

 

ソフト的に切り替えるので割り込みやコンペアマッチでのピン操作がずれ込めば波長に影響することはあるでしょう。他の割り込み処理があればなおさら。これを押さえるのは厄介そうです。

まずは16コ 全部同じパルス:1.5msにしてみましたが、1.52msのままでふれはありませんでした。
処理でのピン出力の時間の遅れが原因でしょうから、全部波長を固定にしたプログラムでは再現させられないのかもしれません。(波長のびたままになることが現れそうですが、なかったです)

続いて、割り込み周期と同じ2.5ms(GR値=5000)にすると、、、波形はでませんでした。

 

ジッタの起こりはうまく再現させられないのですが、今度、実使用のとき考えてみましょう。

 

スポンサーリンク
  • facebook
  • twtter
  • google+
  • hatena