FC2ブログ

爪車

SH7125でPWMサーボの制御

SH7125で16個のPWMサーボの制御です。
需要がありそうなので紹介します。

マイコンでPWMサーボを制御する方法はいろいろありますが、今回の方法は、タイマを使った割り込みで生成したパルスで、16個のPWMサーボを制御します。

けっこう前に書いたプログラムなので、自分でも何でこうなったか忘れてしまっているので、簡単な説明だけしておきます。

タイマはMTU2の0~4チャネルを同期動作で使用。
ポートEのPE0~PE15からパルス出力。
想定している制御周期は20ms。
TCNTオーバーフローで2.5msごとの割り込みが4回、その後10msで割り込み。
2.5msの割り込みで4個ずつまとめてパルスを立ち上げ。コンペアマッチで0出力。
割り込みで呼ばれるのはvoid MTU20_INT_OVF(void);

と言う感じです。

パルスの立ち上げは割り込みで 4つまとめて立ち上げています。立ち下げは割り込みではなく、ハードウェアでそれぞれ処理されるので、割り込みによるジッタの発生が抑えられていると思います。
ServoPuls[]にサーボの指令値を入れます。基準パルス幅1.5msは10進数で2344です。16進数だと0x928です。
秋月電子のAKI-7125を想定しています。
AKI-7125に搭載されているクリスタルは12.5MHzなので、MTU2クロックMPΦは25MHzで使用します。
アルファプロジェクトのSTK-7125などはクリスタルが10MHzなので、プログラムの修正が必要になります。
MPΦ=25MHz/16でカウントするので、1msは1562.5カウントになります。

以下のプログラムは実際のプログラムからの一部抜粋なので、このままでは動きません。
環境はHEW4です。

//プログラムここから

#include "iodefine.h"
#include <machine.h>

volatile unsigned int ServoPuls[16]; // サーボパルス

// main関数の一部
void main(void)
{
IO_init();
MTU2_init();
set_imask(0); // 割り込みマスクの解除

ServoPuls_init(); // ServoPuls初期化
MTU2_syn_start(); // タイマシンクロスタート

while (1) {
}
}

// ServoPulsの初期化
void ServoPuls_init(void)
{
int i;
for (i = 0; i < 16; i++) {
ServoPuls[i] = 0x928; // パルス1.5msにセット
}
}

// IO Portのセット
void IO_init(void)
{

// ------------------------------------------------------------------
// PortE サーボパルス用 [PE15:PE0] MTU2
// ------------------------------------------------------------------
PFC.PECRL4.BIT.PE15MD = 0x01; // PE15 is TIOC4D
PFC.PECRL4.BIT.PE14MD = 0x01; // PE14 is TIOC4C
PFC.PECRL4.BIT.PE13MD = 0x01; // PE13 is TIOC4B
PFC.PECRL4.BIT.PE12MD = 0x01; // PE12 is TIOC4A
PFC.PECRL3.BIT.PE11MD = 0x01; // PE11 is TIOC3D
PFC.PECRL3.BIT.PE10MD = 0x01; // PE10 is TIOC3C
PFC.PECRL3.BIT.PE9MD = 0x01; // PE9 is TIOC3B
PFC.PECRL3.BIT.PE8MD = 0x01; // PE8 is TIOC3A
PFC.PECRL2.BIT.PE7MD = 0x01; // PE7 is TIOC2D
PFC.PECRL2.BIT.PE6MD = 0x01; // PE6 is TIOC2C
PFC.PECRL2.BIT.PE5MD = 0x01; // PE5 is TIOC1B
PFC.PECRL2.BIT.PE4MD = 0x01; // PE4 is TIOC1A
PFC.PECRL1.BIT.PE3MD = 0x01; // PE3 is TIOC0D
PFC.PECRL1.BIT.PE2MD = 0x01; // PE2 is TIOC0C
PFC.PECRL1.BIT.PE1MD = 0x01; // PE1 is TIOC0B
PFC.PECRL1.BIT.PE0MD = 0x01; // PE0 is TIOC0A

PFC.PEIORL.WORD = 0xFFFF; // PE15-PE0 is output
}

