Arduinoとトワイライトを使って距離データをモニターする

組み込みエンジニア

こんにちは、ENGかぴです。

Arduinoとトワイライトを使って超音波距離センサーUS-015から取得した距離のデータを表示するシステムを作ることができます。ArduinoとUS-015で一定間隔で距離データを計算しトワイライトで無線通信を使用してモニターします。

今回はArduinoとUS-015とトワイライトを組み合わせた距離測定モジュールを製作してみました。下記リンクの応用編になります。US-015単体の動作については下記リンクを参考にしてください。

Arduinoと超音波距離センサーUS-015で距離を測定

Arduino UNOを使って動作確認を行ったことを下記リンクにまとめています。

Arduinoで学べるマイコンのソフト開発と標準ライブラリの使い方

トライライト(TWELITE)で動作確認したことについてまとめています。

トワイライト(TWELITE)のソフト開発と無線通信でできること

Arduinoとトワイライトの組み合わせの全体構成

Arduinoとトワイライトの組み合わせの全体構成
Arduinoとトワイライトの組み合わせの全体構成

親機はMONOSTICK(USBスティックの親機)を使用してパソコンでモニタ(Tera Term)します。親機はタイムスタンプを表示しながら子機からの受信を待ちます。動作手順は以下の通りです。

  1. Arduinoで超音波距離センサ(US-015)から距離データを取得し計算する
  2. トワイライトがウェイクアップしたらArduinoに対して距離データを要求する
  3. トワイライトはArduinoから取得した距離データを親機に無線通信する
  4. トワイライトは無線通信後スリープする

ArduinoはUS-015に対して一定間隔でパルス出力を行い距離データを計算します。トワイライトから距離データの要求電文が受信できれば距離データを送信します。トワイライトは消費電流を抑えるためにスリープを使って一定間隔(今回は1秒)毎に距離データの要求を行います。

スリープすることによって数mAの消費が1uA程度に抑えられるため電池が少しでも長く使えるようになります。MONOSTICKは距離データをモニタ表示するためにデータを換算するようにします。

Arduinoとトワイライトによる子機の構成(距離測定モジュール)

子機の構成(距離測定モジュール)
子機の構成(距離測定モジュール)

Arduinoは5V電源で動作しトワイライトは3.3V電源で動作するためArduinoの送信側の電圧をそのまま印加することができません。そのためR1とR2によってArudinoからトワイライトに向かう信号を分圧しています。Arduinoはトワイライトの出力の3.3Vを受けてもHレベルと判定できるためArduinoのRXに直接信号を入力しています。

Arduinoへの配線はトワイライトをシリアル通信に超音波距離センサをDIDO接続しています。Arduinoは距離センサーから250ms(任意でよい)毎に距離データを取得して4回分のデータを平均化してばらつきを押さえます。(ソースコード全体のUs015Filter()部分)

//トワイライトからシリアル通信を受信
void loop(){
    while( Serial.available()){
        rxarduino.buf[rxarduino.wp] = Serial.read();

        if( ++rxarduino.wp >= RING_SZ ){ //次にデータを入れる場所を更新
            rxarduino.wp = 0;
        }
    }
    RxDataChk();   //受信したデータを確認して距離データを返信
}
/* RX function add */
void RxDataChk(){
        
    if( chkdat[0]== HEADER1 && chkdat[1]== HEADER2 && chkdat[2]== HEADER3 ){ //ヘッダーをチェック 
        if( rxsz >= allsz ){ 
            /*電文サイズ長分を格納*/
            rxdata.sum = CalcSum(&rxdata.buf[0],sz); //電文のSUMを計算
            if( rxdata.sum == rxdata.buf[sz]){ //SUMのチェック
                if( rxdata.buf[0] == 1 ){
                    SerialTxSet(); //距離データを返信する関数
                }
            }
        }
    }
}

トワイライトがスリープからウェイクアップしたときシリアル通信を使ってデータの要求を行います。Arduinoはトワイライトから送信された電文をSerial.read()で受信してRxDataChk()で電文のチェックを行います。

Arduinoが受信するシリアルデータ(トワイライトが送信)もトワイライトのバイナリデータの電文フォーマットに従ってHEADER1(0xA5)・HEADER2(0x5A)・HEADER3(0x80)として電文ヘッダが一致することを確認してから受信データを受け取りSUMが一致すると正常なデータとして受け取るようにします。

