![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
P,I,Dを変数に格納する
Dependencies: mbed AQM0802 CRotaryEncoder TB6612FNG
Diff: main.cpp
- Revision:
- 5:f635f1f01d2d
- Parent:
- 4:ac9e6772ddb3
- Child:
- 6:afd8f0d02c8d
--- a/main.cpp Mon Aug 26 07:47:00 2019 +0000 +++ b/main.cpp Mon Aug 26 09:25:22 2019 +0000 @@ -1,27 +1,46 @@ -////モータPID調整用プログラム -////精密な速度制御を可能とするために -////M_KP,M_KDの調整を行う。 -////DEFAULT_SPEEDに近い速度へ安定して出力 -////できることを確認する。 +////ライントレースサンプルver3 +///モータ速度制御プログラムを追加。 +///ラインセンサPD制御を追加。 +///M_KP,M_KDは別途straight_speed_controlのプログラムで検証した値をセットする。 +///フォトリフレクタのゲイン、ラインセンサの各種成分の調整を行い、 +///ラインをきれいにトレースできるよう挑戦する。 + #include "mbed.h" #include "CRotaryEncoder.h" #include "TB6612.h" //☆★☆★各種パラメータ調整箇所☆★☆★☆★ -#define DEFAULT_SPEED 1000 //機体の直進速度1000[mm/s] - +#define DEFAULT_SPEED 500 //機体の直進速度1000[mm/s] #define PULSE_TO_UM 30 //エンコーダ1パルス当たりのタイヤ移動距離[um] - //実測で値を調整する。 #define INTERRUPT_TIME 1000 //割りこみ周期[us] +//モータ速度のゲイン関連(むやみに調整しない) +#define M_KP 0.002f//P(比例)制御成分 +#define M_KD 0.001f//D(微分)制御成分 + +//フォトリフレクタのゲイン(外側に行くにつれ値を何倍させたいか調整する。) +#define S_K1 1.0f //float演算させる値には必ずfを付ける +#define S_K2 2.0f //2倍 +#define S_K3 4.0f //4倍 + +//ラインセンサ各種制御成分 +#define S_KP 0.5f //ラインセンサ比例成分。大きいほど曲がりやすい +#define S_KD 0.3f //ラインセンサ微分成分。大きいほど急なラインずれに強くなる。 + -//モータ速度のゲイン関連 -#define M_KP 0.002f//P(比例)制御成分 -#define M_KD 0.001f//D(微分)制御成分 //////////☆★☆★☆★☆★☆★////////////// - +/////アナログ入力オブジェクト定義////////// +AnalogIn s1(D3); +AnalogIn s2(A6); +AnalogIn s3(A5); +AnalogIn s4(A4); +AnalogIn s5(A3); +AnalogIn s6(A2); +AnalogIn s7(A1); +AnalogIn s8(A0); +/////////////////////////////////////// Serial PC(USBTX,USBRX); CRotaryEncoder encoder_a(D1,D0); //モータAのエンコーダ CRotaryEncoder encoder_b(D11,D12); //モータBのエンコーダ @@ -30,18 +49,40 @@ 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 S1_Data,S2_Data,S3_Data,S4_Data,S5_Data,S6_Data,S7_Data,S8_Data; +float All_Sensor_Data; //ラインセンサ総データ量 +float Sensor_Diff[2]={0,0}; //ラインセンサ偏差 +float Sensor_P=0.0f; //ラインセンサP(比例成分)制御量 +float Sensor_D=0.0f; //ラインセンサD(微分成分)制御量 +float Sensor_PD=0.0f; //ラインセンサP,D成分の合計 +long int Enc_Count_A=0,Enc_Count_B=0; //エンコーダパルス数を格納 +long int Distance_A=0,Distance_B=0; //タイヤ移動距離を格納[mm] +long int Speed_A=0, Speed_B=0; //現在速度 +long int Target_Speed_A=0,Target_Speed_B=0; //目標速度 +long int Motor_A_Diff[2]={0,0}; //過去の偏差と現在の偏差を格納 +long 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(){ - + Sensor_Diff[1]=Sensor_Diff[0];//過去のラインセンサ偏差を退避 + //各種センサ情報取得 + S1_Data=s1.read(); + S2_Data=s2.read(); + S3_Data=s3.read(); + S4_Data=s4.read(); + S5_Data=s5.read(); + S6_Data=s6.read(); + S7_Data=s7.read(); + S8_Data=s8.read(); + //センサ取得値の重ね合わせ(端のセンサほどモータ制御量を大きくする) + All_Sensor_Data=-(S2_Data*S_K3+S3_Data*S_K2+S4_Data*S_K1)+(S5_Data*S_K1+S6_Data*S_K2+S7_Data*S_K3); + Sensor_Diff[0]=All_Sensor_Data; + Sensor_P=All_Sensor_Data*S_KP; //ラインセンサ比例成分の演算 + Sensor_D=(Sensor_Diff[0]-Sensor_Diff[1])*S_KD; //ラインセンサ微分成分の演算 + Sensor_PD=Sensor_P+Sensor_D; ////モータ現在速度の取得 Enc_Count_A=encoder_a.Get(); //エンコーダパルス数を取得 Enc_Count_B=-encoder_b.Get(); @@ -72,8 +113,12 @@ 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_Pwm=Motor_A_P+Motor_A_D+Sensor_PD; + Motor_B_Pwm=Motor_B_P+Motor_B_D-Sensor_PD; + if(Motor_A_Pwm>1.0f)Motor_A_Pwm=1.0f; + else if(Motor_A_Pwm<-1.0f)Motor_A_Pwm=-1.0f; + if(Motor_B_Pwm>1.0f)Motor_B_Pwm=1.0f; + else if(Motor_B_Pwm<-1.0f)Motor_B_Pwm=-1.0f; //最終的には符号を逆転して出力 motor_a=-Motor_A_Pwm; motor_b=-Motor_B_Pwm;