// MTU2の初期化
void MTU2_init(void)
{
STB.CR4.BIT._MTU2 = 0; // モジュールスタンバイの解除
MTU2.TSTR.BYTE = 0x00; // タイマ0~4停止
MTU2.TRWER.BIT.RWE = 1; // 3,4のレジスタのリードライトを許可する
MTU2.TSYR.BYTE = 0xC7; // TCNT0~4同期動作

// タイマ0~4同期動作
MTU20.TCR.BYTE = 0x62; // 同期クリア、MPφ/16 1count -> 6.4*10^(-7)s 1ms -> 1562.5count
MTU21.TCR.BYTE = 0x62; // 同期クリア、MPφ/16 1count -> 6.4*10^(-7)s 1ms -> 1562.5count
MTU22.TCR.BYTE = 0x62; // 同期クリア、MPφ/16 1count -> 6.4*10^(-7)s 1ms -> 1562.5count
MTU23.TCR.BYTE = 0x62; // 同期クリア、MPφ/16 1count -> 6.4*10^(-7)s 1ms -> 1562.5count
MTU24.TCR.BYTE = 0x62; // 同期クリア、MPφ/16 1count -> 6.4*10^(-7)s 1ms -> 1562.5count

MTU20.TMDR.BYTE = 0x00; // タイマ通常動作
MTU21.TMDR.BYTE = 0x00;
MTU22.TMDR.BYTE = 0x00;
MTU23.TMDR.BYTE = 0x00;
MTU24.TMDR.BYTE = 0x00;

MTU20.TIER.BIT.TCIEV = 1; // OVFによる割り込み許可
MTU21.TIER.BIT.TCIEV = 1; // OVFによる割り込み許可
MTU22.TIER.BIT.TCIEV = 1; // OVFによる割り込み許可
MTU23.TIER.BIT.TCIEV = 1; // OVFによる割り込み許可
MTU24.TIER.BIT.TCIEV = 1; // OVFによる割り込み許可

INTC.IPRD.BIT._MTU20C = 15; // MTU20 OVF 割り込み優先度 15

MTU2.TOER.BYTE = 0xFF; // 3,4の出力許可

MTU20.TIOR.BIT.IOA = 0x00; // TGRA_0 PE0 コンペアマッチで出力保持
MTU20.TIOR.BIT.IOB = 0x00; // TGRB_0 PE1 コンペアマッチで出力保持
MTU20.TIOR.BIT.IOC = 0x00; // TGRC_0 PE2 コンペアマッチで出力保持
MTU20.TIOR.BIT.IOD = 0x00; // TGRD_0 PE3 コンペアマッチで出力保持
MTU21.TIOR.BIT.IOA = 0x00; // TGRA_1 PE4 コンペアマッチで出力保持
MTU21.TIOR.BIT.IOB = 0x00; // TGRB_1 PE5 コンペアマッチで出力保持
MTU22.TIOR.BIT.IOA = 0x00; // TGRA_2 PE6 コンペアマッチで出力保持
MTU22.TIOR.BIT.IOB = 0x00; // TGRB_2 PE7 コンペアマッチで出力保持
MTU23.TIOR.BIT.IOA = 0x00; // TGRA_3 PE8 コンペアマッチで出力保持
MTU23.TIOR.BIT.IOB = 0x00; // TGRB_3 PE9 コンペアマッチで出力保持
MTU23.TIOR.BIT.IOC = 0x00; // TGRC_3 PE10 コンペアマッチで出力保持
MTU23.TIOR.BIT.IOD = 0x00; // TGRD_3 PE11 コンペアマッチで出力保持
MTU24.TIOR.BIT.IOA = 0x00; // TGRA_4 PE12 コンペアマッチで出力保持
MTU24.TIOR.BIT.IOB = 0x00; // TGRB_4 PE13 コンペアマッチで出力保持
MTU24.TIOR.BIT.IOC = 0x00; // TGRC_4 PE14 コンペアマッチで出力保持
MTU24.TIOR.BIT.IOD = 0x00; // TGRD_4 PE15 コンペアマッチで出力保持

MTU20.TGRA = 0xF0BD + 0x928; // コンペアマッチのセット
MTU20.TGRB = 0xF0BD + 0x928;
MTU20.TGRC = 0xF0BD + 0x928;
MTU20.TGRD = 0xF0BD + 0x928;
MTU21.TGRA = 0xF0BD + 0x928;
MTU21.TGRB = 0xF0BD + 0x928;
MTU22.TGRA = 0xF0BD + 0x928;
MTU22.TGRB = 0xF0BD + 0x928;
MTU23.TGRA = 0xF0BD + 0x928;
MTU23.TGRB = 0xF0BD + 0x928;
MTU23.TGRC = 0xF0BD + 0x928;
MTU23.TGRD = 0xF0BD + 0x928;
MTU24.TGRA = 0xF0BD + 0x928;
MTU24.TGRB = 0xF0BD + 0x928;
MTU24.TGRC = 0xF0BD + 0x928;
MTU24.TGRD = 0xF0BD + 0x928;

MTU20.TCNT = 0x0;

MTU20.TSR.BIT.TCFV = 0; // OVFフラグクリア

}

