Diff: main.cpp

Revision:

3:e8c29cd3ca22

Parent:

2:47477c2b1925

Child:

4:edec6fe32ba9

--- a/main.cpp	Fri Nov 16 07:12:57 2018 +0000
+++ b/main.cpp	Tue Nov 20 16:20:44 2018 +0000
@@ -1,22 +1,24 @@
 #include "mbed.h"
 
 #define KP 5.8  //Pゲイン
-#define CHG 1.0 //change_gray用の係数
+#define CHG 0.05 //change_gray用の係数
 #define WHITE_CL 0.92   //初期値
-#define WHITE_CR 0.70    //初期値
+#define WHITE_CR 0.92    //初期値
 #define GRAY_CL 0.46    //初期値
 #define GRAY_CR 0.5     //初期値
-#define GRAY_L 0.28     //初期値
-#define GRAY_R 0.28     //初期値
+#define WHITE_L 0.1     //初期値
+#define WHITE_R 0.1     //初期値
+#define GRAY_L 0.05     //初期値
+#define GRAY_R 0.05     //初期値
 
-#define DEBUG
+//#define DEBUG
 
 DigitalOut ledL( PTB8);
 DigitalOut ledR( PTE5);
 DigitalOut ledC( PTE2); //動作check
 BusOut ledLL( PTB8, PTB9);
 BusOut ledRR( PTE4, PTE5);
-AnalogIn sensorR( PTB1);
+AnalogIn sensorR( PTC2);
 AnalogIn sensorL( PTB3);
 AnalogIn sensorCR( PTB0);
 AnalogIn sensorCL( PTB2);
@@ -28,15 +30,15 @@
 PwmOut Mrightp(PTA12);
 
 float whiteCL[2], whiteCR[2], grayCL[2], grayCR[2]; //[1]が初期設定の基準値，[0]が変動する閾値
-float grayL[2], grayR[2];
-float blackCL = 0.18;   //閾値の跡地
-float blackCR = 0.3;   //↑と同じ
-float whiteL = 0.35, blackL = 0.23;
-float whiteR = 0.35, blackR = 0.23;
+float whiteL[2], whiteR[2], grayL[2], grayR[2];
+float blackCL = 0.18, blackCR = 0.1;   //閾値の跡地
+float blackL = 0.028, blackR = 0.028;
+float stepL = 0.45, stepR = 0.38;
 float sensor[4];    //sensor[0]:sensorL ... sensor[3]:sensorR
 float pr, pl;
 
 void stop_point_ver2();
