![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
マーカーで読み取りLCDを使用してカウントした値を表示する
Dependencies: mbed AQM0802 CRotaryEncoder TB6612FNG
Revision 14:7ed78f52f40e, committed 2019-11-11
- Comitter:
- poritekutama
- Date:
- Mon Nov 11 06:25:06 2019 +0000
- Parent:
- 13:97be8e29ae50
- Commit message:
- test
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Thu Aug 29 01:07:15 2019 +0000 +++ b/main.cpp Mon Nov 11 06:25:06 2019 +0000 @@ -7,12 +7,13 @@ //☆★☆★各種パラメータ調整箇所☆★☆★☆★ #define DEFAULT_SPEED 800 //1走目の基本速度[mm/sec] -#define TURN_POWER 0.47 //コースアウト時の旋回力 -#define PULSE_TO_UM 30 //エンコーダ1パルス当たりのタイヤ移動距離[um] +#define DEFAULT_SPEED1 900 +#define TURN_POWER 0.6 //コースアウト時の旋回力 +#define PULSE_TO_UM 28 //エンコーダ1パルス当たりのタイヤ移動距離[um] #define INTERRUPT_TIME 1000 //割りこみ周期[us] -#define DEFAULT_GRAY 0.5f //フォトリフレクタデジタル入力の閾値 +#define DEFAULT_GRAY 0.2f //フォトリフレクタデジタル入力の閾値 //シリアル通信でSensor_Digital値を確認し調整する。 -#define MARKER_WIDTH 15 //マーカ幅[mm](ビニルテープ幅19[mm]以内) +#define MARKER_WIDTH 10000 //マーカ幅[um](ビニルテープ幅19000[um]以内) //コースの傷によってマーカ誤検知する場合は値を大きくする。 #define CROSS_JUDGE 4 //ラインセンサいくつ以上白線検知で交差点認識するか設定。 //モータ速度のゲイン関連(むやみに調整しない) @@ -21,12 +22,12 @@ //フォトリフレクタのゲイン(外側に行くにつれ値を何倍させたいか調整する。) #define S_K1 1.0f //float演算させる値には必ずfを付ける -#define S_K2 2.0f //2倍 -#define S_K3 4.0f //4倍 +#define S_K2 2.4f //2倍 +#define S_K3 4.7f //4倍 //ラインセンサ各種制御成分 -#define S_KP 1.07f //ラインセンサ比例成分。大きいほど曲がりやすい -#define S_KD 0.7f //ラインセンサ微分成分。大きいほど急なラインずれに強くなる。 +#define S_KP 1.0f //ラインセンサ比例成分。大きいほど曲がりやすい +#define S_KD 0.5f //ラインセンサ微分成分。大きいほど急なラインずれに強くなる。 //////////☆★☆★☆★☆★☆★////////////// @@ -47,13 +48,13 @@ //デジタル入力オブジェクト定義 DigitalIn push_sw(D13); /////アナログ入力オブジェクト定義////////// -AnalogIn s1(A1); -AnalogIn s2(D3); -AnalogIn s3(A6); -AnalogIn s4(A5); -AnalogIn s5(A4); -AnalogIn s6(A3); -AnalogIn s7(A2); +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); @@ -67,6 +68,9 @@ //使用変数の定義 int Sw_Ptn; int Old_Sw_Ptn; +int Sw=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; //ラインセンサ総データ量 double Sensor_Diff[2]={0,0}; //ラインセンサ偏差 @@ -78,12 +82,19 @@ long int Enc_Count_A=0,Enc_Count_B=0; //エンコーダパルス数を格納 long int Enc_A_Rotate=0,Enc_B_Rotate=0; +/*long int memory_A=0; +long int memory_B=0; +char memoryA_Str[5]; +char memoryB_Str[5];*/ + long int Distance_A=0,Distance_B=0; //タイヤ移動距離を格納[mm] 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; 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; //目標速度 long int Motor_A_Diff[2]={0,0}; //過去の速度偏差と現在の速度偏差を格納 long int Motor_B_Diff[2]={0,0}; // float Motor_A_P,Motor_B_P; //モータ速度制御P成分 @@ -201,7 +212,7 @@ Machine_Status|=STOP; //機体停止状態へ SG_Cnt=0; }else if(Corner_Flag==1){//コーナマーカの時 - //地区大会では何もしない + Coner_c++; } }else{//マーカではなく、誤検知だった場合。 //何もしない @@ -247,16 +258,41 @@ encoder_b.Set(0); /////各モータの目標速度の設定 - Target_Speed_A=Machine_Speed; - Target_Speed_B=Machine_Speed; + 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; + + Motor_A_Diff[0]=(Target_Speed_A-Speed_A); + Motor_B_Diff[0]=(Target_Speed_B-Speed_B); + } + else { + + 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; + + Motor_A_Diff[0]=(Target_Speed_A1-Speed_A); + Motor_B_Diff[0]=(Target_Speed_B1-Speed_B); + } /////モータの速度制御 //過去の速度偏差を退避 - 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); + + Motor_A_Diff[0]=(Target_Speed_A1-Speed_A); + Motor_B_Diff[0]=(Target_Speed_B1-Speed_B); + */ + //P成分演算 Motor_A_P=Motor_A_Diff[0]*M_KP; Motor_B_P=Motor_B_Diff[0]*M_KP; @@ -311,37 +347,52 @@ while(1){ Old_Sw_Ptn=Sw_Ptn; //過去のスイッチ入力情報を退避 Sw_Ptn=push_sw; //現在のスイッチ入力情報の取得 - if((!(Old_Machine_Status&=STOP))&&(Machine_Status&=STOP)){//走行終了時 + if((!(Old_Machine_Status&STOP))&&(Machine_Status&STOP)){//走行終了時 lcd.locate(0,0); lcd.print(" "); lcd.locate(0,0); - lcd.print("STOP"); + lcd.print("STOP"); + 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); lcd.locate(0,0); lcd.print(" "); lcd.locate(0,0); - lcd.print(Gray_Str); - Machine_Speed=DEFAULT_SPEED+(Enc_B_Rotate/16);//標準速度調整 - sprintf(Speed_Str,"%04d",Machine_Speed);//速度情報文字列変換 + 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);//速度情報文字列変換 + }else { + Machine_Speed1=DEFAULT_SPEED1+(Enc_B_Rotate/16); + sprintf(Speed_Str,"%04d",Machine_Speed1);//速度情報文字列変換 + } + lcd.locate(0,1); lcd.print(" "); lcd.locate(0,1); - lcd.print(Speed_Str); + lcd.print(Speed_Str); + + } if((Old_Sw_Ptn==PULL)&&(Sw_Ptn==PUSH)){//スイッチが押された瞬間 if(Machine_Status&STOP){//機体停止状態の時 + Coner_c=0; lcd.locate(0,0); lcd.print(" "); lcd.locate(0,0); lcd.print("GO!!"); wait(2); - Machine_Status&=0x7F;//ストップ状態解除 + Machine_Status&=0x7F;//ストップ状態解除 }else{//機体走行中であったとき //各種フラグのクリア Corner_Flag=0;