外部機器と接続する際はメインとなる機器に対して電文が共通になるように管理すると可読性が良くなります。

電文が問題なければSerialTxSet()でUS-015から取得した距離データを返信する処理を行います。

/* TX function add */
void SerialTxSet(){
    uint8_t *adrs;
    uint8_t allsz = 0;

    txdata.header[0] = HEADER1; //0xA5
    txdata.header[1] = HEADER2; //0x5A
    txdata.header[2] = HEADER3; //0x80
    txdata.sz = sizeof(us015.dat);
    txdata.buf[0] = (us015.dat >> 24); //uint32のデータをuint8に変換するためシフトして代入
    txdata.buf[1] = (us015.dat >> 16);
    txdata.buf[2] = (us015.dat >> 8);
    txdata.buf[3] = (us015.dat & 0xFF);
  
    txdata.sum = CalcSum(&txdata.buf[0],txdata.sz); //SUM値を代入
    allsz = sizeof(txdata.header)+ sizeof(txdata.sz)+ txdata.sz; //データ長

    adrs =&txdata.header[0]; //ポインタアドレスで先頭アドレスを指定
    for(uint8_t i = 0; i < allsz; i++ ){
        Serial.write(*adrs); //ポインタが示すアドレスに格納されている値
        adrs++; //ポインタ1バイト分進める
    }
    Serial.write(txdata.sum);
}

トワイライトにバイナリデータでシリアル通信を行うためにはヘッダを含めてルールに従う必要があります。詳細は下記のリンクで説明しています。

トワイライト(TWELITE)とArduino間でシリアル通信する

トワイライトのSerialParser()の電文フォーマットに従って送信電文を作成します。

距離データは4バイトデータですが、txdata.buf[]は1バイトデータとして定義しているため4バイトデータをそのまま格納することができないためシフトによる分割を行って格納しています。

距離データしか送らないためtxdata.buf[]を4バイトデータとして宣言し4バイトデータをセットする方法もあります。この方法においてもデータ送信する際に1バイトのポインタとしてアドレスを更新しながらセットすることで4バイトデータも1バイトの単位で送信できます。

トワイライトの親機

トワイライトの親機はMONOSTICK(USBタイプ)子機からデータを受信するとシリアル通信を使ってArduinoにデータを送信するようにします。

void receive() {
    //子機から無線データを受けた場合先に文字ヘッダを格納し判定する
    if (strncmp(APP_FOURCHAR, fourchars, 4)) {return; }

    expand_bytes(np,rx.get_payload().end(),us015);
    us015_1 = us015 / 100; //整数部分
    us015_2 = us015 % 100; //小数点以下

    Serial << "ts:" << int(ts) << ":" 
	   << " Present Length is: " << (int)us015_1 << "." << (int)us015_2
           << "mm" << mwx::crlf << mwx::flush;
}

親機と子機の間でアプリケーションの判定をするため文字ヘッダによるデータの判別を行っています。strncmp()で文字ヘッダを比較して同一であれば残りのデータを受け入れる処理を行います。

US-015の距離データは100倍値になっているので100で割った商が整数部となり余りが小数点以下の値になります。これらをトワイライトのSerialモニタで表示するようにしています。

動作確認

Arduino+US-015+トワイライトによる距離測定モジュールの動作確認
Arduino+US-015+トワイライトによる距離測定モジュールの動作確認

本棚を使って動作確認を行いました。本棚は床から870mmの高さのものであり本棚の上に今回製作した距離測定モジュールを設置しました。Tera Termのモニタで確認すると割と正確に取得できていることが分かります。

段ボールを使ってUS-015やArduinoを固定できるようにしましたが設置環境によって誤差が出ることもありました。130mmの対象物を測定すると128mm付近になったりと不安定な部分もありましたが、誤差は大きくても30mm以内に収まる傾向でした。

自作の段ボールによる固定の誤差も大きく出ている可能性や音波を当てている床を移動しただけでも結果が数十mm変わることがあったので測定環境の影響もありそうです。誤差はありますが、それなりに測定できており段ボールで固定したりと作るのが面白かったの満足しています。

