キット製作研修マイクロマウス研修（kora編）
HM-StarterKit

マイクロマウス研修（kora編）[11]光センサの利用

2018.12.11

こんにちは。koraです。

今回はHM-StarterKitの光センサを使えるようにします。

AD変換の準備

HM-StarterKitには、壁を検出するための4つのLEDと4つの光センサが搭載されています。それぞれの光センサに対し、壁から跳ね返ってくる光量を電圧に変換して出力しています。電圧をマイコンから読み取るためにADC（Analog to Digital Converter）という機能を使います。ADCでは、リファレンス電圧である 3.3V と 0Vを基準として、電圧を12bit（0～4095）の値に変換します。

init.c

サンプルプログラムのStep7からio_init関数とsensor_init関数をコピーして、自作プロジェクトのinit.cのinit_all関数から呼び込むようにします。io_init関数は、光センサとセットになっているLEDの初期設定です。

/*****************************************************************************************
I/O設定
	LEDの設定	
*****************************************************************************************/
void io_init(void){
	
	//ブザー関連
	PORTB.PDR.BIT.B5 = 1;
		
	
	//赤外LEDのピン設定
	PORTA.PDR.BIT.B3 = 1;	//PA3を出力用に設定
	PORT1.PDR.BIT.B5 = 1;	//P15を出力用に設定
	PORT1.PDR.BIT.B4 = 1;	//P14を出力用に設定
	PORT3.PDR.BIT.B1 = 1;	//P31を出力用に設定
}

/*****************************************************************************************
A/DCの設定
	光センサとバッテリ電圧
*****************************************************************************************/
void sensor_init(void){

	//A/D変換用のピン設定
	SYSTEM.PRCR.WORD = 0xA502;
	MSTP_S12AD = 0;
	SYSTEM.PRCR.WORD = 0xA500;
	
	//A/DポートのPMR設定
	PORT4.PMR.BIT.B6=1;	//P46を周辺機器として使用
	PORT4.PMR.BIT.B2=1;	//P42を周辺機器として使用
	PORT4.PMR.BIT.B1=1;	//P41を周辺機器として使用
	PORT4.PMR.BIT.B0=1;	//P40を周辺機器として使用
	PORTE.PMR.BIT.B7=1;	//PE7を周辺機器として使用
	//A/DポートのPFS設定
	MPC.PWPR.BYTE=0x00;	//プロテクト解除
	MPC.PWPR.BYTE=0x40;	//プロテクト解除
	MPC.P46PFS.BIT.ASEL=1;	//A/D SEN_FR	AN006を使用
	MPC.P42PFS.BIT.ASEL=1;	//A/D SEN_R 	AN002を使用
	MPC.P41PFS.BIT.ASEL=1;	//A/D SEN_FR	AN001を使用
	MPC.P40PFS.BIT.ASEL=1;	//A/D SEN_R 	AN000を使用
	MPC.PWPR.BYTE=0x80;	//プロテクト作動
	
	//A/D変換(デフォルトでシングルモード)
	//S12AD.ADCSR.BYTE = 0x0c;	//A/D変換クロックはPCLKB(48M[ha])
	S12AD.ADCSR.BIT.CKS = 3;	//A/D変換のクロックをPCLKの1分周(48M[Hz])に設定
	S12AD.ADANS0.WORD = 0x0047;	//A/D変換をAN006のみ許可する
	S12AD.ADCSR.BIT.ADCS = 0;	//シングルスキャンモードに設定
}
mh_hm_starterkit.c

Step3のmain関数の一部をコピーして次のようにします。

