理系的な戯れ

理工学系とくにロボットやドローンに関する計算・プログラミング等の話題を扱って、そのようなことに興味がある人たちのお役に立てればと思っております。

初級者用マウス2式のサンプルプログラムの解説

初級者用マウス2式と本記事について

初級者用マウス2式福井大学のI.Sys 所属の岸本氏が開発されたものです。 マイクロマウス北陸同好会では初心者が初めて取り扱うマイクロマウスとして奨励しています。

sites.google.com

本投稿記事ではサンプルプログラムの主要部分の解説をしていきたいと思います。 これは自分用の備忘録を兼ねての執筆ですが、これからこのマウスでデビューしようとする方々へお役に立てれば幸いです。 本ブログの特徴して完結していない記事がたくさんあります。 本記事についても気長に書いていこうと思いますので、時々確認していただけると嬉しいです。

モータ周りのソース

初級者用マウス2式はステッピングモータで走行するマイクロマウスです。 モータ用ドライバICはA3982です。

モータの制御については、モータドライバICに1パルスを加えるとモータが1ステップ分回転します。 本マウスはモータドライバを1−2層励磁に設定していて1周400ステップになっていますので、 400パルスをモータドライバに加えるとモータが一周します。 ステッピングモータの原理はオリエンタルモータ等のホームページが役に立つと思います。

重要なことはパルスを与える速さ(周波数もしくは周期)がモータの回転の速さになるということです。

下図はモータ制御系のブロック図です。

モータ制御系ブロック図
モータ制御系ブロック図

RX220からパルスを出力する

さて、本マウスの頭脳であるRX220マイコンからモータドライバにパルスを出力する方法ですが タイマーカウンタをPWMモード1で使用しています。 ソースコードのMotor.cを参照します。

    //MTU3設定
    MSTP_MTU3 = 0;             //MTU3スタンバイ解除
    MTU.TSTR.BIT.CST3 = 0;     //MTU3タイマー停止
    MTU3.TCR.BIT.TPSC = 2;     //動作周波数設定:CLOCK/16
    MTU3.TCR.BIT.CCLR = 1;     //TGRAのコンペアマッチでTCNTクリア
    MTU3.TMDR.BIT.MD = 2;      //PWMモード1に設定
    MTU3.TMDR.BIT.BFA = 1;     //TGRAとTGRCレジスタはバッファ動作
    MTU3.TIORH.BIT.IOA = 2;        //初期出力L,コンペアマッチでH
    MTU3.TIORH.BIT.IOB = 1;        //初期出力L,コンペアマッチでL
    MTU3.TGRA = 0;
    MTU3.TGRB = 40 - 1;
    MTU3.TGRC = 0;
    MTU3.TIER.BIT.TGIEA = 1;   //割り込み要求許可
    IR(MTU3,TGIA3) = 0;            //割り込み要求フラグクリア
    IEN(MTU3,TGIA3) = 1;       //割り込み要求許可
    IPR(MTU3,TGIA3) = 14;      //割り込み優先度設定

    //MTU4設定
    MSTP_MTU4 = 0;             //MTU4スタンバイ解除
    MTU.TSTR.BIT.CST4 = 0;     //MTU4タイマー停止
    MTU4.TCR.BIT.TPSC = 2;     //動作周波数設定:CLOCK/16
    MTU4.TCR.BIT.CCLR = 1;     //TGRAのコンペアマッチでTCNTクリア
    MTU4.TMDR.BIT.MD = 2;      //PWMモード1に設定
    MTU4.TMDR.BIT.BFA = 1;     //TGRAとTGRCレジスタはバッファ動作
    MTU.TOER.BIT.OE4A = 1;     //出力許可 MTU4では必要
    MTU4.TIORH.BIT.IOA = 2;        //初期出力L,コンペアマッチでH
    MTU4.TIORH.BIT.IOB = 1;        //初期出力L,コンペアマッチでL
    MTU4.TGRA = 0;
    MTU4.TGRB = 40 - 1;
    MTU4.TGRC = 0;
    MTU4.TIER.BIT.TGIEA = 1;   //割り込み要求許可
    IR(MTU4,TGIA4) = 0;            //割り込み要求フラグクリア
    IEN(MTU4,TGIA4) = 1;       //割り込み要求許可
    IPR(MTU4,TGIA4) = 14;      //割り込み優先度設定
タイマのクロック設定

ソースを読むとTPSCというビットを2に設定しています。 TPSCはタイマープリスケーラー選択ビットで動作クロックの 何分の1でカウント動作を行うかということです。 コメントにもあるように1/16と言うことですが、 本マウスは秋月のRX220の動作周波数を32MHに設定していますので

32/16=2MHz

でタイマカウンタがカウントアップしていきます。 つまり1カウントの示す時間は

1/2/1e-6=0.5e-6=0.5μs

です。このソースでMTU3とMTU4のTGRA(タイマジェネラルレジスタA)の値でPWMの周期を設定し TGRB(タイマジェネラルレジスタB)の値でパルスの幅を指定します。 本マウスのモータドライバはパルスの立ち上がりに反応します。 パルスの幅はモータドライバのデータシートにある規定を満たしていればよく TGRBの値は39の固定値になっています。 ちなみに、モータドライバに印加するパルスの立ち上がり時間と 立ち下がり時間の最小値は1μsという仕様です。 パルス幅は

0.5μs*39=19.5μs

(ソースは40−1とわざわざしているので、これに関しては確認中です。 もしかしたらパルス幅は0.5*40になるのかもしれません。こう言うことは良くあります。 ハードウエアマニュアルを良く読む必要があります。)

タイマのバッファ動作