ソースコード全体

以下のソースコードはコンパイルして動作確認をしております。チェックはしておりますがコメントなど細かな部分で間違っている可能性があります。参考としてお使いいただければと思います。

親機側:Arduinoのスケッチ:

#include <MsTimer2.h>

#define TIME_UP 0
#define TIME_OFF -1
#define BASE_CNT 10 //10msがベースタイマとなる
#define TIM_US015 25
#define FLT_MAX 4

#define HEADER1 0xA5
#define HEADER2 0x5A
#define HEADER3 0x80
#define RING_SZ 256
#define BUF_SZ 64
#define RXCHK_SZ 4

struct DIS_MEAN{
  uint8_t wp;
  unsigned long buf[FLT_MAX];
  unsigned long dat;
};

struct COM_TYP{
  uint8_t header[3];
  uint8_t sz;
  uint8_t buf[BUF_SZ];
  uint8_t sum;
};

struct RING_MNG{
  uint8_t wp;
  uint8_t rp;
  uint8_t buf[RING_SZ];
};

unsigned int EchoPin = 2;
unsigned int TrigPin = 3;
unsigned long Time_Echo_us = 0;
unsigned long Len_mm_X100 = 0;
DIS_MEAN us015;
int16_t timus015 = TIME_OFF;
int16_t cnt10ms;

COM_TYP txdata;
COM_TYP rxdata;  //チェック後の受信データ
RING_MNG rxarduino;

/*** Local function prototypes */
void TimerCnt();
void Us015Meas();
bool Us015Filter();

void SerialTxSet();
uint8_t CalcSum(uint8_t *buf, uint8_t sz);
void RxDataChk();

void setup(){
  
    Serial.begin(115200);
    pinMode(EchoPin, INPUT);
    pinMode(TrigPin, OUTPUT);

    MsTimer2::set(1,TimerCnt); //1msごとに関数へ遷移
    MsTimer2::start();

    memset(&us015.buf, 0xFFFFFFFF, sizeof(us015.buf));
    timus015 = TIM_US015;
}   

void loop(){

    Us015Meas();
    mainTimer();
    
    while( Serial.available()){
        rxarduino.buf[rxarduino.wp] = Serial.read();

        if( ++rxarduino.wp >= RING_SZ ){ //次にデータを入れる場所を更新
            rxarduino.wp = 0;
        }
    }

    RxDataChk();    
}
/* US-015のデータを取得 function add */
void Us015Meas(){

    if( timus015 == TIME_UP ){
        timus015 = TIM_US015;
        digitalWrite(TrigPin, HIGH);
        delayMicroseconds(50);
        digitalWrite(TrigPin, LOW);

        Time_Echo_us = pulseIn(EchoPin, HIGH);
      
        if((Time_Echo_us < 60000) && (Time_Echo_us > 1)){
            Us015Filter();
        }
    }
}
/* US-015のデータを平均化 function add */
bool Us015Filter(){
    bool ret = true;
    uint8_t i;
    uint32_t sum = 0;

    Len_mm_X100 = (Time_Echo_us*34)/2;
    us015.buf[us015.wp] = Len_mm_X100;

    for(i=0; i < FLT_MAX; i++){
        if( us015.buf[i] == 0xFFFFFFFF ){ //初期化時の値が残っていれば計算しないようにする
            ret = false;
            break;
        }
        sum += us015.buf[i];
    }

    if( ret ){
        us015.dat = sum >> 2; //バッファ数の8の平均をとるため右に3回シフト
       //us015.dat -=500; //100倍値なので5mm減らすため-500(基準を円筒のトップにする)
    }
  
    if( ++us015.wp >= FLT_MAX ){
        us015.wp = 0;
    }
    
    return ret;
}
/* callback function add */
void TimerCnt(){
    ++cnt10ms;
}
/* Timer Management function add */
void mainTimer(){

    if( cnt10ms >= BASE_CNT ){ //10msごとにここに遷移する
        cnt10ms -=BASE_CNT;
    
        if( timus015 > TIME_UP ){
            timus015--;
        }
    }
}

