![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
P,I,Dを変数に格納する
Dependencies: mbed AQM0802 CRotaryEncoder TB6612FNG
Diff: main.cpp
- Revision:
- 6:afd8f0d02c8d
- Parent:
- 5:f635f1f01d2d
- Child:
- 7:cfbf8d4a4d36
--- a/main.cpp Mon Aug 26 09:25:22 2019 +0000 +++ b/main.cpp Tue Aug 27 05:37:18 2019 +0000 @@ -11,42 +11,52 @@ //☆★☆★各種パラメータ調整箇所☆★☆★☆★ -#define DEFAULT_SPEED 500 //機体の直進速度1000[mm/s] -#define PULSE_TO_UM 30 //エンコーダ1パルス当たりのタイヤ移動距離[um] -#define INTERRUPT_TIME 1000 //割りこみ周期[us] - +#define DEFAULT_SPEED 900 //機体の直進速度1000[mm/s] +#define TURN_POWER 0.5 //コースアウト時の旋回力 +#define PULSE_TO_UM 30 //エンコーダ1パルス当たりのタイヤ移動距離[um] +#define INTERRUPT_TIME 1000 //割りこみ周期[us] +#define GRAY 0.2f //フォトリフレクタデジタル入力の閾値 //モータ速度のゲイン関連(むやみに調整しない) -#define M_KP 0.002f//P(比例)制御成分 -#define M_KD 0.001f//D(微分)制御成分 +#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_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 S_KP 0.5f //ラインセンサ比例成分。大きいほど曲がりやすい +#define S_KD 0.3f //ラインセンサ微分成分。大きいほど急なラインずれに強くなる。 //////////☆★☆★☆★☆★☆★////////////// +//機体状態の定義 +#define STOP 0x80 //機体停止状態 +#define RUN_START 0x40 //スタートマーカ通過 +#define RUN_COURSE_LOUT 0x20 //左コースアウト状態 +#define RUN_COURSE_CENTER 0x18 //ライン中央走行状態 +#define RUN_COURSE_ROUT 0x04 //右コースアウト状態 +#define SECOND_RUN 0x02 //機体停止状態 +#define TUARD_RUN 0x01 //機体設定モード + /////アナログ入力オブジェクト定義////////// -AnalogIn s1(D3); -AnalogIn s2(A6); -AnalogIn s3(A5); -AnalogIn s4(A4); -AnalogIn s5(A3); -AnalogIn s6(A2); -AnalogIn s7(A1); -AnalogIn s8(A0); +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のエンコーダ -Ticker timer; //タイマ割込み用 +Serial PC(USBTX,USBRX); +CRotaryEncoder encoder_a(D1,D0); //モータAのエンコーダ +CRotaryEncoder encoder_b(D11,D12); //モータBのエンコーダ +Ticker timer; //タイマ割込み用 TB6612 motor_a(D2,D7,D6); //モータA制御用(pwma,ain1,ain2) -TB6612 motor_b(D10,D8,D9); //モータB制御用(pwmb,bin1,bin2) +TB6612 motor_b(D10,D8,D9); //モータB制御用(pwmb,bin1,bin2) //使用変数の定義 @@ -65,10 +75,13 @@ float Motor_A_P,Motor_B_P; //モータP制御成分 float Motor_A_D,Motor_B_D; //モータD制御成分 float Motor_A_Pwm,Motor_B_Pwm; //モータへの出力 +unsigned char Sensor_Digital =0x00; +unsigned char Old_Sensor_Digital=0x00; +unsigned char Machine_Status =0x00; //機体状態 +unsigned char Old_Machine_Status=0x00; //過去の機体状態 -void timer_interrupt(){ - Sensor_Diff[1]=Sensor_Diff[0];//過去のラインセンサ偏差を退避 - //各種センサ情報取得 + +void sensor_analog_read(){ S1_Data=s1.read(); S2_Data=s2.read(); S3_Data=s3.read(); @@ -76,13 +89,62 @@ S5_Data=s5.read(); S6_Data=s6.read(); S7_Data=s7.read(); - S8_Data=s8.read(); + S8_Data=s8.read(); +} +void sensor_digital_read(){//8つのフォトリフレクタの入力を8ビットのデジタルパターンに変換 + Old_Sensor_Digital=Sensor_Digital; + if(S1_Data>GRAY)Sensor_Digital |= 0x80; //7ビット目のみセット (1にする。) + else Sensor_Digital &= 0x7F; //7ビット目のみマスク(0にする。) + if(S2_Data>GRAY)Sensor_Digital |= 0x40; //6ビット目のみセット (1にする。) + else Sensor_Digital &= 0xBF; //6ビット目のみマスク(0にする。) + if(S3_Data>GRAY)Sensor_Digital |= 0x20; //5ビット目のみセット (1にする。) + else Sensor_Digital &= 0xDF; //5ビット目のみマスク(0にする。) + if(S4_Data>GRAY)Sensor_Digital |= 0x10; //4ビット目のみセット (1にする。) + else Sensor_Digital &= 0xEF; //4ビット目のみマスク(0にする。) + if(S5_Data>GRAY)Sensor_Digital |= 0x08; //3ビット目のみセット (1にする。) + else Sensor_Digital &= 0xF7; //3ビット目のみマスク(0にする。) + if(S6_Data>GRAY)Sensor_Digital |= 0x04; //2ビット目のみセット (1にする。) + else Sensor_Digital &= 0xFB; //2ビット目のみマスク(0にする。) + if(S7_Data>GRAY)Sensor_Digital |= 0x02; //1ビット目のみセット (1にする。) + else Sensor_Digital &= 0xFD; //1ビット目のみマスク(0にする。) + if(S8_Data>GRAY)Sensor_Digital |= 0x01; //0ビット目のみセット (1にする。) + else Sensor_Digital &= 0xFE; //0ビット目のみマスク(0にする。) +} + +void Machine_Status_Set(){ + Old_Machine_Status=Machine_Status; + //機体がライン中央に位置するとき + if(Sensor_Digital&RUN_COURSE_CENTER )Machine_Status|=RUN_COURSE_CENTER; + else Machine_Status &= 0xE7;//ライン中央情報のマスク + if((Sensor_Digital==0x00)&&(Old_Sensor_Digital==0x40)){//左センサコースアウト時 + Machine_Status|=RUN_COURSE_LOUT;//左コースアウト状態のビットをセット + }else if((Machine_Status&RUN_COURSE_LOUT)&&(Sensor_Digital&RUN_COURSE_CENTER)){ + //左コースアウト状態かつ機体がライン中央に復帰したとき + Machine_Status &= 0xDF;//左コースアウト情報のみマスク + } + if((Sensor_Digital==0x00)&&(Old_Sensor_Digital==0x02)){//右センサコースアウト時 + Machine_Status|=RUN_COURSE_ROUT;//右コースアウト状態のビットをセット + }else if((Machine_Status&RUN_COURSE_ROUT)&&(Sensor_Digital&RUN_COURSE_CENTER)){ + //右コースアウト状態かつ機体がライン中央に復帰したとき + Machine_Status &= 0xFB;//右コースアウト情報のみマスク + } +} + +void timer_interrupt(){ + //ラインセンサ情報取得 + sensor_analog_read(); + sensor_digital_read(); + Machine_Status_Set(); + //センサ取得値の重ね合わせ(端のセンサほどモータ制御量を大きくする) 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[1]=Sensor_Diff[0];//過去のラインセンサ偏差を退避 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; + Sensor_PD=Sensor_P+Sensor_D; + + ////モータ現在速度の取得 Enc_Count_A=encoder_a.Get(); //エンコーダパルス数を取得 Enc_Count_B=-encoder_b.Get(); @@ -102,7 +164,7 @@ /////モータの速度制御 //過去の速度偏差を退避 Motor_A_Diff[1]=Motor_A_Diff[0]; - Motor_A_Diff[1]=Motor_A_Diff[0]; + Motor_B_Diff[1]=Motor_B_Diff[0]; //現在の速度偏差を取得。 Motor_A_Diff[0]=(Target_Speed_A-Speed_A); Motor_B_Diff[0]=(Target_Speed_B-Speed_B); @@ -115,19 +177,36 @@ 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; + if(Motor_A_Pwm>0.95f)Motor_A_Pwm=0.95f; + else if(Motor_A_Pwm<-0.95)Motor_A_Pwm=-0.95f; + if(Motor_B_Pwm>0.95f)Motor_B_Pwm=0.95f; + else if(Motor_B_Pwm<-0.95f)Motor_B_Pwm=-0.95f; + + //モータへの出力 + + if(!(Machine_Status&STOP)){//マシンが停止状態でなければ + if(Machine_Status&RUN_COURSE_LOUT){ + motor_a=-(-TURN_POWER); + motor_b=-(TURN_POWER); + }else if(Machine_Status&RUN_COURSE_ROUT){ + motor_a=-(TURN_POWER); + motor_b=-(-TURN_POWER); + }else{ + motor_a=-Motor_A_Pwm; + motor_b=-Motor_B_Pwm; + } + }else{//停止状態の時はモータへの出力は無効 + motor_a=0; + motor_b=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);//表示 + PC.printf("Sensor_Digital:0x%02x\t Old_Sensor_Digital:0x%02x\t Machine_Status:0x%02x\t Old_Machine_Status:0x%02x\r\n"\ + ,Sensor_Digital,Old_Sensor_Digital\ + ,Machine_Status,Old_Machine_Status);//表示 } } \ No newline at end of file