+void motor_check();
 
 void set_threshold(){   //閾値の初期設定
     for( int i = 0; i < 2; i++){
@@ -44,6 +46,8 @@
         whiteCR[i] = WHITE_CR;
         grayCL[i] = GRAY_CL;
         grayCR[i] = GRAY_CR;
+        whiteL[i] = WHITE_L;
+        whiteR[i] = WHITE_R;
         grayL[i] = GRAY_L;
         grayR[i] = GRAY_R;
     }
@@ -51,11 +55,11 @@
 
 void go_straight_p(){   //P制御でトレース
     Mrighti = 2;
-    Mrightp = (KP * pr) * 1.0f;
+    Mrightp = (KP * pr - 0.2) * 1.0f;
     Mlefti = 2;
-    Mleftp = (KP * pl) * 1.0f;
-    ledR = 1;
-    ledL = 1;
+    Mleftp = (KP * pl - 0.2) * 1.0f;
+    ledRR = 0b10;
+    ledLL = 0b01;
 }
 
 void go_straight_CR(){  //CRのみでトレース
@@ -80,44 +84,54 @@
     Mrighti = 1;
     Mrightp = 0.05f;
     Mlefti = 2;
-    Mleftp = 0.1f;
-    ledR = 1;
-    ledL = 0;
+    Mleftp = 0.5f;
+    ledRR = 0b10;
+    ledLL = 0;
 }
 
 void turn_left(){
     Mrighti = 2;
-    Mrightp = 0.1f;
+    Mrightp = 0.5f;
     Mlefti = 1;
     Mleftp = 0.05f;
-    ledR = 0;
-    ledL = 1;
+    ledRR = 0;
+    ledLL = 0b01;
 }
 
 void turn_right_ver2(){
-    while( sensor[0] > grayL[0]){   //sensorL > grayL[0] 
-        sensor[0] = sensorL.read();
+    while( sensor[2] > blackCR){
+        sensor[1] = sensorCL.read();
+        sensor[2] = sensorCR.read();
+        sensor[3] = sensorR.read();
+        go_straight_p();
+    }
+    motor_check();
+    wait(0.2);
+    while( sensor[3] > blackR){   //sensorR > grayR[0]
+        sensor[3] = sensorR.read();
         turn_right();
     }
     stop_point_ver2();
-    while( sensor[2] > grayCR[0]){  //補正
-        sensor[2] = sensorCR.read();
-        turn_left();
-    }
-    stop_point_ver2();
+    turn_left();
+    wait(0.3);
 }
 
 void turn_left_ver2(){
-    while( sensor[3] > grayR[0]){   //sensorR > grayR[0]
-        sensor[3] = sensorR.read();
+    while( sensor[1] > blackCL){
+        sensor[0] = sensorL.read();
+        sensor[1] = sensorCL.read();
+        sensor[2] = sensorCR.read();
+        go_straight_p();
+    }
+    motor_check();
+    wait(0.2);
+    while( sensor[0] > blackL){   //sensorL > grayL[0]
+        sensor[0] = sensorL.read();
         turn_left();
     }
     stop_point_ver2();
-    while( sensor[1] > grayCL[0]){  //補正
-        sensor[1] = sensorCL.read();
-        turn_right();
-    }
-    stop_point_ver2();
+    turn_right();
+    wait(0.3);
 }
 
 void motor_check(){   //モータドライバの調子の確認用
@@ -150,7 +164,7 @@
     else if( Mlefti == 2)
         Mlefti = 1;
     Mleftp = 1.0f;
-    wait(0.05);
+    wait(0.1);
     Mrighti = 0;
     Mlefti = 0;
     ledRR = 0b11;
@@ -164,7 +178,7 @@
         grayCL[0] = grayCL[1] * ( 1 + CHG * (whiteCR[1] - whiteCR[0]) / whiteCR[1]);
         grayR[0] = grayR[1] * ( 1 + CHG * (whiteCR[1] - whiteCR[0]) / whiteCR[1]);
         grayL[0] = grayL[1] * ( 1 + CHG * (whiteCR[1] - whiteCR[0]) / whiteCR[1]);
-        ledRR = 0b10;
+        ledRR = 0b01;
         ledLL = 0;
     }
     else if( sensor[2] <= grayCR[0]){   //sensorCR <= grayCR
@@ -178,11 +192,77 @@
     }
 }
 
+void change_gray_ver2(int i){
+    if( i == 0){
+        whiteR[0] = sensorR.read();
+        grayCR[0] = grayCR[1] * ( 1 + CHG * (whiteR[1] - whiteR[0]) / whiteR[1]);
+        grayCL[0] = grayCL[1] * ( 1 + CHG * (whiteR[1] - whiteR[0]) / whiteR[1]);
+        grayR[0] = grayR[1] * ( 1 + CHG * (whiteR[1] - whiteR[0]) / whiteR[1]);
+        grayL[0] = grayL[1] * ( 1 + CHG * (whiteR[1] - whiteR[0]) / whiteR[1]);
+        ledRR = 0b01;
+        ledLL = 0;
+    }
+    else {
+        whiteL[0] = sensorL.read();
+        grayCR[0] = grayCR[1] * ( 1 + CHG * (whiteL[1] - whiteL[0]) / whiteL[1]);
+        grayCL[0] = grayCL[1] * ( 1 + CHG * (whiteL[1] - whiteL[0]) / whiteL[1]);
+        grayR[0] = grayR[1] * ( 1 + CHG * (whiteL[1] - whiteL[0]) / whiteL[1]);
+        grayL[0] = grayL[1] * ( 1 + CHG * (whiteL[1] - whiteL[0]) / whiteL[1]);
+        ledRR = 0;
+        ledLL = 0b01;
+    }
+}
+
+void go_step(){
+    if( sensor[0] > stepL){
+        motor_check();
+        wait(0.2);
+        while( sensor[3] < stepR){
+            sensor[0] = sensorL.read();
+            sensor[2] = sensorCR.read();
+            sensor[3] = sensorR.read();
+            pr = (sensor[2] - grayCR[0]) / (whiteCR[0] - grayCR[0]);
+            go_straight_CR();
+            if( sensor[0] < stepL)
+                stop_point_ver2();
+        }
+    } else if( sensor[3] > stepR){
+        motor_check();
+        wait(0.2);
+        while( sensor[0] < stepL){
+            sensor[0] = sensorL.read();
+            sensor[1] = sensorCL.read();
+            pl = (sensor[1] - grayCL[0]) / (whiteCL[0] - grayCL[0]);
+            go_straight_CL();
+        }
+        stop_point_ver2();
+    } else{
+        sensor[0] = sensorL.read();
+        sensor[1] = sensorCL.read();
+        sensor[2] = sensorCR.read();
+        sensor[3] = sensorR.read();
+        pl = (sensor[1] - grayCL[0]) / (whiteCL[0] - grayCL[0]);
+        pr = (sensor[2] - grayCR[0]) / (whiteCR[0] - grayCR[0]);
+        go_straight_p();
+    }
+        
+}
+
 int main() {
+//    int i = 1;
     ledC = 1;
     set_threshold();
     motor_check();
-    wait(5.0);
+    wait(0.2);
+/*    while( 1){
+        sensor[0] = sensorL.read();
+        sensor[1] = sensorCL.read();
+        sensor[2] = sensorCR.read();
+        sensor[3] = sensorR.read();
+        go_straight_p();
+        if( sensor[3] > stepR)
+            break;
+    }*/
     while(1) {
         sensor[0] = sensorL.read();
         sensor[1] = sensorCL.read();
@@ -192,24 +272,25 @@
         pl = (sensor[1] - grayCL[0]) / (whiteCL[0] - grayCL[0]);
         pr = (sensor[2] - grayCR[0]) / (whiteCR[0] - grayCR[0]);
         
-        if( sensor[0] <= grayL[0]){
+//        change_gray_ver2( i);
+/*        if( sensor[0] <= blackL){
             turn_left_ver2();
-            change_gray();
+//            change_gray();
             #ifdef DEBUG
                 break;
             #endif
-        } else if( sensor[3] <= grayR[0]){
+        } else if( sensor[3] <= blackR){
             turn_right_ver2();
-            change_gray();
+//            change_gray();
             #ifdef DEBUG
                 break;
             #endif
-        } else
-            go_straight_p();
+        } else*/
+//            go_straight_p();
 //          go_straight_CR();
 //          go_straight_CL();
-        
-        if( sensor[0] < blackL && sensor[3] < blackR){
+        go_step();
+        if( sensor[0] < grayL[0] && sensor[3] < grayR[0]){
             stop_point_ver2();
             break;
         }