忍者ブログ

Fグループ電子工作講座

秋月電子SH7125ボードで始めるマイコン開発

台から落ちない制御

前回SH7125を使って一応台から落ちない制御を行いましたが、台から落ちないものの台の渕に真正面から突っ込むと両方のセンサーが反応してその場で停止してしまったはずです。

そこでもうちょっと真面目に台から落ちない制御を行います。
何もないときに直進するのはそのままにして

※2019/06/23誤記修正
片方の履帯が落ちそうなときに超新地旋回超信地旋回(そのバターン)させます。

正面から落ちそうなときはバックさせます。
この方式でその場で止まったままになることはなくなります。

右センサー反応しているとき、左モーター正転(前進)
右センサー反応していないとき、左モーター逆転(後退)
左センサー反応しているとき、右モーター正転(前進)
左センサー反応していないとき、右モーター逆転(後退)

モーターの逆転にも対応させるため、PWMの出力先を変更します。
右クローラ:PE0 ⇒ 正転PE12、逆転PE13
左クローラ:PE1 ⇒ 正転PE14、逆転PE15
PE0とPE1はセンサーの動作確認用のインジケータとして使用しておきます。
モータドライバTA7291との接続も

これに合わせてプログラムを修正します。
#include "iodefine.h"
#include "io_setup.h"
#include "intMTU.h"
#include "ADConv.h"

#define SENS_THRESHOLD    800    //判定値
#define PWM_MAX    400        //PWM出力

void main(void){
    short sens_R=0,sens_L=0;
    short sens_R_on=0,sens_L_on=0;
    short pwm_out_L=0,pwm_out_R=0;

    hardware_setup();        //汎用IOの初期化
    init_MTU();            //PWM機能の初期化
    init_ADC();            //AD変換機能の初期化
 
    while(1){
        check_ai();        //AD変換を実行
        sens_R=get_ai(0);    //AD変換値を取得
        sens_L=get_ai(1);    //AD変換値を取得
        sens_R_on=(sens_R>SENS_THRESHOLD)? 1:0;
        sens_L_on=(sens_L>SENS_THRESHOLD)? 1:0;

        pwm_out_R=(sens_L_on==0)?-PWM_MAX:PWM_MAX;
        pwm_out_L=(sens_R_on==0)?-PWM_MAX:PWM_MAX;


        set_pwm( PWM_PE0 , sens_R_on*1000);    //PWM出力
        set_pwm( PWM_PE1 , sens_L_on*1000);    //PWM出力
        set_pwm2( PWM_PE12 , PWM_PE13 , pwm_out_R);    //PWM出力
        set_pwm2( PWM_PE14 , PWM_PE15 , pwm_out_L);    //PWM出力
    } // end while
}

台から落ちない制御の場合、PWM_MAXの値はもう少し大きくても大丈夫そうです。

配線が正しく接続されている事を確認したらテストしてみます。
これで台の渕に真正面から突っ込んだ場合でも停止することは無くなったはずです。

ただ、台の角でウネウネ動いてまともに進ず実質的にその場にとまったままになります。
ということでタイマーを使った処理を行います。

真面目に台から落ちない制御

正直なところ、ここまではライントレースの流用でマイコンを使わなくても動くようなプログラムでした。今回はもうちょっと処理してるっぽいプログラムを作成します。
台車や回路類は前回の物をそのまま使用します。

プログラムの機能はAD、PWMに加えてタイマーを使用します。
使用する設定ファイルは
 汎用IO設定ソース:io_setup.c
 汎用IO設定ヘッダ:io_setup.h
 PWM設定ソース:intMTU.c
 PWM設定ヘッダ:intMTU.h
 AD変換設定ソース:ADConv.c
 AD変換設定ヘッダ:ADConv.h
 CMT設定ソース:intCMT.c ← タイマー機能
 CMT設定ヘッダ:intCMT.h ← タイマー機能