void main(void)
{
    init_all();

    unsigned long i = 0;
    while(1){
        S12AD.ADANS0.WORD = 0x0047;	//A/D変換をAN000,1,2,6のみ許可する
        //センサ発光
        PORTA.PODR.BIT.B3 = 1;	//PA3をHighに設定
        PORT1.PODR.BIT.B5 = 1;	//P15をHighに設定
        PORT1.PODR.BIT.B4 = 1;	//P14をHighに設定
        PORT3.PODR.BIT.B1 = 1;	//P31をHighに設定
        for(i = 0; i < 100*1000/40; i++); //40で100usec 
        S12AD.ADCSR.BIT.ADST = 1;	//A/D変換を開始
        while(S12AD.ADCSR.BIT.ADST);	//A/D変換が終わるまで待つ
        PORTA.PODR.BIT.B3 = 0;	//PA3をLowに設定
        PORT1.PODR.BIT.B5 = 0;	//P15をLowに設定
        PORT1.PODR.BIT.B4 = 0;	//P14をLowに設定
        PORT3.PODR.BIT.B1 = 0;	//P31をLowに設定
        //センサの値を表示
        SCI_printf("sensor_fr = %d\n\r",S12AD.ADDR6);
        SCI_printf("sensor_r = %d\n\r",S12AD.ADDR2);
        SCI_printf("sensor_fl = %d\n\r",S12AD.ADDR1);
        SCI_printf("sensor_l = %d\n\r",S12AD.ADDR0);
        //バッテリー電圧のA/D変換
        S12AD.ADANS0.WORD = 0x0200;	//A/D変換をAN006のみ許可する
        S12AD.ADCSR.BIT.ADST = 1;	//A/D変換を開始
        while(S12AD.ADCSR.BIT.ADST);	//A/D変換が終わるまで待つ

        SCI_printf("V_BAT = %d\n\r",S12AD.ADDR9);
        for(i = 0; i < 100*1000*100; i++);

        //画面クリアシーケンス
        SCI_printf("\x1b[2J");			//クリアスクリーン[CLS]
        SCI_printf("\x1b[0;0H");		//カーソルを0,0に移動
    }
}

AD変換のテスト

TeraTermで出力されたセンサー値を表示すると、次のようになります。


壁なしの値


壁ありの値

ちなみに、HM-StarterKitはバッテリーの電圧もAD変換で測定できるように配線されていますが、バッテリー電圧がマイコンの動作電圧の3.3Vより高いので、抵抗で分圧して半分にしてから読み取るようになっています。

AD変換を割り込みで利用する

前回ジャイロセンサとエンコーダをタイマ割り込みで処理したように、今回も一定間隔でAD変換を行い、壁の情報を更新するようにします。

intprg.c

以下を追加します。

extern int_cmt1();

そして、void Excep_CMT1_CMI1(void){ } を次のように書き換えます。

void Excep_CMT1_CMI1(void){
    int_cmt1();
}
mytypedef.h

新しくファイルを作り、壁の情報を保存する構造体を作ります。

#ifdef _MYTYPEDEF
#else
typedef enum
{
    false = 0, //偽
    true = 1, //真
}t_bool; //真偽値を取り扱う列挙型

typedef struct
{
    short value; //現在の値
    short d_value; //差分フィルタ用
    short p_value; //１mS過去の値
    short p2_value;
    short error; //value - ref
    short ref; //リファレンス値
    short th_wall; //壁があるか否かの閾値
    short th_control; //制御をかけるか否かの閾値
    t_bool is_wall; //壁があるか無いか ( true = 壁あり false = 壁なし )
    t_bool is_control; //制御に使うか否か
}t_sensor; //センサ構造体

typedef struct
{
    float control; //最終的な制御量
    float omega; //目標角速度
    float p_omega;
    float theta; //目標角度
    float p_theta; //過去の目標角度
    float error; //偏差
    float p_error; //過去の偏差
    float diff; //偏差の微分値
    float sum; //偏差の積分値
    float sum_max; //積分値の制限値
    float kp; //比例制御定数
    float kd; //微分制御定数
    float ki; //積分制御定数
    t_bool enable; //制御のon/off
}t_control; //制御構造体

#define _MYTYPEDEF
#endif
interrupt.c

先ほど作った構造体をインクルードします。

#include "mytypedef.h"
#include "interface.h"

また、サンプルプログラムのStep7からint_cmt1関数をコピーします。なお、int_cmt1関数の割り込みは4kHzで発生しますが、その都度4つのセンサうち1つを読んでおり、トータルでは1kHzで更新しています。

glob_var.h

光センサ用と電源監視用のグローバル変数を追加します。

#include "mytypedef.h"
#include "parameters.h"

GLOBAL t_sensor sen_r, sen_l, sen_fr, sen_fl; //センサ構造体
GLOBAL t_control con_wall; //制御構造体

//電圧監視用グローバル変数
GLOBAL long cnt; //割り込み中のカウント
GLOBAL float V_bat; //電源電圧[V]
portdef.h

光センサのLEDを操作しやすいように次のマクロを追加します。

