１６軸ラジコンサーボ制御 （タイマーＷ）

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

ＳＩＳＯさんが作られた３６６４Ｆ用のタイマーＷを使った１６軸サーボ制御ソフトがあります。

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

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

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

多数のパルス発生を考える　であるように、出力を出す、落とすのＯＮ、ＯＦＦを、Ｉ／Ｏ出力でするのですが、そのＯＮからＯＦＦまで時間を計るのにタイマーＷの機能を使います。

割り込み関数実行と　各タイマーＷのＡ－Ｄ　で時間を設定すると４つの時間を測れます。 その時間でＩ／ＯピンをＯＮ、ＯＦＦするのです。

タイマーＷは一度に４つまで時間測定が出来るので、それを
ＰＷＭパルス発生を切り替える
のように、ＩＯピンを切り替えると、周期１０msなら　最大パルス長２．５msで　　周期１０ms内で４回

一度にタイマーＷで４個　×　毎回切り替えて４回で　１６通りのパルス発生が可能です。（周期を２０ｍｓにすると原理では３２通りのパルス発生が可能）

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

回路

パルスの出力は

• IO.PDR1.BYTE 　　　上位４ビット
• IO.PDR5.BYTE 　　　下位４ビット
• IO.PDR8.BYTE 　　　８ビット全部

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

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

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

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

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

プログラム

 #include <3664.h>

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

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


//
// タイマＷ割り込みルーチン
//
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になっていたら
    //   数値を０に戻す。
    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;    // クロックセレクト
                          //   内部クロックをΦ８でカウントアップ

  // タイマインタラプトイネーブルレジスタ
  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;              // ステップを０に初期化

  // データの初期化
  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の説明内にあるように、タイマーＷの割り込みでピンをＯＮ、コンペアマッチでＯＦＦにする方法です。
処理もコメントも簡潔にまとまっていて見やすいです。

変数

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

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

処理の流れ

処理の大きなポイントは

割り込み関数は　オーバーフローやコンペアマッチで実行されタイマーＷ割り込みのオーバーフローで ピン出力ＯＮ、　コンペアマッチＡ～ＤでＯＦＦになります。

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

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

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

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

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

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

周　期

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

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

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

各コンペアマッチ

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

割り込み関数

bStep　のカウントで区切り

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

内部での処理

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

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

一度にタイマーＷで４個　×　毎回切り替えて４回で　１６通りのパルス発生が可能です。 （周期を２０ｍｓにすると原理では３２通りのパルス発生が可能）

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

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

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

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

ＯＲをとって目的ビットだけを１に。これで設定したbSrvA　～B　を　ピンにＯＮ出力でＯＮ信号が出力される流れです。

●各コンペアマッチ

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

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

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

実行

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

オシロでの測定です。

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

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

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