// MTU20 OVF 割り込み
// intprg.cで
//// 92 MTU2_0 TCIV0
//void INT_MTU2_0_TCIV0(void)
//{
// MTU20_INT_OVF();
//}
void MTU20_INT_OVF(void)
{
volatile static int count = 0;

MTU2.TSTR.BYTE = 0x00; // タイマ0~4停止
MTU20.TSR.BIT.TCFV = 0; // OVFフラグクリア

switch (count) {
case 0:
MTU20.TCNT = 0xF0BD; // 2.5ms セット

MTU20.TIOR.BIT.IOA = 0x05; // TGRA_0 PE0 初期1出力コンペアマッチで0出力
MTU20.TIOR.BIT.IOB = 0x05; // TGRB_0 PE1 初期1出力コンペアマッチで0出力
MTU20.TIOR.BIT.IOC = 0x05; // TGRC_0 PE2 初期1出力コンペアマッチで0出力
MTU20.TIOR.BIT.IOD = 0x05; // TGRD_0 PE3 初期1出力コンペアマッチで0出力

MTU2.TCSYSTR.BYTE = 0xF8; // 0,1,2,3,4 タイマシンクロスタート
count++;
break;
case 1:
MTU20.TCNT = 0xF0BD; // 2.5ms セット

MTU21.TIOR.BIT.IOA = 0x05; // TGRA_1 PE4 初期1出力コンペアマッチで0出力
MTU21.TIOR.BIT.IOB = 0x05; // TGRB_1 PE5 初期1出力コンペアマッチで0出力
MTU22.TIOR.BIT.IOA = 0x05; // TGRA_2 PE6 初期1出力コンペアマッチで0出力
MTU22.TIOR.BIT.IOB = 0x05; // TGRB_2 PE7 初期1出力コンペアマッチで0出力

MTU2.TCSYSTR.BYTE = 0xF8; // 0,1,2,3,4 タイマシンクロスタート
count++;
break;
case 2:
MTU20.TCNT = 0xF0BD; // 2.5ms セット

MTU23.TIOR.BIT.IOA = 0x05; // TGRA_3 PE8 初期1出力コンペアマッチで0出力
MTU23.TIOR.BIT.IOB = 0x05; // TGRB_3 PE9 初期1出力コンペアマッチで0出力
MTU23.TIOR.BIT.IOC = 0x05; // TGRC_3 PE10 初期1出力コンペアマッチで0出力
MTU23.TIOR.BIT.IOD = 0x05; // TGRD_3 PE11 初期1出力コンペアマッチで0出力

MTU2.TCSYSTR.BYTE = 0xF8; // 0,1,2,3,4 タイマシンクロスタート
count++;
break;
case 3:
MTU20.TCNT = 0xF0BD; // 2.5ms セット

MTU24.TIOR.BIT.IOA = 0x05; // TGRA_4 PE12 初期1出力コンペアマッチで0出力
MTU24.TIOR.BIT.IOB = 0x05; // TGRB_4 PE13 初期1出力コンペアマッチで0出力
MTU24.TIOR.BIT.IOC = 0x05; // TGRC_4 PE14 初期1出力コンペアマッチで0出力
MTU24.TIOR.BIT.IOD = 0x05; // TGRD_4 PE15 初期1出力コンペアマッチで0出力

MTU2.TCSYSTR.BYTE = 0xF8; // 0,1,2,3,4 タイマシンクロスタート
count++;
break;
default:
MTU20.TCNT = 0x3D09; // 10ms セット

MTU2.TCSYSTR.BYTE = 0xF8; // 0,1,2,3,4 タイマシンクロスタート

// servo puls set
MTU20.TGRA = 0xF0BD + ServoPuls[0];
MTU20.TGRB = 0xF0BD + ServoPuls[1];
MTU20.TGRC = 0xF0BD + ServoPuls[2];
MTU20.TGRD = 0xF0BD + ServoPuls[3];
MTU21.TGRA = 0xF0BD + ServoPuls[4];
MTU21.TGRB = 0xF0BD + ServoPuls[5];
MTU22.TGRA = 0xF0BD + ServoPuls[6];
MTU22.TGRB = 0xF0BD + ServoPuls[7];
MTU23.TGRA = 0xF0BD + ServoPuls[8];
MTU23.TGRB = 0xF0BD + ServoPuls[9];
MTU23.TGRC = 0xF0BD + ServoPuls[10];
MTU23.TGRD = 0xF0BD + ServoPuls[11];
MTU24.TGRA = 0xF0BD + ServoPuls[12];
MTU24.TGRB = 0xF0BD + ServoPuls[13];
MTU24.TGRC = 0xF0BD + ServoPuls[14];
MTU24.TGRD = 0xF0BD + ServoPuls[15];

count = 0;

break;
}
}

// タイマ0~4シンクロスタート
void MTU2_syn_start(void)
{
MTU2.TCSYSTR.BYTE = 0xF8; // タイマ0~4シンクロスタート
}

// タイマ0~4停止
void MTU2_syn_stop(void)
{
MTU2.TSTR.BYTE = 0x00; // タイマ0~4停止
}

// プログラムここまで

« ヒト型ロボット 24 数値計算ソフト|Top|ヒト型ロボット 23 USB-シリアル変換 »

コメント

コメントの投稿

管理者にだけ表示を許可する

トラックバック

http://tsumeguruma.blog46.fc2.com/tb.php/71-67be487c

Top

HOME

tsumehashi

Author:tsumehashi
FC2ブログへようこそ!

09 | 2019/10 | 11
- - 1 2 3 4 5
6 7 8 9 10 11 12
13 14 15 16 17 18 19
20 21 22 23 24 25 26
27 28 29 30 31 - -