![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
精密な速度制御をおこなうためのモータPD調整用プログラム。 シリアル通信にて現在速度の表示を行い。M_KP(モータ比例成分)とM_KD(モータ微分成分)の値調整を行う。
Dependencies: mbed CRotaryEncoder TB6612FNG
Diff: main.cpp
- Revision:
- 3:e455433c8cae
- Parent:
- 2:a28ca7c25ed1
- Child:
- 4:ac9e6772ddb3
diff -r a28ca7c25ed1 -r e455433c8cae main.cpp --- a/main.cpp Mon Aug 26 04:57:20 2019 +0000 +++ b/main.cpp Mon Aug 26 06:31:19 2019 +0000 @@ -1,27 +1,37 @@ //エンコーダの動作確認。 - //左右モータの回転数を検出するプログラム - #include "mbed.h" - #include "CRotaryEncoder.h" - - #define PULSE_TO_UM 10 //エンコーダ1パルス当たりのタイヤ移動距離[um] - //実測で値を調整する。 - - - Serial PC(USBTX,USBRX); - CRotaryEncoder encoder_a(D1,D0); //モータAのエンコーダ - CRotaryEncoder encoder_b(D11,D12); //モータBのエンコーダ - + //左右モータの回転速度の計測プログラム +#include "mbed.h" +#include "CRotaryEncoder.h" - int main(){ - int enc_count_a=0,enc_count_b=0; //エンコーダパルス数を格納 - int distance_a=0,distance_b=0; //タイヤ移動距離を格納[mm] - while(1) - { - enc_count_a=encoder_a.Get(); //エンコーダパルス数を取得 - enc_count_b=-encoder_b.Get(); - distance_a=(enc_count_a*PULSE_TO_UM)/1000; //移動距離をmm単位で格納 - distance_b=(enc_count_b*PULSE_TO_UM)/1000; - PC.printf("dis_a:%d dis_b:%d\r\n",distance_a,distance_b);//表示 - wait(0.5); +#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);//表示 } - } +} \ No newline at end of file