Diff: main.cpp

Revision:

4:edec6fe32ba9

Parent:

3:e8c29cd3ca22

Child:

5:30251ab9313a

--- a/main.cpp	Tue Nov 20 16:20:44 2018 +0000
+++ b/main.cpp	Tue Nov 20 18:31:33 2018 +0000
@@ -11,13 +11,14 @@
 #define GRAY_L 0.05     //初期値
 #define GRAY_R 0.05     //初期値
 
-//#define DEBUG
+//#define sensor_not_straight
+//#define graychange
 
-DigitalOut ledL( PTB8);
-DigitalOut ledR( PTE5);
-DigitalOut ledC( PTE2); //動作check
-BusOut ledLL( PTB8, PTB9);
-BusOut ledRR( PTE4, PTE5);
+DigitalOut ledCheck( PTE2); //動作check
+DigitalOut ledCount( PTB11);    //プログラムの切り替え確認
+BusOut ledCHG( PTB10, PTE3);    //change_gray用
+BusOut ledLL( PTE5, PTE4);  //ロボットから進行方向を見て左（わざと逆書き）
+BusOut ledRR( PTB8, PTB9);  //ロボットから進行方向を見て右
 AnalogIn sensorR( PTC2);
 AnalogIn sensorL( PTB3);
 AnalogIn sensorCR( PTB0);
@@ -31,7 +32,7 @@
 
 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 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
@@ -53,30 +54,41 @@
     }
 }
 
+void set_sensor(){      //センサ系
+    sensor[0] = sensorL.read();
+    sensor[1] = sensorCL.read();
+    sensor[2] = sensorCR.read();
+    sensor[3] = sensorR.read();
+}
+
 void go_straight_p(){   //P制御でトレース
+    pl = (sensor[1] - grayCL[0]) / (whiteCL[0] - grayCL[0]);
+    pr = (sensor[2] - grayCR[0]) / (whiteCR[0] - grayCR[0]);
     Mrighti = 2;
-    Mrightp = (KP * pr - 0.2) * 1.0f;
+    Mrightp = (KP * pr) * 1.0f;
     Mlefti = 2;
-    Mleftp = (KP * pl - 0.2) * 1.0f;
-    ledRR = 0b10;
+    Mleftp = (KP * pl) * 1.0f;
+    ledRR = 0b01;
     ledLL = 0b01;
 }
 
 void go_straight_CR(){  //CRのみでトレース
+    pr = (sensor[2] - grayCR[0]) / (whiteCR[0] - grayCR[0]);
     Mrighti = 2;
     Mrightp = (KP * pr + 0.3) * 1.0f;
     Mlefti = 2;
     Mleftp = (1.3 - KP * pr) * 1.0f;
     ledRR = 0b11;
-    ledLL = 0b10;
+    ledLL = 0b01;
 }
 
 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 = 0b10;
+    ledRR = 0b01;
     ledLL = 0b11;
 }
 
@@ -85,7 +97,7 @@
     Mrightp = 0.05f;
     Mlefti = 2;
     Mleftp = 0.5f;
-    ledRR = 0b10;
+    ledRR = 0b01;
     ledLL = 0;
 }
 
@@ -98,13 +110,13 @@
     ledLL = 0b01;
 }
 