/* TX function add */
void SerialTxSet(){
    uint8_t *adrs;
    uint8_t allsz = 0;

    txdata.header[0] = HEADER1; //0xA5
    txdata.header[1] = HEADER2; //0x5A
    txdata.header[2] = HEADER3; //0x80
    txdata.sz = sizeof(us015.dat);
    txdata.buf[0] = (us015.dat >> 24);
    txdata.buf[1] = (us015.dat >> 16);
    txdata.buf[2] = (us015.dat >> 8);
    txdata.buf[3] = (us015.dat & 0xFF);
  
    txdata.sum = CalcSum(&txdata.buf[0],txdata.sz);
    allsz = sizeof(txdata.header)+ sizeof(txdata.sz)+ txdata.sz;

    adrs =&txdata.header[0];
    for(uint8_t i = 0; i < allsz; i++ ){
        Serial.write(*adrs);
        adrs++;
    }
    Serial.write(txdata.sum);
}
/* ChkSum function add */
uint8_t CalcSum(uint8_t *buf, uint8_t sz=0){
    uint8_t ret = 0;

    for(uint8_t i=0; i < sz; i++ ){
        ret = ret ^ (*buf);
        buf++; 
    }
    return ret;
}
/* RX function add */
void RxDataChk(){
    int rxsz;
    uint8_t sz;
    uint8_t allsz;
    uint8_t *adrs;
    uint8_t rp = rxarduino.rp;
    uint8_t chkdat[RXCHK_SZ];

    rxsz = rxarduino.wp - rxarduino.rp; //受信データ数の算出
    if( rxsz < 0 ){
        rxsz = rxsz + RING_SZ;
    }

    if(rxsz >= RXCHK_SZ){
        for(uint8_t i=0; i< RXCHK_SZ; i++ ){  //ヘッダーのチェックのため一時保存
            chkdat[i] = rxarduino.buf[rp];
            if(++rp > RING_SZ ){
                rp = 0;
            }
        }
        if( chkdat[0]== HEADER1 && chkdat[1]== HEADER2 && chkdat[2]== HEADER3 ){
            sz = chkdat[3] & 0x3F;//szが63を超えないようにする(データ長がおかしい場合の対策)
            allsz = RXCHK_SZ + sz + 1; //+1はSUMで受信データすべての長さを算出

            if( rxsz >= allsz ){
                adrs = &rxdata.header[0];
                for(uint8_t i=0; i< allsz; i++ ){
                    *adrs = rxarduino.buf[rxarduino.rp];
                    adrs++;
                    if(++rxarduino.rp >= RING_SZ ){
                        rxarduino.rp = 0;
                    }
                }
                rxdata.sum = CalcSum(&rxdata.buf[0],sz);
                if( rxdata.sum == rxdata.buf[sz]){//SUMのチェック
                    if( rxdata.buf[0] == 1 ){
                        SerialTxSet();
                    }
                }
            }
        }
     }else{
        if(++rxarduino.rp >= RING_SZ ){
            rxarduino.rp = 0;
        }
    }
}

子機:トワイライト

#include <TWELITE>
#include <NWK_SIMPLE>

#define TIME_UP 0
#define TIME_OFF -1
#define ZIG_WAIT_MAX 100
#define DATASZ 4   //uint32のデータを1つ送る
#define SLEEP_MAX 950
#define HEADER1 0xA5
#define HEADER2 0x5A
#define HEADER3 0x80
#define US015CMD 0x01
#define BUF_SZ 64

enum  E_STATE {
    INIT = 0,    
    WORK_JOB,    
    TX,         
    WAIT_TX,     
    EXIT_NORMAL, 
    EXIT_FATAL   
};

struct  COM_TYP{
  uint8_t buf[BUF_SZ];
};

struct TXCOM_TYP{
  uint8_t header[3];
  uint8_t sz;
  uint8_t buf[BUF_SZ];
  uint8_t sum;
};
/*** Config part */
const uint32_t APP_ID = 0x1234abcd; // application ID
const uint8_t CHANNEL = 13; // channel
/*** application defs */
const char APP_FOURCHAR[] = "TEMP";

// application use
E_STATE eState;
int16_t timzigwait = TIME_OFF;
uint8_t u8txid = 0;
uint8_t u8devid = 0x00;
byte us015data[DATASZ];
MWX_APIRET txreq_stat;
uint8_t errres_cnt =0;
uint32_t u32millis_tx; 
COM_TYP rcvdata;
TXCOM_TYP txdata;