以上をプロジェクトに読み込みます。
※intCMT.cとintCMT.hは初期化関数名が他と統一できていなかったので修正を加えました。
※既にダウンロードされている方はダウンロードして上書きしてください。

プロジェクトに読み込み後は今まで通り
resetprg.c 内 #define SR_Init の編集と
intprg.c 内 のMTUおよびCMT関連の個所の編集を行います。
該当箇所は各ページを参照してください。

サンプルでプログラムを作成してみました。
動作の優先順位は
1)センサが左右とも反応していないときは後退する
2)センサがどちらか反応するようになっても一定時間(1000ms)後退を続ける
3)十分後退したら一定時間(3000ms)旋回する
4)通常の制御を行う。
とします。

#include "iodefine.h"
#include "io_setup.h"
#include "intMTU.h"
#include "ADConv.h"
#include "intCMT.h"

#define SENS_THRESHOLD    800    //判定値
#define PWM_MAX    600        //PWM出力

void main(void){
    short sens_R=0,sens_L=0;
    short sens_R_on=0,sens_L_on=0;
    short pwm_out_L=0,pwm_out_R=0;
    unsigned long timer_back=0;
    unsigned long timer_turn=0;
    short mode_back=0;
    short mode_turn=0;

    hardware_setup();        //汎用IOの初期化
    init_MTU();            //PWM機能の初期化
    init_ADC();            //AD変換機能の初期化
    init_CMT0( 1 );      //タイマ機能の初期化(1ms)

    while(1){
        check_ai();        //AD変換を実行
        sens_R=get_ai(0);    //AD変換値を取得
        sens_L=get_ai(1);    //AD変換値を取得
        sens_R_on=(sens_R>SENS_THRESHOLD)? 1:0;
        sens_L_on=(sens_L>SENS_THRESHOLD)? 1:0;

        //後退用タイマーを設定
        if((sens_R_on==0 && sens_L_on==0)){    //両方のセンサーが反応していない
            timer_back=getsystime()+1000;    //後退タイマーを現在時刻+1000msに設定
        }

        if(timer_back>getsystime()){    //timer_backが設定されているときは後退する
            pwm_out_R=-PWM_MAX;
            pwm_out_L=-PWM_MAX;
            timer_turn=getsystime()+3000;    //旋回タイマーを現在時刻+3000msに設定
        }else if(timer_turn>getsystime()){    //timer_turnが設定されているときは右旋回する
            pwm_out_R=-PWM_MAX;
            pwm_out_L=PWM_MAX;
        }else{    //通常の走行モード
            pwm_out_R=(sens_L_on==0)?-PWM_MAX:PWM_MAX;
            pwm_out_L=(sens_R_on==0)?-PWM_MAX:PWM_MAX;
        }

        set_pwm( PWM_PE0 , sens_R_on*1000);    //PWM出力
        set_pwm( PWM_PE1 , sens_L_on*1000);    //PWM出力
        set_pwm2( PWM_PE12 , PWM_PE13 , pwm_out_R);    //PWM出力
        set_pwm2( PWM_PE14 , PWM_PE15 , pwm_out_L);    //PWM出力
    } // end while
}

このプログラムではタイマー処理をしている間でも待機処理(空のfor文等)を行っていません。
タイマー部分が少々ややこしいかもしれません。
・ある条件を満たすと現在時刻から一定時間進んだ時刻をタイマーに設定する。
・ある条件を満たしている間はタイマーに設定する時刻を更新しつづける(先延ばしにする)
・今の時刻とタイマーに設定した時刻を比較して設定した時刻になるまでは何かの動作を行う。
・タイマーが切れたら元の動作に戻る。
といった動作になります。
これでそれらしい動きをすると思います。

直進しっぱしの時はintprg.cのCMT割り込みが設定できていない可能性があります。
電池切れになると動作が不安定になります。

関連:ライントレース関連記事一覧
PR

コメント

プロフィール

HN:
ぼんどF博士
性別:
男性

最新コメント