![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
ニュージェネの28日午前時点のベストチューニング。1走目900[mm/sec]、2走目850[mm/sec],3走目1000[mm/sec]で走行可能。バッテリは満充電8.4[V]で確認。
Dependencies: mbed AQM0802 CRotaryEncoder TB6612FNG
Diff: main.cpp
- Revision:
- 12:dc4c569248d7
- Parent:
- 11:2cd6f8be124e
- Child:
- 13:97be8e29ae50
--- a/main.cpp Wed Aug 28 23:14:20 2019 +0000 +++ b/main.cpp Thu Aug 29 00:59:01 2019 +0000 @@ -10,7 +10,7 @@ #define TURN_POWER 0.5 //コースアウト時の旋回力 #define PULSE_TO_UM 30 //エンコーダ1パルス当たりのタイヤ移動距離[um] #define INTERRUPT_TIME 1000 //割りこみ周期[us] -#define GRAY 0.6f //フォトリフレクタデジタル入力の閾値 +#define DEFAULT_GRAY 0.5f //フォトリフレクタデジタル入力の閾値 //シリアル通信でSensor_Digital値を確認し調整する。 #define MARKER_WIDTH 15 //マーカ幅[mm](ビニルテープ幅19[mm]以内) //コースの傷によってマーカ誤検知する場合は値を大きくする。 @@ -73,8 +73,11 @@ double Sensor_P =0.0f; //ラインセンサP(比例成分)制御量 double Sensor_D =0.0f; //ラインセンサD(微分成分)制御量 double Sensor_PD=0.0f; //ラインセンサP,D成分の合計 +char Gray_Str[5]; //LCD閾値表示用文字列 +float Gray=DEFAULT_GRAY; long int Enc_Count_A=0,Enc_Count_B=0; //エンコーダパルス数を格納 -long int Enc_B_Rotate=0; +long int Enc_A_Rotate=0,Enc_B_Rotate=0; + long int Distance_A=0,Distance_B=0; //タイヤ移動距離を格納[mm] long int Marker_Run_Distance=0; long int Speed_A=0, Speed_B=0; //現在速度 @@ -113,34 +116,34 @@ void sensor_digital_read(){//8つのフォトリフレクタの入力を8ビットのデジタルパターンに変換 Sensor_Cnt=0; Old_Sensor_Digital=Sensor_Digital; - if(S1_Data>GRAY){ + if(S1_Data>Gray){ Sensor_Digital |= 0x80; //7ビット目のみセット (1にする。) }else Sensor_Digital &= 0x7F; //7ビット目のみマスク(0にする。) - if(S2_Data>GRAY){ + if(S2_Data>Gray){ Sensor_Digital |= 0x40; //6ビット目のみセット (1にする。) Sensor_Cnt++; }else Sensor_Digital &= 0xBF; //6ビット目のみマスク(0にする。) - if(S3_Data>GRAY){ + if(S3_Data>Gray){ Sensor_Digital |= 0x20; //5ビット目のみセット (1にする。) Sensor_Cnt++; }else Sensor_Digital &= 0xDF; //5ビット目のみマスク(0にする。) - if(S4_Data>GRAY){ + if(S4_Data>Gray){ Sensor_Digital |= 0x10; //4ビット目のみセット (1にする。) Sensor_Cnt++; }else Sensor_Digital &= 0xEF; //4ビット目のみマスク(0にする。) - if(S5_Data>GRAY){ + if(S5_Data>Gray){ Sensor_Digital |= 0x08; //3ビット目のみセット (1にする。) Sensor_Cnt++; }else Sensor_Digital &= 0xF7; //3ビット目のみマスク(0にする。) - if(S6_Data>GRAY){ + if(S6_Data>Gray){ Sensor_Digital |= 0x04; //2ビット目のみセット (1にする。) Sensor_Cnt++; }else Sensor_Digital &= 0xFB; //2ビット目のみマスク(0にする。) - if(S7_Data>GRAY){ + if(S7_Data>Gray){ Sensor_Digital |= 0x02; //1ビット目のみセット (1にする。) Sensor_Cnt++; }else Sensor_Digital &= 0xFD; //1ビット目のみマスク(0にする。) - if(S8_Data>GRAY){ + if(S8_Data>Gray){ Sensor_Digital |= 0x01; //0ビット目のみセット (1にする。) }else Sensor_Digital &= 0xFE; //0ビット目のみマスク(0にする。) } @@ -227,7 +230,10 @@ Speed_B=(Distance_B*1000)/INTERRUPT_TIME; if(Machine_Status&STOP){//機体停止状態の時 - Enc_B_Rotate+=Enc_Count_B;//モード設定用に右エンコーダ値の蓄積 + Enc_A_Rotate+=Enc_Count_A;//閾値用に左エンコーダ値の蓄積 + if(Enc_A_Rotate<-6400)Enc_A_Rotate=-6400; + if(Enc_A_Rotate>6400)Enc_A_Rotate=6400; + Enc_B_Rotate+=Enc_Count_B;//速度用に右エンコーダ値の蓄積 if(Enc_B_Rotate<-6400)Enc_B_Rotate=-6400; if(Enc_B_Rotate>6400)Enc_B_Rotate=6400; } @@ -312,8 +318,15 @@ lcd.print("STOP"); } if(Machine_Status&STOP){//機体停止状態の時 + + Gray=DEFAULT_GRAY+((float)Enc_A_Rotate/16000);//センサ閾値調整 + sprintf(Gray_Str,"%3.2f",Gray);//速度情報文字列変換 + 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);//速度情報文字列変換 + sprintf(Speed_Str,"%04d",Machine_Speed);//速度情報文字列変換 lcd.locate(0,1); lcd.print(" "); lcd.locate(0,1);