Diff: main.cpp

Revision:

1:3b12960a1b5e

Parent:

0:5e29061237cd

--- a/main.cpp	Tue Jan 14 04:31:57 2020 +0000
+++ b/main.cpp	Tue Jan 14 11:38:30 2020 +0000
@@ -42,30 +42,30 @@
 const float roll_duty_large = 0.21f;//右左折時に作用 白黒黒//0.30
 const float medium_roll_duty_small = 0.19f;//中右左折時に作用 白白黒//0.25
 const float medium_roll_duty_large = 0.25f;//中右左折時に作用 白白黒//0.35
-const float sharp_roll_duty_forward = 0.20f;//急カーブ時に作用    超信地旋回   白白白//0.25
-const float sharp_roll_duty_back = 0.20f;//急カーブ時に作用    超信地旋回   白白白//0.25
+const float sharp_roll_duty_forward = 0.25f;//急カーブ時に作用    超信地旋回   白白白//0.25
+const float sharp_roll_duty_back = 0.25f;//急カーブ時に作用    超信地旋回   白白白//0.25
 //const float duty_ignition = 0.20f;//急カーブ修了時にスピードアップ   
 
 //int re_ignition_counter = 0;//下の格納
 //int re_ignition_limit = 2;//スピードアップのカウンター(回数＊0.01s)
 
 //フォトリフレクタセンサ関連の定数
-
-const float L_black = 2.3;//左センサの黒値
-const float L_white = 0.3;//白値
-const float R_black = 0.8;//右センサの黒値
-const float R_white = 0.2;//
-const float C_black = 1.2;//中心センサの黒値
-const float C_white = 0.2;//
-
+/*
+const float L_black = 2.4;//左センサの黒値
+const float L_white = 1.2;//白値
+const float R_black = 1.4;//右センサの黒値
+const float R_white = 0.6;//
+const float C_black = 2.2(1.5まで);//中心センサの黒値
+const float C_white = 0.6;//
+*/
 
 //float L_target = (L_black + L_white) / 2;
 //float R_target = (R_black + R_white) / 2;
 //float C_target = (C_black + C_white) / 2;
 
-float L_target = 1.0;
-float R_target = 0.6;
-float C_target = 0.6;
+float L_target = 1.5;
+float R_target = 1.5;
+float C_target = 1.5;
 
 //回転数関連の変数、定数
 float R_rot=0;//右タイヤの１秒間あたり回転数
@@ -181,6 +181,7 @@
     
 int mode;//展開中の制御状態記号の格納
 int pre_mode = mode;//一つ前の状態記号の格納:線を外れた際に引用
+int mode_change = 0;//modeが変わったら１
 
         
 enum restart_ignition{//減速後の復帰のための操作に使用する信号
@@ -228,90 +229,9 @@
         C_on_off=0;
     }
     
