Diff: main.cpp

Revision:

7:f1f9bc420dba

Parent:

6:28c5a5992a81

Child:

8:b47cebb6aeb8

--- a/main.cpp	Tue Nov 20 18:54:06 2018 +0000
+++ b/main.cpp	Wed Nov 21 06:37:09 2018 +0000
@@ -1,17 +1,16 @@
 #include "mbed.h"
 
 #define KP 5.8  //Pゲイン
-#define CHG 0.05 //change_gray用の係数
-#define WHITE_CL 0.92   //初期値
-#define WHITE_CR 0.92    //初期値
-#define GRAY_CL 0.46    //初期値
-#define GRAY_CR 0.5     //初期値
+#define CHG 1.0 //change_gray用の係数
+#define WHITE_CL 0.27   //初期値
+#define WHITE_CR 0.235    //初期値
+#define GRAY_CL 0.15    //初期値
+#define GRAY_CR 0.13     //初期値
 #define WHITE_L 0.1     //初期値
 #define WHITE_R 0.1     //初期値
 #define GRAY_L 0.05     //初期値
 #define GRAY_R 0.05     //初期値
 
-//#define sensor_not_straight
 //#define graychange
 
 DigitalOut ledCheck( PTE2); //動作check
@@ -32,8 +31,8 @@
 
 float whiteCL[2], whiteCR[2], grayCL[2], grayCR[2]; //[1]が初期設定の基準値，[0]が変動する閾値
 float whiteL[2], whiteR[2], grayL[2], grayR[2];
-float blackCL = 0.18, blackCR = 0.1;   //閾値
-float blackL = 0.028, blackR = 0.028;
+float blackCL = 0.03, blackCR = 0.02;   //閾値
+float blackL = 0.018, blackR = 0.018;
 float stepL = 0.45, stepR = 0.38;
 float sensor[4];    //sensor[0]:sensorL ... sensor[3]:sensorR
 float pr, pl, chg;
@@ -65,9 +64,15 @@
     pl = (sensor[1] - grayCL[0]) / (whiteCL[0] - grayCL[0]);
     pr = (sensor[2] - grayCR[0]) / (whiteCR[0] - grayCR[0]);
     Mrighti = 2;
-    Mrightp = (KP * pr) * 1.0f;
+    if( pl >= 0.0)
+        Mrightp = (KP * pr) * 1.0f;
+    else
+        Mrightp = 1.0f;
     Mlefti = 2;
-    Mleftp = (KP * pl) * 1.0f;
+    if( pr >= 0.0)
+        Mleftp = (KP * pl + 1.0) * 1.0f;
+    else
+        Mleftp = 1.0f;
     ledRR = 0b01;
     ledLL = 0b01;
 }
@@ -75,23 +80,13 @@
 void go_straight_CR(){  //CRのみでトレース
     pr = (sensor[2] - grayCR[0]) / (whiteCR[0] - grayCR[0]);
     Mrighti = 2;
-    Mrightp = (KP * pr + 0.3) * 1.0f;
+    Mrightp = (KP * pr) * 1.0f;
     Mlefti = 2;
-    Mleftp = (1.3 - KP * pr) * 1.0f;
+    Mleftp = (1.0 - KP * pr) * 1.0f;
     ledRR = 0b11;
     ledLL = 0;
 }
 
-void go_straight_CL(){  //CLのみでトレース
-    pl = (sensor[1] - grayCL[0]) / (whiteCL[0] - grayCL[0]);
-    Mrighti = 2;
-    Mrightp = (1.3 - KP * pl) * 1.0f;
-    Mlefti = 2;
-    Mleftp = (KP * pl + 0.3) * 1.0f;
-    ledRR = 0;
-    ledLL = 0b11;
-}
-
 void turn_right(){
     Mrighti = 1;
     Mrightp = 0.05f;
@@ -110,40 +105,30 @@
     ledLL = 0b01;
 }
 