TMDRのBFAビットを1にしてバッファ動作を設定しています。 これによってタイマカウンタ(TCNT)の値がTGRAの値と一致すると(これをコンペアマッチと呼ぶ) TGRCの値がTGRAに自動的に転送されます。 これは、次のコンペアマッチが発生するまでにTGRCにモータの角速度を表す値を設定しておけば マイコンは他の作業をしていても良いということになり、シビアなモータの速度制御に遅延が発生しないですみます。

パルスの出力設定

本マウスは上のブロック図にも示しましたが左モータMTU3に右モータがMTU4に対応しています。 そして、左モータドライバがMTIOCA3A(P14)に右モータドライバがMTIOCA4A(PB3)に接続されています。 (この接続についてはソースのコメントに間違いがありますので確認訂正をお願いします。2019/10/31現在)

TIORHのIOAビット列を2にすると、初期値はLow出力ですが、TGRAのコンペアマッチが起きると TIOCA*AからHighが出力されます。 また、TIORHのIOBビット列を 1にすると、TGRBのコンペアマッチが起きるとTIOCA*AからLowが出力されます。

タイマ割り込み

TGRAの値でコンペアマッチが起こりTCNTが0にクリアされます。その際に、 TIERのTGIEAビットを1にしておけば割り込みが発生します。 以下に参照したのはMotor.c の中の割り込みによって呼ばれる関数です。

//左モータ割り込み
void motor_interrpt_l(void)
{
    motor_step_l++;
    motor_step_sum++;
}

//右モータ割り込み
void motor_interrpt_r(void)
{
    motor_step_r++;
    motor_step_sum++;
}

モータドライバに印加されたパルス数は記憶されません。 しかし、このパルス数がモータのステップ数すなわち回転量になりますので マウスがどのくらい進んだのかを調べるためにはパルス数を覚えておく必要があり これらの関数で何パルスモータドライバに与えられたかをカウントしています。

モータの速度を設定する関数

Motor.cの中で重要な関数は以下に参照するモータの速度を設定する関数です。

//モータの速度を変える関数
//speed_l  左モータ速度[mm/s]
//speed_r  右モータ速度[mm/s]
//正の場合は正転,負の場合は逆転
void change_motor_speed(short speed_l, short speed_r)
{
    unsigned short tgr_l, tgr_r;
    char start_l, start_r;

    turn_off_motor_timer(TURN_OFF_MOTOR_TIME);

    if(speed_l != 0){
        if(speed_l > 0){
            MOTOR_DIR_L = MOTOR_GO_L;
        }
        else{
            MOTOR_DIR_L = MOTOR_BACK_L;
            speed_l = -speed_l;
        }
        if(speed_l < MIN_SPEED){speed_l = MIN_SPEED;}
        if(speed_l > MAX_SPEED){speed_l = MAX_SPEED;}
        tgr_l = (unsigned short)((MOTOR_CLOCK / MOTOR_STEP_NUM * (long)(PI*TIRE_DIAMETER) / 1000) / speed_l);
        start_l = 1;
    }
    else{
        tgr_l = 0;
        start_l = 0;
    }

    if(speed_r != 0){
        if(speed_r > 0){
            MOTOR_DIR_R = MOTOR_GO_R;
        }
        else{
            MOTOR_DIR_R = MOTOR_BACK_R;
            speed_r = -speed_r;
        }
        if(speed_r < MIN_SPEED){speed_r = MIN_SPEED;}
        if(speed_r > MAX_SPEED){speed_r = MAX_SPEED;}
        tgr_r = (unsigned short)((MOTOR_CLOCK / MOTOR_STEP_NUM * (long)(PI*TIRE_DIAMETER) / 1000) / speed_r);
        start_r = 1;
    }
    else{
        tgr_r = 0;
        start_r = 0;
    }

    MOTOR_TGR_L = tgr_l;
    MOTOR_TGR_R = tgr_r;
    MOTOR_START_L = start_l;
    MOTOR_START_R = start_r;
}

マクロを使って可読性が高いソースになっています。 念のためソースコードの順とは逆行しますが、ソースの冒頭にあるマクロの定義を以下に参照します。

#define MOTOR_DIR_L     PORTH.PODR.BIT.B3
#define MOTOR_GO_L     0
#define MOTOR_BACK_L   1
#define MOTOR_DIR_R        PORTB.PODR.BIT.B5
#define MOTOR_GO_R     1
#define MOTOR_BACK_R   0

#define MOTOR_START_L  MTU.TSTR.BIT.CST3
#define MOTOR_TGR_L        MTU3.TGRC
#define MOTOR_START_R  MTU.TSTR.BIT.CST4
#define MOTOR_TGR_R        MTU4.TGRC

#define MOTOR_EN       PORTH.PODR.BIT.B2

#define MOTOR_CLOCK        (CLOCK * 1000000 / 16)

#define TURN_OFF_MOTOR_TIME    1000//モーターを無効にするまでの待ち時間[ms]
速度の設定=TGRAの値の決定

マウスを進めたい速度にするためにTGRAの値を決めなければなりませんが、ソースを読むと以下の式で計算されています。

TGRA=(π x タイヤ直径[mm] x モータ用タイマの動作クロック周波数[Hz] )÷(1周のモータのステップ数 x 設定したい速度[mm/s] )

{
\displaystyle 
\begin{equation}
TGRA=\frac{\pi D f_c }{400 v} 
\end{equation}
}

 

ソースは上式と全く同じではありません。その理由はRX220マイコン浮動小数点演算をハードウエア的に実行できません、 簡単に言えば小数同士の計算をするには物凄く時間がかかります。よって、全ての計算を整数で済ませる工夫が必要です。 例えば

3.141 x 24

のような計算をさせたい場合は

3141 x 24 / 1000

のような事を考えてコーディングする必要があります。小数点以下の計算結果は捨てられます。