main.cpp

00001 ////ライントレースサンプルver3
00002 ///モータ速度制御プログラムを追加。
00003 ///ラインセンサPD制御を追加。
00004 ///M_KP,M_KDは別途straight_speed_controlのプログラムで検証した値をセットする。
00005 ///フォトリフレクタのゲイン、ラインセンサの各種成分の調整を行い、
00006 ///ラインをきれいにトレースできるよう挑戦する。
00007 
00008 #include "mbed.h"
00009 #include "CRotaryEncoder.h"
00010 #include "TB6612.h"
00011 
00012 
00013 //☆★☆★各種パラメータ調整箇所☆★☆★☆★
00014 #define     DEFAULT_SPEED   900   //機体の直進速度1000[mm/s] 
00015 #define     PULSE_TO_UM     28  //エンコーダ1パルス当たりのタイヤ移動距離[um]
00016 #define     INTERRUPT_TIME  1000  //割りこみ周期[us]
00017 
00018 //モータ速度のゲイン関連(むやみに調整しない)
00019 #define     M_KP    0.002f//P(比例)制御成分
00020 #define     M_KD    0.001f//D(微分)制御成分
00021 
00022 //フォトリフレクタのゲイン（外側に行くにつれ値を何倍させたいか調整する。）
00023 #define S_K1    1.4f    //float演算させる値には必ずfを付ける
00024 #define S_K2    2.4f    //2倍
00025 #define S_K3    4.5f    //4倍
00026 
00027 //ラインセンサ各種制御成分
00028 #define S_KP    0.8f    //ラインセンサ比例成分。大きいほど曲がりやすい
00029 #define S_KD    0.3f    //ラインセンサ微分成分。大きいほど急なラインずれに強くなる。
00030 
00031 
00032 //////////☆★☆★☆★☆★☆★//////////////
00033 
00034 /////アナログ入力オブジェクト定義//////////
00035 AnalogIn s1(D3);
00036 AnalogIn s2(A6);
00037 AnalogIn s3(A5);
00038 AnalogIn s4(A4);
00039 AnalogIn s5(A3);
00040 AnalogIn s6(A2);
00041 AnalogIn s7(A1);
00042 AnalogIn s8(A0);
00043 ///////////////////////////////////////  
00044 Serial PC(USBTX,USBRX);
00045 CRotaryEncoder encoder_a(D1,D0);       //モータAのエンコーダ
00046 CRotaryEncoder encoder_b(D11,D12);     //モータBのエンコーダ
00047 Ticker      timer;     //タイマ割込み用 
00048 TB6612      motor_a(D2,D7,D6);  //モータA制御用(pwma,ain1,ain2)
00049 TB6612      motor_b(D10,D8,D9);  //モータB制御用(pwmb,bin1,bin2)
00050 
00051 
00052 //使用変数の定義
00053 float S1_Data,S2_Data,S3_Data,S4_Data,S5_Data,S6_Data,S7_Data,S8_Data; 
00054 float All_Sensor_Data;      //ラインセンサ総データ量
00055 float Sensor_Diff[2]={0,0}; //ラインセンサ偏差
00056 float Sensor_P=0.0f;        //ラインセンサP(比例成分)制御量
00057 float Sensor_D=0.0f;        //ラインセンサD(微分成分)制御量
00058 float Sensor_PD=0.0f;       //ラインセンサP,D成分の合計
00059 long int Enc_Count_A=0,Enc_Count_B=0;        //エンコーダパルス数を格納
00060 long int Distance_A=0,Distance_B=0;          //タイヤ移動距離を格納[mm] 
00061 long int Speed_A=0,  Speed_B=0;              //現在速度
00062 long int Target_Speed_A=0,Target_Speed_B=0;  //目標速度
00063 long int Motor_A_Diff[2]={0,0};    //過去の偏差と現在の偏差を格納
00064 long int Motor_B_Diff[2]={0,0};   
00065 float Motor_A_P,Motor_B_P;              //モータP制御成分
00066 float Motor_A_D,Motor_B_D;              //モータD制御成分
00067 float Motor_A_Pwm,Motor_B_Pwm;          //モータへの出力
00068 
00069 void timer_interrupt(){
00070     Sensor_Diff[1]=Sensor_Diff[0];//過去のラインセンサ偏差を退避
00071     //各種センサ情報取得
00072     S1_Data=s1.read();
00073     S2_Data=s2.read();
00074     S3_Data=s3.read();
00075     S4_Data=s4.read();
00076     S5_Data=s5.read();
00077     S6_Data=s6.read();
00078     S7_Data=s7.read();
00079     S8_Data=s8.read();
00080     //センサ取得値の重ね合わせ（端のセンサほどモータ制御量を大きくする）
00081     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);
00082     Sensor_Diff[0]=All_Sensor_Data;
00083     Sensor_P=All_Sensor_Data*S_KP;                  //ラインセンサ比例成分の演算       
00084     Sensor_D=(Sensor_Diff[0]-Sensor_Diff[1])*S_KD;  //ラインセンサ微分成分の演算
00085     Sensor_PD=Sensor_P+Sensor_D;       
00086     ////モータ現在速度の取得            
00087     Enc_Count_A=encoder_a.Get();   //エンコーダパルス数を取得
00088     Enc_Count_B=-encoder_b.Get();
00089     Distance_A=(Enc_Count_A*PULSE_TO_UM);  //移動距離をmm単位で格納
00090     Distance_B=(Enc_Count_B*PULSE_TO_UM);
00091     Speed_A=(Distance_A*1000)/INTERRUPT_TIME;//走行速度演算[mm/s]
00092     Speed_B=(Distance_B*1000)/INTERRUPT_TIME; 
00093     Distance_A=0;
00094     Distance_B=0;
00095     encoder_a.Set(0);
00096     encoder_b.Set(0);
00097     
00098     /////各モータの目標速度の設定
00099     Target_Speed_A=DEFAULT_SPEED;
00100     Target_Speed_B=DEFAULT_SPEED;
00101     
00102     /////モータの速度制御
00103     //過去の速度偏差を退避
00104     Motor_A_Diff[1]=Motor_A_Diff[0];
00105     Motor_B_Diff[1]=Motor_B_Diff[0];
00106     //現在の速度偏差を取得。    
00107     Motor_A_Diff[0]=(Target_Speed_A-Speed_A);
00108     Motor_B_Diff[0]=(Target_Speed_B-Speed_B);
00109     //P成分演算
00110     Motor_A_P=Motor_A_Diff[0]*M_KP;
00111     Motor_B_P=Motor_B_Diff[0]*M_KP;
00112     //D成分演算
00113     Motor_A_D=(Motor_A_Diff[0]-Motor_A_Diff[1])*M_KD;
00114     Motor_B_D=(Motor_B_Diff[0]-Motor_B_Diff[1])*M_KD;
00115     
00116     Motor_A_Pwm=Motor_A_P+Motor_A_D+Sensor_PD;
00117     Motor_B_Pwm=Motor_B_P+Motor_B_D-Sensor_PD;
00118     if(Motor_A_Pwm>1.0f)Motor_A_Pwm=1.0f;
00119     else if(Motor_A_Pwm<-1.0f)Motor_A_Pwm=-1.0f;
00120     if(Motor_B_Pwm>1.0f)Motor_B_Pwm=1.0f;
00121     else if(Motor_B_Pwm<-1.0f)Motor_B_Pwm=-1.0f;    
00122     //最終的には符号を逆転して出力
00123     motor_a=-Motor_A_Pwm;
00124     motor_b=-Motor_B_Pwm;                       
00125 }
00126 
00127 int main() { 
00128     timer.attach_us(&timer_interrupt,INTERRUPT_TIME);//タイマ割り込みスタート     
00129     while(1){
00130         wait(1);
00131         PC.printf("spd_a:%d[mm/sec]   spd_b:%d[mm/sec]\r\n",Speed_A,Speed_B);//表示                
00132     }
00133 }