![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
玉田機体の調整
Dependencies: mbed AQM0802 CRotaryEncoder TB6612FNG
Diff: main.cpp
- Revision:
- 27:9e234e087e70
- Parent:
- 26:7d4150475983
- Child:
- 28:00cb072ad251
--- a/main.cpp Sat Nov 23 07:09:32 2019 +0000 +++ b/main.cpp Mon Nov 25 03:17:25 2019 +0000 @@ -1,6 +1,6 @@ -//簡易コースを使って走行テストすること。 -//加減速走行時は1msごとの割込み処理の精度が重要になるので -//printf等の時間のかかる処理は控えるようにする。 +//LCD表示系を一新した +//1行目に走行モード、機体状態、記憶マーカ数を表示 +//2行目にDefault_Speedの値を表示する。 ////ライントレースサンプル @@ -11,28 +11,28 @@ //☆★☆★各種パラメータ調整箇所☆★☆★☆★ -#define DEFAULT_SPEED 750 //1走目の基本速度 -#define LOW_SPEED 450 //2走目の低速速度[mm/sec] -#define MEDIUM_SPEED 800 //2走目の低速速度[mm/sec] -#define HIGH_SPEED 1500 //2走目の低速速度[mm/sec] -#define DEFAULT_HIGH_SPEED 950 //3走目の低速速度[mm/sec] -#define STOP_DISTANCE 200000 //停止距離200000[um]⇒20[cm] +#define DEFAULT_SPEED 300 //1走目の基本速度 +#define LOW_SPEED 300 //2走目の低速速度[mm/sec] +#define MEDIUM_SPEED 500 //2走目の低速速度[mm/sec] +#define HIGH_SPEED 750 //2走目の低速速度[mm/sec] +#define DEFAULT_HIGH_SPEED 700 //3走目の低速速度[mm/sec] +#define STOP_DISTANCE 100000 //停止距離200000[um]⇒20[cm] #define TURN_POWER 0.6 //コースアウト時の旋回力 #define PULSE_TO_UM 28 //エンコーダ1パルス当たりのタイヤ移動距離[um] -#define INTERRUPT_TIME 3000 //割りこみ周期[us] +#define INTERRUPT_TIME 1000 //割りこみ周期[us] #define DEFAULT_GRAY 0.2f //フォトリフレクタデジタル入力の閾値 //シリアル通信でSensor_Digital値を確認し調整する。 #define MARKER_WIDTH 10000 //マーカ幅[um](ビニルテープ幅19000[um]以内) //コースの傷によってマーカ誤検知する場合は値を大きくする。 #define CROSS_JUDGE 4 //ラインセンサいくつ以上白線検知で交差点認識するか設定。 //モータ速度のゲイン関連(むやみに調整しない) -#define M_KP 0.001f //P(比例)制御成分 -#define M_KD 0.0005f //D(微分)制御成分 +#define M_KP 0.002f //P(比例)制御成分 +#define M_KD 0.001f //D(微分)制御成分 //フォトリフレクタのゲイン(外側に行くにつれ値を何倍させたいか調整する。) #define S_K1 1.0f //float演算させる値には必ずfを付ける -#define S_K2 2.4f //2倍 -#define S_K3 4.7f //4倍 +#define S_K2 2.0f //2倍 +#define S_K3 3.0f //4倍 //ラインセンサ各種制御成分 @@ -46,7 +46,7 @@ #define S_KD_HIGH 1.0f #define S_KD_MEDIUM 1.0f #define S_KD_LOW 1.0f -#define S_KD_DEFAULT_HIGH 1.0f +#define S_KD_DEFAULT_HIGH 0.7f //////////☆★☆★☆★☆★☆★////////////// @@ -77,19 +77,7 @@ AnalogIn s7(A1); AnalogIn s8(A0); -/* -AnalogIn s1(A1); -AnalogIn s2(D3); -AnalogIn s3(A6); -AnalogIn s4(A5); -AnalogIn s5(A4); -AnalogIn s6(A3); -AnalogIn s7(A2); -AnalogIn s8(A0); -*/ - /////////////////////////////////////// -//Serial PC(USBTX,USBRX); CRotaryEncoder encoder_a(D1,D0); //モータAのエンコーダ CRotaryEncoder encoder_b(D11,D12); //モータBのエンコーダ Ticker timer; //タイマ割込み用 @@ -100,7 +88,7 @@ //使用変数の定義 int Sw_Ptn = PULL; //スイッチの現在情報 int Old_Sw_Ptn = PULL; //スイッチの過去の情報 -int Sw = 0; //スイッチの押された回数 +int Sw_Cnt = 0; //スイッチの押された回数 char Coner_str[3]; //LCD用配列。コーナマーカ通過数表示 ///各ラインセンサ情報格納用 double S1_Data,S2_Data,S3_Data,S4_Data,S5_Data,S6_Data,S7_Data,S8_Data; @@ -153,9 +141,11 @@ int SG_Cnt=0; //スタート・ゴールセンサのマーカ通過数 int Cross_Flag=0; //交差点を通過したことを示すフラグ long int Imaginary_Speed=0; //曲率演算にて求めた2走目以降の仮想走行速度 -float course_data[100][3]; //コース情報の記憶用配列マーカ数は行数分まで対応し、 +long long int course_data[100][3]; //コース情報の記憶用配列マーカ数は行数分まで対応し、 //列情報は左タイヤ走行距離、右タイヤ走行距離、演算で求めた2走目走行速度を格納する。 int Row=0; //行情報を格納。マーカ通過ごとに加算し、走行終了後リセットされる。 +int Marker_Cnt=0; + void sensor_analog_read(){//ラインセンサの情報取得処理。 S1_Data=s1.read(); @@ -232,20 +222,21 @@ void corner_curvature(){ course_data[Row][0]=0;//予め配列の情報は初期化しておく。 course_data[Row][1]=0;// + Imaginary_Speed=0; ////まずはマーカ間の走行距離を取得する。 course_data[Row][0]=(Memory_Enc_Count_A*PULSE_TO_UM);//左タイヤのマーカ間の走行距離を格納 course_data[Row][1]=(Memory_Enc_Count_B*PULSE_TO_UM);//右タイヤのマーカ間の走行距離を格納 //左右タイヤの走行距離の差分を用いた曲率演算 - if(((course_data[Row][0])>(course_data[Row][1]*4.0f)) || ((course_data[Row][1])>(course_data[Row][0]*4.0f))){//左が右より80%以上早いか20%以上遅いとき - Imaginary_Speed=Low_Speed;//急カーブは低速とする。 - }else if((course_data[Row][0]>(course_data[Row][1]*2.5f)) || (course_data[Row][1]>(course_data[Row][0]*2.5f))){//左が右より50%以上早いか50%以上遅いとき + if(((course_data[Row][0])<(course_data[Row][1]*1.2f)) || ((course_data[Row][1])<(course_data[Row][0]*1.2f))){//左右の回転差が20%以下の時 + Imaginary_Speed=High_Speed;//高速とする。 + }else if((course_data[Row][0]<(course_data[Row][1]*1.5)) || (course_data[Row][1]<(course_data[Row][0]*1.5))){//左右の回転差が50%以下の時 Imaginary_Speed=Medium_Speed;//中カーブは中速とする。 - }else if((course_data[Row][0]>(course_data[Row][1]*1.2f)) || (course_data[Row][1]>(course_data[Row][0]*1.2f))){//左が右より20%以上早いか20%以上遅いとき⇒直線 - Imaginary_Speed=High_Speed;//緩いカーブは高速とする。 + }else if((course_data[Row][0]<(course_data[Row][1]*2)) || (course_data[Row][1]<(course_data[Row][0]*2))){//左右の回転差が100%以下の時 + Imaginary_Speed=Low_Speed;//低速 } else{ - Imaginary_Speed=High_Speed;//直線は高速とする。 + Imaginary_Speed=Low_Speed;//低速 } course_data[Row][2]=Imaginary_Speed; //仮想の演算速度を格納 @@ -274,6 +265,36 @@ } +/////LCD表示用関数。機体の現在状態を表示する。 +void display_print(void){ + char lcd_top_str[8],lcd_low_str[8]; //lcd表示用配列を用意 各行分 + char stop_or_go; //機体停止状態'-'か走行状態'G'を格納 + //////表示クリア + lcd.locate(0,0); + lcd.print(" "); + lcd.locate(0,1); + lcd.print(" "); + + ////////0行目の表示////////////////////////////// + if(Machine_Status&STOP){ //機体停止状態のとき + stop_or_go='-'; + }else if(!(Machine_Status&STOP)){ //機体走行状態のとき + stop_or_go='G'; + } + sprintf(lcd_top_str,"%d:%c %03d",Sw_Cnt,stop_or_go,Marker_Cnt);//0行の表示文字の整理 + lcd.locate(0,0); //表示位置を0行目にセット + lcd.print(lcd_top_str); //表示 + + ////////1行目の表示///////////////////////////// + if(Sw_Cnt<=2){ //1走目か2走目のとき + sprintf(lcd_low_str,"SPD:%04d",Default_Speed); //1行の表示文字の整理 + }else sprintf(lcd_low_str,"SPD:%04d",Default_High_Speed); //3走目のとき + lcd.locate(0,1); //表示位置を1行目にセット + lcd.print(lcd_low_str); //表示 +} + + + //タイマ割り込み1[ms]周期 void timer_interrupt(){ @@ -290,24 +311,12 @@ //エンコーダパルス数の取得 Enc_Count_A=encoder_a.Get(); Enc_Count_B=-encoder_b.Get(); - //////コース記憶に使用するエンコーダパルス数の蓄積処理 - Memory_Enc_Count_A+=Enc_Count_A; - Memory_Enc_Count_B+=Enc_Count_B; - - //////機体停止時の徐行用の走行距離演算 - if(Machine_Status&STOP){//機体停止状態の時 - if(Stop_Distance>=STOP_DISTANCE){ - Enc_A_Rotate+=Enc_Count_A;//閾値用に左エンコーダ値の蓄積 - Enc_B_Rotate+=Enc_Count_B;//速度用に右エンコーダ値の蓄積 - } - if(Enc_A_Rotate<-6400)Enc_A_Rotate=-6400; - if(Enc_A_Rotate>6400)Enc_A_Rotate=6400; - if(Enc_B_Rotate<-6400)Enc_B_Rotate=-6400; - if(Enc_B_Rotate>6400)Enc_B_Rotate=6400; - if(Stop_Distance<0)Stop_Distance=0; - if(Stop_Distance>STOP_DISTANCE)Stop_Distance=STOP_DISTANCE; - } + if(!(Machine_Status&STOP)){//機体走行中のとき + //////コース記憶に使用するエンコーダパルス数の蓄積処理 + Memory_Enc_Count_A+=Enc_Count_A; + Memory_Enc_Count_B+=Enc_Count_B; + } //各種マーカの検知 Old_Marker_Pass_Flag=Marker_Pass_Flag;//過去のフラグを退避 if(Sensor_Digital&0x81){ //マーカセンサ検知時 @@ -324,44 +333,54 @@ if(Cross_Flag==1); //交差点の時は何もしない else if((SG_Flag==1)&&(SG_Cnt==0)){ //ゴールスタートマーカの時⇒1回目 SG_Cnt=1; - if(Sw==0){ //1走目のとき - Memory_Enc_Count_A=0; - Memory_Enc_Count_B=0; - corner_curvature(); //曲率に応じた速度推定処理 + if(Sw_Cnt==1){ //1走目のとき + corner_curvature(); //曲率に応じた速度推定処理 + Memory_Enc_Count_A=0; //マーカ間の左タイヤエンコーダパルス数のクリア + Memory_Enc_Count_B=0; //マーカ間の右タイヤエンコーダパルス数のクリア Row++; - }else if(Sw==1){ //2走目のとき - ++Row;//一つ先のコース情報を読む - second_speed_control();//記憶情報から目標速度を設定する処理 + }else if(Sw_Cnt==2){ //2走目のとき + second_speed_control(); //記憶情報から目標速度を設定する処理 + Row++; //次のコース情報へ }else{//3走目は定速で高速走行 Target_Speed_A=Default_High_Speed; Target_Speed_B=Default_High_Speed; } }else if((SG_Flag==1)&&(SG_Cnt==1)){//ゴールスタートマーカの時⇒2回目 - corner_curvature(); //曲率に応じた速度推定処理 - Row++; + if(Sw_Cnt==1){ + corner_curvature(); //曲率に応じた速度推定処理 + Memory_Enc_Count_A=0; //マーカ間の左タイヤエンコーダパルス数のクリア + Memory_Enc_Count_B=0; //マーカ間の右タイヤエンコーダパルス数のクリア + }else if(Sw_Cnt==2){ //2走目のとき 何もしない + }else; //3走目のとき 何もしない + Marker_Cnt=Row; //マーカカウント数をLCD表示用に退避させる Machine_Status|=STOP; //機体停止状態へ - Row=0; //コース記憶用配列の行情報[通過マーカ情報]の初期化 - SG_Cnt=0;//スタート・ゴールマーカ情報のリセット - }else if(Corner_Flag==1){//コーナマーカの時 - if(Sw==0){ //1走目のとき - corner_curvature(); //曲率に応じた速度推定処理 + Row=0; //コース記憶用配列の行情報[通過マーカ情報]の初期化 + SG_Cnt=0; //スタート・ゴールマーカ情報のリセット + }else if(Corner_Flag==1){ //コーナマーカの時 + if(Sw_Cnt==1){ //1走目のとき + corner_curvature(); //曲率に応じた速度推定処理 + Memory_Enc_Count_A=0; //マーカ間の左タイヤエンコーダパルス数のクリア + Memory_Enc_Count_B=0; //マーカ間の右タイヤエンコーダパルス数のクリア Row++; - }else if(Sw==1){ //2走目のとき - ++Row;//一つ先のコース情報を読む - second_speed_control();//記憶情報から目標速度を設定する処理 + }else if(Sw_Cnt==2){ //2走目のとき + second_speed_control(); //記憶情報から目標速度を設定する処理 + Row++; //次のコース情報へ }else{//3走目は定速で高速走行 Target_Speed_A=Default_High_Speed; Target_Speed_B=Default_High_Speed; } } - }else{//マーカではなく、誤検知だった場合。 - //何もしない - } + }else;//マーカではなく、誤検知だった場合。何もしない + + + + //////マーカ通過後は各種マーカ判定用フラグ、パラメータのリセット処理を行う。 Corner_Flag=0; SG_Flag=0; Cross_Flag=0; - Marker_Run_Distance=0;//マーカ通過距離情報リセット - } + Marker_Run_Distance=0; //マーカ通過距離情報リセット + + }/////////////////各種マーカ判定処理の終了 //センサ取得値の重ね合わせ(端のセンサほどモータ制御量を大きくする) 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); @@ -419,13 +438,33 @@ motor_b=-Motor_B_Pwm; } }else{//停止状態の時はモータへの出力は無効 - motor_a=0; - motor_b=0; - } + if(Stop_Distance<STOP_DISTANCE){ + motor_a=-Motor_A_Pwm/4; + motor_b=-Motor_B_Pwm/4; + + }else{ + motor_a=0; + motor_b=0; + } + } + //////機体停止時の徐行用の走行距離演算 + if(Machine_Status&STOP){//機体停止状態の時 + if(Stop_Distance>=STOP_DISTANCE){ + Enc_A_Rotate+=Enc_Count_A;//閾値用に左エンコーダ値の蓄積 + Enc_B_Rotate+=Enc_Count_B;//速度用に右エンコーダ値の蓄積 + } + if(Enc_A_Rotate<-6400)Enc_A_Rotate=-6400; + if(Enc_A_Rotate>6400)Enc_A_Rotate=6400; + if(Enc_B_Rotate<-6400)Enc_B_Rotate=-6400; + if(Enc_B_Rotate>6400)Enc_B_Rotate=6400; + if((Distance_A>=0)&&(Distance_B>=0))Stop_Distance+=(Distance_A+Distance_B)/2; + if(Stop_Distance<0)Stop_Distance=0; + if(Stop_Distance>STOP_DISTANCE)Stop_Distance=STOP_DISTANCE; + } if(Marker_Pass_Flag==1){//マーカ通過中は通過距離情報を蓄積する。 Marker_Run_Distance+=(Distance_A+Distance_B)/2; - } - + } + //割込み終了時の各種パラメータリセット処理 //エンコーダ関連情報のリセット encoder_a.Set(0);//エンコーダクラスのパルス数情報のリセット @@ -440,78 +479,34 @@ int main() { timer.attach_us(&timer_interrupt,INTERRUPT_TIME);//タイマ割り込みスタート lcd.cls();//表示クリア - lcd.locate(0,0); - lcd.print("STOP"); - lcd.locate(0,1); - while(1){ Old_Sw_Ptn=Sw_Ptn; //過去のスイッチ入力情報を退避 Sw_Ptn=push_sw; //現在のスイッチ入力情報の取得 - if((!(Old_Machine_Status&STOP))&&(Machine_Status&STOP)){//走行終了時 - //sprintf(MemoryA_Str,"%d",memory_A); - //sprintf(MemoryB_Str,"%d",memory_B); - lcd.locate(0,0); - lcd.print(" "); - lcd.locate(0,0); - lcd.print("STOP"); - - //lcd.locate(0,0); - //lcd.print(MemoryA_Str); - //lcd.locate(0,1); - //lcd.print(MemoryB_Str); - wait(5); - Gray=DEFAULT_GRAY; - Medium_Speed=MEDIUM_SPEED; - Sw++; - } + display_print(); //LCDに現在の機体状態を表示 - if(Machine_Status&STOP){//機体停止状態の時 - Gray=DEFAULT_GRAY+((float)Enc_A_Rotate/16000);//センサ閾値調整 - sprintf(Gray_Str,"%3.2f",Gray);//速度情報文字列変換 - sprintf(Coner_str,"%d",Row); //マーカ通過数の表示 - lcd.locate(0,0); - lcd.print(Gray_Str); - lcd.print(" "); - lcd.print(Coner_str); - - lcd.locate(0,1); - lcd.print(" "); - - if(Sw==0){ - Default_Speed=DEFAULT_SPEED+(Enc_B_Rotate/16);//標準速度調整 - sprintf(Speed_Str,"%04d",Default_Speed);//速度情報文字列変換 - } - else if(Sw==1) {//2走目。加減速による高速走行 - Low_Speed=LOW_SPEED+(Enc_B_Rotate/16); - Default_Speed=DEFAULT_SPEED+(Enc_B_Rotate/16); - sprintf(Speed_Str,"%04d",Low_Speed);//速度情報文字列変換 - } - else if(Sw==2){//3走目。加減速による超高速走行 - Default_High_Speed=DEFAULT_HIGH_SPEED+(Enc_B_Rotate/16); - sprintf(Speed_Str,"%04d",Default_High_Speed);//速度情報文字列変換 - } - } - lcd.locate(0,1); - lcd.print(Speed_Str); + if(Machine_Status&STOP){//機体停止状態の時 + if(Sw_Cnt<=2)Default_Speed=DEFAULT_SPEED+(Enc_B_Rotate/16); //1,2走目標準速度調整 + else if(Sw_Cnt>2)Default_High_Speed=DEFAULT_HIGH_SPEED+(Enc_B_Rotate/16); //3走目 + } if((Old_Sw_Ptn==PULL)&&(Sw_Ptn==PUSH)){//スイッチが押された瞬間 if(Machine_Status&STOP){//機体停止状態の時 - lcd.locate(0,0); - lcd.print(" "); - lcd.locate(0,0); - lcd.print("GO!!"); - wait(2); - if(Sw==0){ //1走目のとき + Sw_Cnt++; + wait(0.5); + display_print(); //LCDに現在の機体状態を表示 + wait(1.5); + Corner_Flag=0; + SG_Flag=0; + Cross_Flag=0; + Row=0; //コース記憶用配列の行情報[通過マーカ情報]の初期化 + SG_Cnt=0; //スタート・ゴールマーカ情報のリセット + Marker_Run_Distance=0;//マーカ通過距離情報リセット + if(Sw_Cnt<=2){ //1,2走目のとき S_Kp=S_KP_DEFAULT; S_Kd=S_KD_DEFAULT; Target_Speed_A=Default_Speed; Target_Speed_B=Default_Speed; - }else if(Sw==1){ //2走目のとき - S_Kp=S_KP_DEFAULT; - S_Kd=S_KD_DEFAULT; - Target_Speed_A=Default_Speed; - Target_Speed_B=Default_Speed; }else{ //3走目以降のとき S_Kp=S_KP_DEFAULT_HIGH; S_Kd=S_KD_DEFAULT_HIGH; @@ -520,20 +515,16 @@ } Stop_Distance=0; Machine_Status&=0x7F;//ストップ状態解除 - - - + }else{//機体走行中であったとき //各種フラグのクリア Corner_Flag=0; SG_Flag=0; Cross_Flag=0; + Row=0; //コース記憶用配列の行情報[通過マーカ情報]の初期化 + SG_Cnt=0; //スタート・ゴールマーカ情報のリセット Marker_Run_Distance=0;//マーカ通過距離情報リセット - Machine_Status |= STOP;//機体停止状態にする。 - lcd.locate(0,0); - lcd.print(" "); - lcd.locate(0,0); - lcd.print("STOP"); + Machine_Status |= STOP;//機体停止状態にする。 } } }