![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
加減速走行時の次マーカ検出前のブレーキングシステムを導入。各速度域におけるブレーキのタイミングとPDパラメータの調整を入念に行うこと。また、LCDの表示事項はストップゴールの表示は必要性を感じなかったため、走行交差点数のパラメータ表示に変更した。
Dependencies: mbed AQM0802 CRotaryEncoder TB6612FNG
Diff: main.cpp
- Revision:
- 16:017874772ea7
- Parent:
- 15:cfe79ebfcb25
- Child:
- 17:b29e2c88b3c5
diff -r cfe79ebfcb25 -r 017874772ea7 main.cpp --- a/main.cpp Fri Nov 15 01:32:08 2019 +0000 +++ b/main.cpp Mon Nov 18 04:31:02 2019 +0000 @@ -1,4 +1,4 @@ -////ライントレースサンプルver7_1 +////ライントレースサンプル #include "mbed.h" #include "CRotaryEncoder.h" #include "TB6612.h" @@ -6,11 +6,13 @@ //☆★☆★各種パラメータ調整箇所☆★☆★☆★ -#define DEFAULT_SPEED 800 //1走目の基本速度[mm/sec] -#define DEFAULT_SPEED1 900 +#define DEFAULT_SPEED 300 //1走目の基本速度[mm/sec] +#define DEFAULT_SPEED1 600 +#define DEFAULT_SPEED2 900 +#define STOP_DISTANCE 200000 //停止距離200000[um]⇒20[cm] #define TURN_POWER 0.6 //コースアウト時の旋回力 #define PULSE_TO_UM 28 //エンコーダ1パルス当たりのタイヤ移動距離[um] -#define INTERRUPT_TIME 1000 //割りこみ周期[us] +#define INTERRUPT_TIME 3000 //割りこみ周期[us] #define DEFAULT_GRAY 0.2f //フォトリフレクタデジタル入力の閾値 //シリアル通信でSensor_Digital値を確認し調整する。 #define MARKER_WIDTH 10000 //マーカ幅[um](ビニルテープ幅19000[um]以内) @@ -56,6 +58,17 @@ AnalogIn s6(A2); 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のエンコーダ @@ -69,7 +82,7 @@ int Sw_Ptn; int Old_Sw_Ptn; int Sw=0; -int Coner_c=0; +int Coner_c=0; //カウントを格納 char Coner_str[3]; double S1_Data,S2_Data,S3_Data,S4_Data,S5_Data,S6_Data,S7_Data,S8_Data; double All_Sensor_Data; //ラインセンサ総データ量 @@ -81,18 +94,21 @@ float Gray=DEFAULT_GRAY; long int Enc_Count_A=0,Enc_Count_B=0; //エンコーダパルス数を格納 long int Enc_A_Rotate=0,Enc_B_Rotate=0; +long int Stop_Distance=STOP_DISTANCE; -long int memory_A=0; -long int memory_B=0; -char MemoryA_Str[5]; +long int memory_A=0; //移動距離格納 +long int memory_B=0; +char MemoryA_Str[5]; //LCD閾値表示用文字列 char MemoryB_Str[5]; long int Distance_A=0,Distance_B=0; //タイヤ移動距離を格納[mm] long int Distance_memory_A=0, Distance_memory_B=0; long int Marker_Run_Distance=0; long int Speed_A=0, Speed_B=0; //現在速度 -long int Machine_Speed=DEFAULT_SPEED; -long int Machine_Speed1=DEFAULT_SPEED1; + +long int Low_Speed=DEFAULT_SPEED; +long int Medium_Speed=DEFAULT_SPEED1; +long int High_Speed=DEFAULT_SPEED2; char Speed_Str[5]; //LCD速度表示用文字列 long int Target_Speed_A=0,Target_Speed_B=0; //目標速度 long int Target_Speed_A1=0,Target_Speed_B1=0; //目標速度 @@ -113,6 +129,10 @@ int SG_Flag=0; int SG_Cnt=0; int Cross_Flag=0; + +int Row=0; //行変数 +float course_data[100][3]; //記憶走行用配列 + void sensor_analog_read(){ S1_Data=s1.read(); S2_Data=s2.read(); @@ -213,7 +233,25 @@ Machine_Status|=STOP; //機体停止状態へ SG_Cnt=0; }else if(Corner_Flag==1){//コーナマーカの時 - Coner_c++; + + Distance_memory_A=0; + Distance_memory_B=0; + Enc_Count_A=encoder_a.Get(); //エンコーダパルス数を取得 + Enc_Count_B=-encoder_b.Get(); + Distance_memory_A=(Enc_Count_A*PULSE_TO_UM); + Distance_memory_B=(Enc_Count_B*PULSE_TO_UM); + + course_data[Row][0]=Distance_memory_A; + course_data[Row][1]=Distance_memory_B; + course_data[Row][2]=Medium_Speed; + + /*PC.printf("左:%.2f",course_data[Row][0]); + PC.printf("右:%.2f",course_data[Row][1]); + PC.printf("速度:%.2f",course_data[Row][2]); + */ + + Row++; + } }else{//マーカではなく、誤検知だった場合。 //何もしない @@ -236,14 +274,14 @@ ////モータ現在速度の取得 Enc_Count_A=encoder_a.Get(); //エンコーダパルス数を取得 Enc_Count_B=-encoder_b.Get(); - Distance_A=(Enc_Count_A*PULSE_TO_UM); //移動距離をmm単位で格納 - Distance_B=(Enc_Count_B*PULSE_TO_UM); + //Distance_A=(Enc_Count_A*PULSE_TO_UM); //移動距離をmm単位で格納 + //Distance_B=(Enc_Count_B*PULSE_TO_UM); Distance_memory_A=(Enc_Count_A*PULSE_TO_UM); Distance_memory_B=(Enc_Count_B*PULSE_TO_UM); - Speed_A=(Distance_A*1000);//走行速度演算[mm/s] - Speed_B=(Distance_B*1000); + Speed_A=(Distance_A*1000)/INTERRUPT_TIME;//走行速度演算[mm/s] + Speed_B=(Distance_B*1000)/INTERRUPT_TIME; if(Machine_Status&STOP){//機体停止状態の時 Enc_A_Rotate+=Enc_Count_A;//閾値用に左エンコーダ値の蓄積 @@ -252,13 +290,16 @@ Enc_B_Rotate+=Enc_Count_B;//速度用に右エンコーダ値の蓄積 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(Marker_Pass_Flag==1){//マーカ通過中は通過距離情報を蓄積する。 Marker_Run_Distance+=(Distance_A+Distance_B)/2; } //エンコーダ関連情報のリセット - Distance_A=0; - Distance_B=0; + //Distance_A=0; + //Distance_B=0; + encoder_a.Set(0); encoder_b.Set(0); @@ -266,13 +307,13 @@ memory_B=Distance_memory_B; /////各モータの目標速度の設定 - /*if(Sw==0){ + if(Sw==0){ Motor_A_Diff[1]=Motor_A_Diff[0]; Motor_B_Diff[1]=Motor_B_Diff[0]; - Target_Speed_A=Machine_Speed; - Target_Speed_B=Machine_Speed; + Target_Speed_A=Medium_Speed; + Target_Speed_B=Medium_Speed; Motor_A_Diff[0]=(Target_Speed_A-Speed_A); Motor_B_Diff[0]=(Target_Speed_B-Speed_B); @@ -282,8 +323,8 @@ Motor_A_Diff[1]=Motor_A_Diff[0]; Motor_B_Diff[1]=Motor_B_Diff[0]; - Target_Speed_A1=Machine_Speed1; - Target_Speed_B1=Machine_Speed1; + Target_Speed_A1=High_Speed; + Target_Speed_B1=High_Speed; Motor_A_Diff[0]=(Target_Speed_A1-Speed_A); Motor_B_Diff[0]=(Target_Speed_B1-Speed_B); @@ -291,7 +332,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); @@ -299,9 +340,9 @@ Motor_A_Diff[0]=(Target_Speed_A1-Speed_A); Motor_B_Diff[0]=(Target_Speed_B1-Speed_B); - */ + - /*//P成分演算 + //P成分演算 Motor_A_P=Motor_A_Diff[0]*M_KP; Motor_B_P=Motor_B_Diff[0]*M_KP; //D成分演算 @@ -314,7 +355,7 @@ Motor_A_Pwm=Motor_A_PD+Sensor_PD; Motor_B_Pwm=Motor_B_PD-Sensor_PD; - /* //モータ制御量の上限下限設定 + //モータ制御量の上限下限設定 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; @@ -339,7 +380,7 @@ }else{//停止状態の時はモータへの出力は無効 motor_a=0; motor_b=0; - }*/ + } } int main() { @@ -348,54 +389,54 @@ lcd.locate(0,0); lcd.print("STOP"); lcd.locate(0,1); - lcd.print(" "); + //lcd.print(" "); - //sprintf(Speed_Str,"%04d",Machine_Speed); - //lcd.print(Speed_Str); + sprintf(Speed_Str,"%04d",Medium_Speed); + lcd.print(Speed_Str); 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); + //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.print("STOP"); - lcd.locate(0,0); - lcd.print(MemoryA_Str); - lcd.locate(0,1); - lcd.print(MemoryB_Str); - - Sw++; - + //lcd.locate(0,0); + //lcd.print(MemoryA_Str); + //lcd.locate(0,1); + //lcd.print(MemoryB_Str); + wait(5); + Gray=DEFAULT_GRAY; + Medium_Speed=DEFAULT_SPEED1; + + Sw++; } if(Machine_Status&STOP){//機体停止状態の時 Gray=DEFAULT_GRAY+((float)Enc_A_Rotate/16000);//センサ閾値調整 - //sprintf(Gray_Str,"%3.2f",Gray);//速度情報文字列変換 - //sprintf(Coner_str,"%d",Coner_c); - + sprintf(Gray_Str,"%3.2f",Gray);//速度情報文字列変換 + sprintf(Coner_str,"%d",Coner_c); - //lcd.print(Gray_Str); - //lcd.print(" "); - //lcd.print(Coner_str); + lcd.print(Gray_Str); + lcd.print(" "); + lcd.print(Coner_str); if(Sw==0){ - Machine_Speed=DEFAULT_SPEED+(Enc_B_Rotate/16);//標準速度調整 - sprintf(Speed_Str,"%04d",Machine_Speed);//速度情報文字列変換 - + Medium_Speed=DEFAULT_SPEED1+(Enc_B_Rotate/16);//標準速度調整 + sprintf(Speed_Str,"%04d",Medium_Speed);//速度情報文字列変換 }else { - Machine_Speed1=DEFAULT_SPEED1+(Enc_B_Rotate/16); - sprintf(Speed_Str,"%04d",Machine_Speed1);//速度情報文字列変換 + High_Speed=DEFAULT_SPEED2+(Enc_B_Rotate/16); + sprintf(Speed_Str,"%04d",High_Speed);//速度情報文字列変換 } - /*lcd.locate(0,1); + lcd.locate(0,1); lcd.print(" "); lcd.locate(0,1); - lcd.print(Speed_Str);*/ + lcd.print(Speed_Str); } @@ -408,6 +449,7 @@ lcd.locate(0,0); lcd.print("GO!!"); wait(2); + Stop_Distance=0; Machine_Status&=0x7F;//ストップ状態解除 }else{//機体走行中であったとき //各種フラグのクリア