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ビットのデジタルパターンに変換
     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);