/*** Local function prototypes */
void sleepNow();
MWX_APIRET transmit();
void RcvData(uint8_t *b);
uint8_t CalcSum(uint8_t *buf, uint8_t sz);
void SerialTxSet();

/*** the setup procedure (called on boot) */
void setup() {

    SerialParser.begin(PARSER::BINARY,128);
    txreq_stat = MWX_APIRET(false, 0);
    the_twelite
	<< TWENET::appid(APP_ID)    
	<< TWENET::channel(CHANNEL) 
	<< TWENET::rx_when_idle(false ); 

    auto&& nwksmpl = the_twelite.network.use<NWK_SIMPLE>();
    nwksmpl << NWK_SIMPLE::logical_id(0xFE); 
    the_twelite.begin();
}
/*** the loop procedure (called every event) */
void loop() {
    bool loop_more; 

    do {
        loop_more = false; 

        switch(eState) {
	case E_STATE::INIT:
	    eState = E_STATE::WORK_JOB;
	    loop_more = true;
	    timzigwait = ZIG_WAIT_MAX;
	    break;
	case E_STATE::WORK_JOB:
            while(Serial.available()){
                if(SerialParser.parse(Serial.read())){
                    auto&& b = SerialParser.get_buf();
                    RcvData(&b[0]);
                    eState = E_STATE::TX;
                    timzigwait =TIME_OFF;
                }
            }

            if(TickTimer.available()){
                if(timzigwait > TIME_UP){
                    --timzigwait;
                }
          
                if(timzigwait == TIME_UP){
                    eState = E_STATE::INIT;
                    SerialTxSet();
                }
                loop_more = true;
            }
	    break;
	case E_STATE::TX:
            txreq_stat = transmit();
            if (txreq_stat) {
                u32millis_tx = millis();
                eState = E_STATE::WAIT_TX;
                loop_more = true;
            } else {
                eState = E_STATE::EXIT_FATAL;
                loop_more = true;
            }
            break;
        case E_STATE::WAIT_TX:
            if (the_twelite.tx_status.is_complete(txreq_stat.get_value())) {
                eState = E_STATE::EXIT_NORMAL;
            } else if (millis() - u32millis_tx > 100) {
                eState = E_STATE::EXIT_FATAL;
                loop_more = true;
            }
	    break;
	case E_STATE::EXIT_NORMAL:
	    sleepNow();
	    break;
	case E_STATE::EXIT_FATAL:
	    delay(100);
	    the_twelite.reset_system();
	    break;
	}
    } while (loop_more);
}
/* callback begin */
void begin(){
    eState = E_STATE::INIT;
    SerialTxSet();
}
/* callback wakeup */
void wakeup(){
    eState = E_STATE::INIT;
    SerialTxSet();
}
/* perform period sleep */
void sleepNow(){
    uint32_t u32ct = SLEEP_MAX + random(50,100);
    the_twelite.sleep(u32ct);
}
/* transmit a packet */
MWX_APIRET transmit() {

    if (auto&& pkt = the_twelite.network.use<NWK_SIMPLE>().prepare_tx_packet()) {
	pkt << tx_addr(u8devid)  
	    << tx_retry(0x1) 
	    << tx_packet_delay(0,0,2); 

	pack_bytes(pkt.get_payload() , make_pair(APP_FOURCHAR, 4));
        
        for(int i = 0; i < DATASZ; i++){
            pack_bytes(pkt.get_payload(),us015data[i]);
        }

	return pkt.transmit();
    }
    return MWX_APIRET(false, 0);
}
/* TX function add */
void SerialTxSet(){
    uint8_t *adrs;
    uint8_t allsz = 0;

    txdata.header[0] = HEADER1;
    txdata.header[1] = HEADER2;
    txdata.header[2] = HEADER3;
    txdata.sz = 1; 
    txdata.buf[0] = US015CMD;
    txdata.sum = CalcSum(&txdata.buf[0],txdata.sz);
    allsz = sizeof(txdata.header)+ sizeof(txdata.sz)+ txdata.sz;

    adrs =&txdata.header[0];
    for(uint8_t i = 0; i < allsz; i++ ){
        Serial.write(*adrs);
        adrs++;
    }
    Serial.write(txdata.sum);
}
/* ChkSum function add */
uint8_t CalcSum(uint8_t *buf, uint8_t sz=0){
    uint8_t ret = 0;

    for(uint8_t i=0; i < sz; i++ ){
        ret = ret ^ (*buf);
        buf++; 
    }
    return ret;
}
/* RX function add */
void RcvData(uint8_t *b ){
    uint8_t i;
    memcpy(&rcvdata.buf[0],b,sizeof(rcvdata));

    for( i=0;i< sizeof(us015data);i++){
        us015data[i] = rcvdata.buf[i];
    }
}

