![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
精密な速度制御をおこなうためのモータPD調整用プログラム。 シリアル通信にて現在速度の表示を行い。M_KP(モータ比例成分)とM_KD(モータ微分成分)の値調整を行う。
Dependencies: mbed CRotaryEncoder TB6612FNG
Revision 4:ac9e6772ddb3, committed 2019-08-26
- Comitter:
- yusaku0125
- Date:
- Mon Aug 26 07:47:00 2019 +0000
- Parent:
- 3:e455433c8cae
- Commit message:
- test; ;
Changed in this revision
TB6612FNG.lib | Show annotated file Show diff for this revision Revisions of this file |
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/TB6612FNG.lib Mon Aug 26 07:47:00 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/Robotrace_shimane/code/TB6612FNG/#80ac3e662fe4
--- a/main.cpp Mon Aug 26 06:31:19 2019 +0000 +++ b/main.cpp Mon Aug 26 07:47:00 2019 +0000 @@ -1,37 +1,88 @@ - //エンコーダの動作確認。 - //左右モータの回転速度の計測プログラム +////モータPID調整用プログラム +////精密な速度制御を可能とするために +////M_KP,M_KDの調整を行う。 +////DEFAULT_SPEEDに近い速度へ安定して出力 +////できることを確認する。 #include "mbed.h" #include "CRotaryEncoder.h" +#include "TB6612.h" + + +//☆★☆★各種パラメータ調整箇所☆★☆★☆★ +#define DEFAULT_SPEED 1000 //機体の直進速度1000[mm/s] #define PULSE_TO_UM 30 //エンコーダ1パルス当たりのタイヤ移動距離[um] //実測で値を調整する。 -#define INTERRUPT_TIME 10000 //割りこみ周期[us] +#define INTERRUPT_TIME 1000 //割りこみ周期[us] + + +//モータ速度のゲイン関連 +#define M_KP 0.002f//P(比例)制御成分 +#define M_KD 0.001f//D(微分)制御成分 +//////////☆★☆★☆★☆★☆★////////////// + 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; +TB6612 motor_a(D2,D7,D6); //モータA制御用(pwma,ain1,ain2) +TB6612 motor_b(D10,D8,D9); //モータB制御用(pwmb,bin1,bin2) + + +int Enc_Count_A=0,Enc_Count_B=0; //エンコーダパルス数を格納 +int Distance_A=0,Distance_B=0; //タイヤ移動距離を格納[mm] +int Speed_A=0, Speed_B=0; //現在速度 +int Target_Speed_A=0,Target_Speed_B=0; //目標速度 +int Motor_A_Diff[2]={0,0}; //過去の偏差と現在の偏差を格納 +int Motor_B_Diff[2]={0,0}; +float Motor_A_P,Motor_B_P; //モータP制御成分 +float Motor_A_D,Motor_B_D; //モータD制御成分 +float Motor_A_Pwm,Motor_B_Pwm; //モータへの出力 -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; +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); + encoder_b.Set(0); + + /////各モータの目標速度の設定 + Target_Speed_A=DEFAULT_SPEED; + Target_Speed_B=DEFAULT_SPEED; + + /////モータの速度制御 + //過去の速度偏差を退避 + Motor_A_Diff[1]=Motor_A_Diff[0]; + Motor_A_Diff[1]=Motor_A_Diff[0]; + //現在の速度偏差を取得。 + Motor_A_Diff[0]=(Target_Speed_A-Speed_A); + Motor_B_Diff[0]=(Target_Speed_B-Speed_B); + //P成分演算 + Motor_A_P=Motor_A_Diff[0]*M_KP; + Motor_B_P=Motor_B_Diff[0]*M_KP; + //D成分演算 + Motor_A_D=(Motor_A_Diff[0]-Motor_A_Diff[1])*M_KD; + Motor_B_D=(Motor_B_Diff[0]-Motor_B_Diff[1])*M_KD; + + Motor_A_Pwm=Motor_A_P+Motor_A_D; + Motor_B_Pwm=Motor_B_P+Motor_B_D; + //最終的には符号を逆転して出力 + motor_a=-Motor_A_Pwm; + motor_b=-Motor_B_Pwm; } 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);//表示 + PC.printf("spd_a:%d[mm/sec] spd_b:%d[mm/sec]\r\n",Speed_A,Speed_B);//表示 } } \ No newline at end of file