#define SLED_L (PORT1.PODR.BIT.B5) //左センサLED
#define SLED_R (PORT1.PODR.BIT.B4) //右センサLED
#define SLED_FL (PORT3.PODR.BIT.B1) //左前センサLED
#define SLED_FR (PORTA.PODR.BIT.B3) //右前センサLED
paramters.h

センサ関連のパラメータを追加します。
マウスが壁に近づけば光センサの値は大きくなり、逆に遠ざかれば小さくなります。これを利用して迷路の壁を頼りに走行を制御することを壁制御といいます。REF_SEN_R・Lは、マウスが迷路の中央を走るために参照します。TH_SEN_R・L・FR・FLは、マウスの右・左・前に壁があるか判断するための閾値です。CONTH_SEN_R・Lは右壁・左壁を壁制御に反映するかどうかの閾値です。今回の例では壁があると判断したら即、壁制御に反映するようになっています。

//センサ関連パラメータ
#define WAITLOOP_SLED 180 //LEDを光らせてからAD変換を開始するまでの時間稼ぎ用定数
#define REF_SEN_R 1200 //マウスを迷路中央に置いた時のセンサの値
#define REF_SEN_L 1200 //マウスを迷路中央に置いた時のセンサの値
#define TH_SEN_R 50 //壁があるか否かの閾値 車体を区画の左へ寄せた時のセンサ値(壁あり)
#define TH_SEN_L 50 //壁があるか否かの閾値 車体を区画の右へ寄せた時のセンサ値(壁あり)
#define TH_SEN_FR 50 //壁があるか否かの閾値
#define TH_SEN_FL 50 //壁があるか否かの閾値
#define CONTH_SEN_R TH_SEN_R //制御をかけるか否かの閾値
#define CONTH_SEN_L TH_SEN_L //制御をかけるか否かの閾値
#define CON_WALL_KP (10.0) //壁センサによる姿勢制御の比例制御の比例定数

実際走行する際には、これらの値を環境に合わせて調整します。

init.c

サンプルプログラムのStep7からinit_parameters関数をコピーします。ここではparameters.hで指定したパラメータを光センサ用の構造体に代入しています。

/*****************************************************************************************
光センサー系のパラメータ初期化
リファレンスとか壁の閾値とか
*****************************************************************************************/
void init_parameters(void)
{
    sen_r.ref = REF_SEN_R; //右センサのリファレンス値を初期化
    sen_l.ref = REF_SEN_L; //左センサのリファレンス値を初期化

    sen_r.th_wall = TH_SEN_R; //右センサの壁有無判断の閾値を初期化
    sen_l.th_wall = TH_SEN_L; //左センサの壁有無判断の閾値を初期化

    sen_fr.th_wall = TH_SEN_FR; //右前センサの壁有無判断の閾値を初期化
    sen_fl.th_wall = TH_SEN_FL; //左前センサの壁有無判断の閾値を初期化

    sen_r.th_control = CONTH_SEN_R; //右センサの壁制御かけるか否かの閾値を初期化
    sen_l.th_control = CONTH_SEN_L; //左センサの壁制御かけるか否かの閾値を初期化

    con_wall.kp = CON_WALL_KP/10000.0; //壁比例制御の比例定数を初期化
}

また、忘れずにinit_all関数からinit_parameters関数を呼ぶようにします。

init_parameters();
my_hm_starterkit.c

テスト用にmain関数を次のように書き換えます。

void main(void)
{
    init_all();

    unsigned long i = 0;
    while(1){
        SCI_printf("sensor_fr.is_wall%d\n\r",sen_fr.is_wall);
        SCI_printf("sensor_r.is_wall%d\n\r",sen_r.is_wall);
        SCI_printf("sensor_fl.is_wall%d\n\r",sen_fl.is_wall);
        SCI_printf("sensor_l.is_wall%d\n\r",sen_l.is_wall);
        for(i = 0; i < 100*1000*100; i++);

        //画面クリアシーケンス
        SCI_printf("\x1b[2J");			//クリアスクリーン[CLS]
        SCI_printf("\x1b[0;0H");		//カーソルを0,0に移動
    }
}

割り込みのテスト

実行した結果をTeraTermで表示すると次のようになります。右・左・右前・左前の光センサの値がそれぞれTH_SEN_R・L・FR・FLで指定した値より大きければ、壁の有無を1・0で出力します。

次回

ジャイロ、エンコーダ、光センサの値を読み込めるようになりました。これでマウスの自律走行が可能になります。次回はこれを実装したいと思います。

タイトルとURLをコピーしました