-void turn_right_corner(){
-    #ifdef sensor_not_straight
-    while( sensor[2] > blackCR){
-        set_sensor();
-        go_straight_p();
-    }
-    #endif
+void turn_right_corner( float f){
     motor_check();
-    wait(0.2);
-    while( sensor[3] > blackR){   //sensorR > grayR[0]
+    wait(f);
+    sensor[3] = sensorR.read();
+    while( sensor[3] >= blackR){   //sensorR > grayR[0]
         sensor[3] = sensorR.read();
         turn_right();
     }
     stop_point_ver2();
-    turn_left();
-    wait(0.3);
+//    turn_left();
+//    wait(0.3);
 }
 
-void turn_left_corner(){
-    #ifdef sensor_not_straight
-    while( sensor[1] > blackCL){
-        set_sensor();
-        go_straight_p();
-    }
-    #endif
+void turn_left_corner( float f){
     motor_check();
-    wait(0.2);
-    while( sensor[0] > blackL){   //sensorL > grayL[0]
+    wait(f);
+    sensor[0] = sensorL.read();
+    while( sensor[0] >= blackL){   //sensorL > grayL[0]
         sensor[0] = sensorL.read();
         turn_left();
     }
     stop_point_ver2();
-    turn_right();
-    wait(0.3);
+//    turn_right();
+//    wait(0.3);
 }
 
 void motor_check(){   //モータドライバの調子の確認用と単純に直進
@@ -196,29 +181,6 @@
     }
 }
 
-void go_step(){
-    if( sensor[0] > stepL){     //左側に段差がある場合
-        #ifdef sensor_not_straight
-        motor_check();
-        wait(0.2);
-        #endif
-        while( sensor[3] < stepR){
-            set_sensor();
-            go_straight_CR();
-        }
-    } else if( sensor[3] > stepR){      //右側に段差がある場合
-        #ifdef sensor_not_straight
-        motor_check();
-        wait(0.2);
-        #endif
-        while( sensor[0] < stepL){
-            set_sensor();
-            go_straight_CL();
-        }
-    } else
-        go_straight_p();        
-}
-
 void LED_blinky(){      //プログラムの切り替わり確認用
     for( int i = 0; i < 4; i++){
         ledCount = !ledCount;
@@ -233,15 +195,13 @@
     ledCheck = 1;
     set_threshold();
     LED_blinky();
-    motor_check();
-    wait(0.2);
     while(1){      //スタート～最初の左折カーブ
         set_sensor();
         #ifdef graychange
         change_gray_ver2(i);
         #endif
         if( sensor[3] <= blackR){
-            turn_right_corner();
+            turn_right_corner( 0.2);
             break;
         } else
             go_straight_p();
@@ -250,10 +210,10 @@
     while(1){       //左折カーブ～段差～左折カーブ
         set_sensor();
         if( sensor[0] <= blackL){
-            turn_left_corner();
+            turn_left_corner( 0.2);
             break;
         } else
-            go_step();
+            go_straight_p();
     }
     LED_blinky();
     #ifdef graychange
@@ -265,7 +225,7 @@
         change_gray_ver2(i);
         #endif
         if( sensor[0] <= blackL){
-            turn_left_corner();
+            turn_left_corner( 0.2);
             break;
         } else
             go_straight_p();
@@ -273,7 +233,7 @@
     LED_blinky();
     while(1){       //左折カーブ～トンネル手前のマーク
         set_sensor();
-        if( sensor[0] <= blackL && sensor[3] <= blackR)
+        if( sensor[0] <= grayL[0] && sensor[3] <= grayR[0])
             break;
         else
             go_straight_CR();
@@ -282,7 +242,7 @@
     while(1){       //トンネル手前のマーク～Ｔ字
         set_sensor();
         if( sensor[0] <= blackL && sensor[3] <= blackR){
-            turn_right_corner();
+            turn_right_corner( 0.2);
             break;
         } else
             go_straight_p();
@@ -291,25 +251,25 @@
     #ifdef graychange
     i = 0;
     #endif
-    while(1){       //Ｔ字～半円終わり
+    while(1){       //Ｔ字～半円～鋭角カーブ
         set_sensor();
         #ifdef graychange
         change_gray_ver2(i);
         #endif
         if( sensor[0] <= blackL){
-            turn_left_corner();
+            turn_left_corner( 0.5);
             break;
         } else
-            go_straight_p();
+            go_straight_CR();
     }
     LED_blinky();
-    while(1){       //半円終わり～ゴール
+    while(1){       //鋭角カーブ終わり～ゴール
         set_sensor();
         if( sensor[0] <= blackL)
-            turn_left_corner();
+            turn_left_corner( 0.2);
         else if( sensor[3] <= blackR)
-            turn_right_corner();
-        else if( sensor[0] <= blackL && sensor[3] <= blackR){
+            turn_right_corner( 0.2);
+        else if( sensor[0] <= grayL[0] && sensor[3] <= grayR[0]){
             stop_point_ver2();
             break;
         } else