-void turn_right_ver2(){
+void turn_right_corner(){
+    #ifdef sensor_not_straight
     while( sensor[2] > blackCR){
-        sensor[1] = sensorCL.read();
-        sensor[2] = sensorCR.read();
-        sensor[3] = sensorR.read();
+        set_sensor();
         go_straight_p();
     }
+    #endif
     motor_check();
     wait(0.2);
     while( sensor[3] > blackR){   //sensorR > grayR[0]
@@ -116,13 +128,13 @@
     wait(0.3);
 }
 
-void turn_left_ver2(){
+void turn_left_corner(){
+    #ifdef sensor_not_straight
     while( sensor[1] > blackCL){
-        sensor[0] = sensorL.read();
-        sensor[1] = sensorCL.read();
-        sensor[2] = sensorCR.read();
+        set_sensor();
         go_straight_p();
     }
+    #endif
     motor_check();
     wait(0.2);
     while( sensor[0] > blackL){   //sensorL > grayL[0]
@@ -134,35 +146,23 @@
     wait(0.3);
 }
 
-void motor_check(){   //モータドライバの調子の確認用
+void motor_check(){   //モータドライバの調子の確認用と単純に直進
     Mrighti = 2;
     Mrightp = 1.0f;
     Mlefti = 2;
     Mleftp = 1.0f;
 }
 
-void stop_point(){
-    Mrighti = 1;
-    Mrightp = 1.0f;
-    Mlefti = 1;
-    Mleftp = 1.0f;
-    wait(0.05);
-    Mrighti = 0;
-    Mlefti = 0;
-    ledRR = 0b11;
-    ledLL = 0b11;
-}
-
 void stop_point_ver2(){
     if( Mrighti == 1)
         Mrighti = 2;
     else if( Mrighti == 2)
         Mrighti = 1;
-    Mrightp = 1.0f;
     if( Mlefti == 1)
         Mlefti = 2;
     else if( Mlefti == 2)
         Mlefti = 1;
+    Mrightp = 1.0f;
     Mleftp = 1.0f;
     wait(0.1);
     Mrighti = 0;
@@ -188,115 +188,166 @@
         grayR[0] = grayR[1] * ( 1 + CHG * (whiteCL[1] - whiteCL[0]) / whiteCL[1]);
         grayL[0] = grayL[1] * ( 1 + CHG * (whiteCL[1] - whiteCL[0]) / whiteCL[1]);
         ledRR = 0;
-        ledLL = 0b10;
+        ledLL = 0b01;
     }
 }
 
 void change_gray_ver2(int i){
-    if( i == 0){
+    if( i == 0){        //Rが白
         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;
+        ledCHG = 0b01;
+        wait( 0.1);
+        ledCHG = 0;
     }
-    else {
+    else {      //Lが白
         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;
+        ledCHG = 0b10;
+        wait( 0.1);
+        ledCHG = 0;
     }
 }
 
 void go_step(){
-    if( sensor[0] > stepL){
+    if( sensor[0] > stepL){     //左側に段差がある場合
+        #ifdef sensor_not_straight
         motor_check();
         wait(0.2);
+        #endif
         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]);
+            set_sensor();
             go_straight_CR();
-            if( sensor[0] < stepL)
-                stop_point_ver2();
         }
-    } else if( sensor[3] > stepR){
+    } else if( sensor[3] > stepR){      //右側に段差がある場合
+        #ifdef sensor_not_straight
         motor_check();
         wait(0.2);
+        #endif
         while( sensor[0] < stepL){
-            sensor[0] = sensorL.read();
-            sensor[1] = sensorCL.read();
-            pl = (sensor[1] - grayCL[0]) / (whiteCL[0] - grayCL[0]);
+            set_sensor();
             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();
+    } else
+        go_straight_p();        
+}
+
+void LED_blinky(){      //プログラムの切り替わり確認用
+    for( int i = 0; i < 4; i++){
+        ledCount = !ledCount;
+        wait(0.2);
     }
-        
 }
 
 int main() {
-//    int i = 1;
-    ledC = 1;
+    #ifdef graychange
+    int i = 1;      //i == 0ならRがwhite，i == 1ならLがwhite
+    #endif
+    ledCheck = 1;
     set_threshold();
+    LED_blinky();
     motor_check();
     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)
+    while(1){      //スタート～最初の左折カーブ
+        set_sensor();
+        #ifdef graychange
+        change_gray_ver2(i);
+        #endif
+        if( sensor[3] <= blackR){
+            turn_right_corner();
+            break;
+        } else
+            go_straight_p();
+    }
+    LED_blinky();
+    while(1){       //左折カーブ～段差～左折カーブ
+        set_sensor();
+        if( sensor[0] <= blackL){
+            turn_left_corner();
+            break;
+        } else
+            go_step();
+    }
+    LED_blinky();
+    #ifdef graychange
+    i = 0;
+    #endif
+    while(1){       //左折カーブ～左折カーブ
+        set_sensor();
+        #ifdef graychange
+        change_gray_ver2(i);
+        #endif
+        if( sensor[0] <= blackL){
+            turn_left_corner();
+            break;
+        } else
+            go_straight_p();
+    }
+    LED_blinky();
+    while(1){       //左折カーブ～トンネル手前のマーク
+        set_sensor();
+        if( sensor[0] <= blackL && sensor[3] <= blackR)
             break;
-    }*/
-    while(1) {
-        sensor[0] = sensorL.read();
-        sensor[1] = sensorCL.read();
-        sensor[2] = sensorCR.read();
-        sensor[3] = sensorR.read();
+        else
+            go_straight_CR();
+    }
+    LED_blinky();
+    while(1){       //トンネル手前のマーク～Ｔ字
+        set_sensor();
+        if( sensor[0] <= blackL && sensor[3] <= blackR){
+            turn_right_corner();
+            break;
+        } else
+            go_straight_p();
+    }
+    LED_blinky();
+    #ifdef graychange
+    i = 0;
+    #endif
+    while(1){       //Ｔ字～半円終わり
+        set_sensor();
+        #ifdef graychange
+        change_gray_ver2(i);
+        #endif
+        if( sensor[0] <= blackL){
+            turn_left_corner();
+            break;
+        } else
+            go_straight_p();
+    }
+    LED_blinky();
+    while(1){       //半円終わり～ゴール
+        set_sensor();
+        if( sensor[0] <= blackL)
+            turn_left_corner();
+        else if( sensor[3] <= blackR)
+            turn_right_corner();
+        else if( sensor[0] <= blackL && sensor[3] <= blackR){
+            stop_point_ver2();
+            break;
+        } else
+            go_straight_p();
+    }
+    LED_blinky();
+}
 
-        pl = (sensor[1] - grayCL[0]) / (whiteCL[0] - grayCL[0]);
-        pr = (sensor[2] - grayCR[0]) / (whiteCR[0] - grayCR[0]);
-        
 //        change_gray_ver2( i);
 /*        if( sensor[0] <= blackL){
-            turn_left_ver2();
+            turn_left_corner();
 //            change_gray();
-            #ifdef DEBUG
-                break;
-            #endif
         } else if( sensor[3] <= blackR){
-            turn_right_ver2();
+            turn_right_corner();
 //            change_gray();
-            #ifdef DEBUG
-                break;
-            #endif
         } else*/
 //            go_straight_p();
 //          go_straight_CR();
 //          go_straight_CL();
-        go_step();
-        if( sensor[0] < grayL[0] && sensor[3] < grayR[0]){
-            stop_point_ver2();
-            break;
-        }
-    }
-}
-
 /*以下メモ
 ・段差
         if( sensor[0] > 閾値){