親機側:MONOSTICK(トワイライト)

// use twelite mwx c++ template library
#include <TWELITE>
#include <NWK_SIMPLE>
#include <MONOSTICK>

#define CNT_MAX 1000

/*** Config part */
const uint32_t APP_ID = 0x1234abcd;
const uint8_t CHANNEL = 13;
int16_t cnt1ms;
uint32_t ts;
bool rcvflg;
/*** application defs */
const char APP_FOURCHAR[] = "TEMP";
uint8_t u8devid = 0;
/*** function prototype */
void receive();
/*** application defs */

/*** setup procedure (run once at cold boot) */
void setup() {
	
    auto&& brd = the_twelite.board.use<MONOSTICK>();
    brd.set_led_red(LED_TIMER::ON_RX, 200); // RED (on receiving)
    brd.set_led_yellow(LED_TIMER::BLINK, 500); // YELLOW (blinking)

    the_twelite
        << TWENET::appid(APP_ID)    
	<< TWENET::channel(CHANNEL)
	<< TWENET::rx_when_idle();  

    auto&& nwksmpl = the_twelite.network.use<NWK_SIMPLE>();
    nwksmpl << NWK_SIMPLE::logical_id(0x00);

    the_twelite.begin(); 
}
/*** loop procedure (called every event) */
void loop() {

    if(TickTimer.available()){
        ++cnt1ms;
    }

    while (the_twelite.receiver.available()) {
        receive();
	rcvflg = true;
    }

    if( cnt1ms >= CNT_MAX){
        cnt1ms -= CNT_MAX;

	ts = (ts + 1 ) % 100000;

	if(rcvflg){
	    rcvflg = false;
	}else{
	    Serial << "ts:" <<  int(ts) << mwx::crlf << mwx::flush;
	}
    }
}
/*add function recive()*/
void receive() {
    uint32_t us015;
    double us015_1;
    double us015_2;
    uint8_t buf[10];
    uint8_t i;

    auto&& rx = the_twelite.receiver.read();
    char fourchars[5]{}; 
    auto&& np = expand_bytes(rx.get_payload().begin(),rx.get_payload().end(),make_pair((uint8_t*)fourchars,4));
    // check header
    if (strncmp(APP_FOURCHAR, fourchars, 4)) {return; }

    expand_bytes(np,rx.get_payload().end(),us015);
    us015_1 = us015 / 100; //整数部分
    us015_2 = us015 % 100; //小数点以下

    Serial << "ts:" << int(ts) << ":" 
	   << " Present Length is: " << (int)us015_1 << "." << (int)us015_2
           << "mm" << mwx::crlf << mwx::flush;
}

参考記事のArduinoと超音波距離センサーUS-015で距離を測定では超音波距離センサの円筒のトップを基準にするために5mm分補正していましたが、距離が長くなるほど誤差が気にならなくなったためコメントアウトしています。

関連リンク

Arduinoのライブラリを使って動作確認を行ったことを下記リンクにまとめています。

Arduinoで学べるマイコンのソフト開発と標準ライブラリの使い方

Seeeduino XIAOで学べるソフト開発と標準ライブラリの使い方

ESP32-WROOM-32Eで学べるソフト開発と標準ライブラリの使い方

また無線モジュールであるトワイライトを使ってソフト開発したことについてまとめています。

トワイライト(TWELITE)のソフト開発と無線通信でできること

GEEKJOB-未経験からITエンジニアに【オンライン無料体験】

最後まで、読んでいただきありがとうございました。

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