島根短大　ライントレーサ制作チーム / Mbed 2 deprecated line_trace_sample_ver7_2_count

マーカーで読み取りLCDを使用してカウントした値を表示する

Dependencies:   mbed AQM0802 CRotaryEncoder TB6612FNG

main.cpp@4:ac9e6772ddb3, 2019-08-26 (annotated)

Committer:
yusaku0125
Date:
Mon Aug 26 07:47:00 2019 +0000
Revision:
4:ac9e6772ddb3
Parent:
3:e455433c8cae
Child:
5:f635f1f01d2d
test; ;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yusaku0125 4:ac9e6772ddb3 1 ////モータPID調整用プログラム
yusaku0125 4:ac9e6772ddb3 2 ////精密な速度制御を可能とするために
yusaku0125 4:ac9e6772ddb3 3 ////M_KP,M_KDの調整を行う。
yusaku0125 4:ac9e6772ddb3 4 ////DEFAULT_SPEEDに近い速度へ安定して出力
yusaku0125 4:ac9e6772ddb3 5 ////できることを確認する。
yusaku0125 3:e455433c8cae 6 #include "mbed.h"
yusaku0125 3:e455433c8cae 7 #include "CRotaryEncoder.h"
yusaku0125 4:ac9e6772ddb3 8 #include "TB6612.h"
yusaku0125 4:ac9e6772ddb3 9
yusaku0125 4:ac9e6772ddb3 10
yusaku0125 4:ac9e6772ddb3 11 //☆★☆★各種パラメータ調整箇所☆★☆★☆★
yusaku0125 4:ac9e6772ddb3 12 #define DEFAULT_SPEED 1000 //機体の直進速度1000[mm/s]
yusaku0125 0:df99e50ed3fd 13
yusaku0125 3:e455433c8cae 14 #define PULSE_TO_UM 30 //エンコーダ1パルス当たりのタイヤ移動距離[um]
yusaku0125 3:e455433c8cae 15 //実測で値を調整する。
yusaku0125 4:ac9e6772ddb3 16 #define INTERRUPT_TIME 1000 //割りこみ周期[us]
yusaku0125 4:ac9e6772ddb3 17
yusaku0125 4:ac9e6772ddb3 18
yusaku0125 4:ac9e6772ddb3 19 //モータ速度のゲイン関連
yusaku0125 4:ac9e6772ddb3 20 #define M_KP 0.002f//P(比例)制御成分
yusaku0125 4:ac9e6772ddb3 21 #define M_KD 0.001f//D(微分)制御成分
yusaku0125 4:ac9e6772ddb3 22 //////////☆★☆★☆★☆★☆★//////////////
yusaku0125 4:ac9e6772ddb3 23
yusaku0125 3:e455433c8cae 24
yusaku0125 3:e455433c8cae 25 Serial PC(USBTX,USBRX);
yusaku0125 3:e455433c8cae 26 CRotaryEncoder encoder_a(D1,D0); //モータAのエンコーダ
yusaku0125 3:e455433c8cae 27 CRotaryEncoder encoder_b(D11,D12); //モータBのエンコーダ
yusaku0125 3:e455433c8cae 28 Ticker timer; //タイマ割込み用
yusaku0125 4:ac9e6772ddb3 29 TB6612 motor_a(D2,D7,D6); //モータA制御用(pwma,ain1,ain2)
yusaku0125 4:ac9e6772ddb3 30 TB6612 motor_b(D10,D8,D9); //モータB制御用(pwmb,bin1,bin2)
yusaku0125 4:ac9e6772ddb3 31
yusaku0125 4:ac9e6772ddb3 32
yusaku0125 4:ac9e6772ddb3 33 int Enc_Count_A=0,Enc_Count_B=0; //エンコーダパルス数を格納
yusaku0125 4:ac9e6772ddb3 34 int Distance_A=0,Distance_B=0; //タイヤ移動距離を格納[mm]
yusaku0125 4:ac9e6772ddb3 35 int Speed_A=0, Speed_B=0; //現在速度
yusaku0125 4:ac9e6772ddb3 36 int Target_Speed_A=0,Target_Speed_B=0; //目標速度
yusaku0125 4:ac9e6772ddb3 37 int Motor_A_Diff[2]={0,0}; //過去の偏差と現在の偏差を格納
yusaku0125 4:ac9e6772ddb3 38 int Motor_B_Diff[2]={0,0};
yusaku0125 4:ac9e6772ddb3 39 float Motor_A_P,Motor_B_P; //モータP制御成分
yusaku0125 4:ac9e6772ddb3 40 float Motor_A_D,Motor_B_D; //モータD制御成分
yusaku0125 4:ac9e6772ddb3 41 float Motor_A_Pwm,Motor_B_Pwm; //モータへの出力
yusaku0125 3:e455433c8cae 42
yusaku0125 4:ac9e6772ddb3 43 void timer_interrupt(){
yusaku0125 4:ac9e6772ddb3 44
yusaku0125 4:ac9e6772ddb3 45 ////モータ現在速度の取得
yusaku0125 4:ac9e6772ddb3 46 Enc_Count_A=encoder_a.Get(); //エンコーダパルス数を取得
yusaku0125 4:ac9e6772ddb3 47 Enc_Count_B=-encoder_b.Get();
yusaku0125 4:ac9e6772ddb3 48 Distance_A=(Enc_Count_A*PULSE_TO_UM); //移動距離をmm単位で格納
yusaku0125 4:ac9e6772ddb3 49 Distance_B=(Enc_Count_B*PULSE_TO_UM);
yusaku0125 4:ac9e6772ddb3 50 Speed_A=(Distance_A*1000)/INTERRUPT_TIME;//走行速度演算[mm/s]
yusaku0125 4:ac9e6772ddb3 51 Speed_B=(Distance_B*1000)/INTERRUPT_TIME;
yusaku0125 4:ac9e6772ddb3 52 Distance_A=0;
yusaku0125 4:ac9e6772ddb3 53 Distance_B=0;
yusaku0125 3:e455433c8cae 54 encoder_a.Set(0);
yusaku0125 4:ac9e6772ddb3 55 encoder_b.Set(0);
yusaku0125 4:ac9e6772ddb3 56
yusaku0125 4:ac9e6772ddb3 57 /////各モータの目標速度の設定
yusaku0125 4:ac9e6772ddb3 58 Target_Speed_A=DEFAULT_SPEED;
yusaku0125 4:ac9e6772ddb3 59 Target_Speed_B=DEFAULT_SPEED;
yusaku0125 4:ac9e6772ddb3 60
yusaku0125 4:ac9e6772ddb3 61 /////モータの速度制御
yusaku0125 4:ac9e6772ddb3 62 //過去の速度偏差を退避
yusaku0125 4:ac9e6772ddb3 63 Motor_A_Diff[1]=Motor_A_Diff[0];
yusaku0125 4:ac9e6772ddb3 64 Motor_A_Diff[1]=Motor_A_Diff[0];
yusaku0125 4:ac9e6772ddb3 65 //現在の速度偏差を取得。
yusaku0125 4:ac9e6772ddb3 66 Motor_A_Diff[0]=(Target_Speed_A-Speed_A);
yusaku0125 4:ac9e6772ddb3 67 Motor_B_Diff[0]=(Target_Speed_B-Speed_B);
yusaku0125 4:ac9e6772ddb3 68 //P成分演算
yusaku0125 4:ac9e6772ddb3 69 Motor_A_P=Motor_A_Diff[0]*M_KP;
yusaku0125 4:ac9e6772ddb3 70 Motor_B_P=Motor_B_Diff[0]*M_KP;
yusaku0125 4:ac9e6772ddb3 71 //D成分演算
yusaku0125 4:ac9e6772ddb3 72 Motor_A_D=(Motor_A_Diff[0]-Motor_A_Diff[1])*M_KD;
yusaku0125 4:ac9e6772ddb3 73 Motor_B_D=(Motor_B_Diff[0]-Motor_B_Diff[1])*M_KD;
yusaku0125 4:ac9e6772ddb3 74
yusaku0125 4:ac9e6772ddb3 75 Motor_A_Pwm=Motor_A_P+Motor_A_D;
yusaku0125 4:ac9e6772ddb3 76 Motor_B_Pwm=Motor_B_P+Motor_B_D;
yusaku0125 4:ac9e6772ddb3 77 //最終的には符号を逆転して出力
yusaku0125 4:ac9e6772ddb3 78 motor_a=-Motor_A_Pwm;
yusaku0125 4:ac9e6772ddb3 79 motor_b=-Motor_B_Pwm;
yusaku0125 3:e455433c8cae 80 }
yusaku0125 3:e455433c8cae 81
yusaku0125 3:e455433c8cae 82 int main() {
yusaku0125 3:e455433c8cae 83 timer.attach_us(&timer_interrupt,INTERRUPT_TIME);//タイマ割り込みスタート
yusaku0125 3:e455433c8cae 84 while(1){
yusaku0125 3:e455433c8cae 85 wait(1);
yusaku0125 4:ac9e6772ddb3 86 PC.printf("spd_a:%d[mm/sec] spd_b:%d[mm/sec]\r\n",Speed_A,Speed_B);//表示
yusaku0125 0:df99e50ed3fd 87 }
yusaku0125 3:e455433c8cae 88 }