配列を使用し記憶走行を行う

Dependencies:   mbed AQM0802 CRotaryEncoder TB6612FNG

main.cpp

Committer:
yusaku0125
Date:
2019-08-26
Revision:
3:e455433c8cae
Parent:
2:a28ca7c25ed1
Child:
4:ac9e6772ddb3

File content as of revision 3:e455433c8cae:

 //エンコーダの動作確認。
 //左右モータの回転速度の計測プログラム
#include "mbed.h"
#include "CRotaryEncoder.h"
 
#define     PULSE_TO_UM     30  //エンコーダ1パルス当たりのタイヤ移動距離[um]
                                //実測で値を調整する。
#define     INTERRUPT_TIME  10000  //割りこみ周期[us]
  
Serial PC(USBTX,USBRX);
CRotaryEncoder encoder_a(D1,D0);       //モータAのエンコーダ
CRotaryEncoder encoder_b(D11,D12);     //モータBのエンコーダ
Ticker      timer;     //タイマ割込み用 
int enc_count_a=0,enc_count_b=0;    //エンコーダパルス数を格納
int distance_a=0,distance_b=0;      //タイヤ移動距離を格納[mm] 
int speed_a=0,  speed_b=0;

void timer_interrupt(){            
    enc_count_a=encoder_a.Get();   //エンコーダパルス数を取得
    enc_count_b=-encoder_b.Get();
    distance_a=(enc_count_a*PULSE_TO_UM);  //移動距離をmm単位で格納
    distance_b=(enc_count_b*PULSE_TO_UM);
    speed_a=(distance_a*1000)/INTERRUPT_TIME;//走行速度演算[mm/s]
    speed_b=(distance_b*1000)/INTERRUPT_TIME; 
    distance_a=0;
    distance_b=0;
    encoder_a.Set(0);
    encoder_b.Set(0);                   
}

int main() { 
    timer.attach_us(&timer_interrupt,INTERRUPT_TIME);//タイマ割り込みスタート     
    while(1){
        wait(1);
        PC.printf("spd_a:%d[mm/sec]   spd_b:%d[mm/sec]\r\n",speed_a,speed_b);//表示                
    }
}