-    
-    if(L_on_off==0 && C_on_off==0 && R_on_off==0)//線の外
-    {
-        if(mode == MEDIUM_R || mode == RIGHT || mode == SHARP_R)
-        {
-            mode = SHARP_R;
-            Right_high_rotation(sharp_roll_duty_back,sharp_roll_duty_forward);
-            ////re_ignition = ready;
-        }else if(mode == MEDIUM_L || mode == LEFT || mode == SHARP_L){
-            mode = SHARP_L;
-            Right_high_rotation(sharp_roll_duty_forward,sharp_roll_duty_back);
-            //re_ignition = ready;
-        }
-            
-        if(mode_debug==1){
-        printf("I'm going out of line! Previous condition is %d \n",mode);
-        }
-        
-    }
-    else if(L_on_off==1 && C_on_off==0 && R_on_off==0)//左折
-    {
-        mode = MEDIUM_L;
-        machine_Forward(medium_roll_duty_large,medium_roll_duty_small);
-        //re_ignition = caution;
-        if(mode_debug==1){
-        printf("M_LEFT\n");
-        }
-    }
-    else if(L_on_off==0 && C_on_off==0 && R_on_off==1)//右折
-    {
-        mode = MEDIUM_R;
-        machine_Forward(medium_roll_duty_small,medium_roll_duty_large);
-        //re_ignition = caution;
-        if(mode_debug==1){
-        printf("M_RIGHT\n");
-        }
-    }
-    else if(L_on_off==1 && C_on_off==0 && R_on_off==1)//黒白黒、エラー、back
+    if(pre_C_on_off != C_on_off || pre_R_on_off != R_on_off || pre_L_on_off != L_on_off)//modeが変わるか否かの判断
     {
-        //mode = REVERSE;
-        //re_ignition = no;
-        if(mode_debug==1){
-        printf("REVERSE\n");
-        }
-    }
-    else if(L_on_off==0 && C_on_off==1 && R_on_off==0)//前進
-    {
-        //re_ignition = no;
-        machine_Forward(R_duty_basic,L_duty_basic);
-        mode = STRAIGHT;
-        if(mode_debug==1){
-        printf("STRAIGHT\n");
-        }
-    }
-    else if(L_on_off==1 && C_on_off==1 && R_on_off==0)//左折
-    {
-        //re_ignition =no;
-        machine_Forward(roll_duty_large,roll_duty_small);
-        mode = LEFT;
-        if(mode_debug==1){
-        printf("LEFT\n");
-        }
-    }
-    else if(L_on_off==0 && C_on_off==1 && R_on_off==1)//右折
-    {
-        //re_ignition = no;
-        machine_Forward(roll_duty_small,roll_duty_large);
-        mode = RIGHT;
-        if(mode_debug==1){
-        printf("RIGHT\n");
-        }
-    }
-    else if(L_on_off==1 && C_on_off==1 && R_on_off==1)//all black（空中or十字部)よって直進
-    {
-        //re_ignition = no;
-        machine_Forward(R_duty_basic,L_duty_basic);
-        mode = STRAIGHT;
-        if(mode_debug==1){
-        printf("STRAIGHT\n");
-        }
-    }
-    
-    if(pre_C_on_off != C_on_off || pre_R_on_off != R_on_off || pre_L_on_off != L_on_off)//新地回転の前と後でre_ignitionを添加
-    {
+        mode_change = 1;
         /*if(re_ignition == caution || re_ignition ==ready)
         {
             re_ignition = yes;
@@ -321,10 +241,97 @@
              printf("Judgement of sensor L[%d] C[%d] R[%d]\n",L_on_off,C_on_off,R_on_off);//[1]なら黒判定
         }
         //frame_counter = 0;
+    }else{
+        //frame_counter++;
+        mode_change = 0;
+    }    
+    if(L_on_off==0 && C_on_off==0 && R_on_off==0 && mode_change ==1)//線の外
+    {
+        if(mode == MEDIUM_R || mode == RIGHT || mode == SHARP_R)
+        {
+            //mode = SHARP_R;
+            //Right_high_rotation(sharp_roll_duty_back,sharp_roll_duty_forward);
+            ////re_ignition = ready;
+            mode = MEDIUM_R;
+            machine_Forward(medium_roll_duty_small,medium_roll_duty_large);
+            
+        }else if(mode == MEDIUM_L || mode == LEFT || mode == SHARP_L){
+            //mode = SHARP_L;
+            //Right_high_rotation(sharp_roll_duty_forward,sharp_roll_duty_back);
+            //re_ignition = ready;
+            mode = MEDIUM_L;
+            machine_Forward(medium_roll_duty_large,medium_roll_duty_small);
+        }
+            
+        if(mode_debug==1){
+        printf("I'm going out of line! Previous condition is %d \n",mode);
+        }
+        
     }
-    else{
-        //frame_counter++;
-        }       
+    else if(L_on_off==1 && C_on_off==0 && R_on_off==0 && mode_change ==1)//左折
+    {
+        mode = MEDIUM_L;
+        machine_Forward(medium_roll_duty_large,medium_roll_duty_small);
+        //re_ignition = caution;
+        if(mode_debug==1){
+        printf("M_LEFT\n");
+        }
+    }
+    else if(L_on_off==0 && C_on_off==0 && R_on_off==1 && mode_change ==1)//右折
+    {
+        mode = MEDIUM_R;
+        machine_Forward(medium_roll_duty_small,medium_roll_duty_large);
+        //re_ignition = caution;
+        if(mode_debug==1){
+        printf("M_RIGHT\n");
+        }
+    }
+    else if(L_on_off==1 && C_on_off==0 && R_on_off==1 && mode_change ==1)//黒白黒、エラー、back
+    {
+        //mode = REVERSE;
+        //re_ignition = no;
+        if(mode_debug==1){
+        printf("REVERSE\n");
+        }
+    }
+    else if(L_on_off==0 && C_on_off==1 && R_on_off==0 && mode_change ==1)//前進
+    {
+        //re_ignition = no;
+        machine_Forward(R_duty_basic,L_duty_basic);
+        mode = STRAIGHT;
+        if(mode_debug==1){
+        printf("STRAIGHT\n");
+        }
+    }
+    else if(L_on_off==1 && C_on_off==1 && R_on_off==0 && mode_change ==1)//左折
+    {
+        //re_ignition =no;
+        machine_Forward(roll_duty_large,roll_duty_small);
+        mode = LEFT;
+        if(mode_debug==1){
+        printf("LEFT\n");
+        }
+    }
+    else if(L_on_off==0 && C_on_off==1 && R_on_off==1 && mode_change ==1)//右折
+    {
+        //re_ignition = no;
+        machine_Forward(roll_duty_small,roll_duty_large);
+        mode = RIGHT;
+        if(mode_debug==1){
+        printf("RIGHT\n");
+        }
+    }
+    else if(L_on_off==1 && C_on_off==1 && R_on_off==1 && mode_change ==1)//all black（空中or十字部)よって直進
+    {
+        //re_ignition = no;
+        machine_Forward(R_duty_basic,L_duty_basic);
+        mode = STRAIGHT;
+        if(mode_debug==1){
+        printf("STRAIGHT\n");
+        }
+    }
+
+   
 }