Diff: main.cpp

Revision:

5:30251ab9313a

Parent:

4:edec6fe32ba9

Child:

6:28c5a5992a81

--- a/main.cpp	Tue Nov 20 18:31:33 2018 +0000
+++ b/main.cpp	Tue Nov 20 18:51:23 2018 +0000
@@ -36,7 +36,7 @@
 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;
+float pr, pl, chg;
 
 void stop_point_ver2();
 void motor_check();
@@ -171,44 +171,25 @@
     ledLL = 0b11;
 }
 
-void change_gray(){
-    if( sensor[1] <= grayCL[0]){    //sensorCL <=grayCL
-        whiteCR[0] = sensorCR.read();
-        grayCR[0] = grayCR[1] * ( 1 + CHG * (whiteCR[1] - whiteCR[0]) / whiteCR[1]);
-        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 = 0b01;
-        ledLL = 0;
-    }
-    else if( sensor[2] <= grayCR[0]){   //sensorCR <= grayCR
-        whiteCL[0] = sensorCL.read();
-        grayCR[0] = grayCR[1] * ( 1 + CHG * (whiteCL[1] - whiteCL[0]) / whiteCL[1]);
-        grayCL[0] = grayCL[1] * ( 1 + CHG * (whiteCL[1] - whiteCL[0]) / whiteCL[1]);
-        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 = 0b01;
-    }
-}
-
 void change_gray_ver2(int i){
     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]);
+        chg = (whiteR[1] - whiteR[0]) / whiteR[1];
+        grayCR[0] = grayCR[1] * ( 1 + CHG * chg);
+        grayCL[0] = grayCL[1] * ( 1 + CHG * chg);
+        grayR[0] = grayR[1] * ( 1 + CHG * chg);
+        grayL[0] = grayL[1] * ( 1 + CHG * chg);
         ledCHG = 0b01;
         wait( 0.1);
         ledCHG = 0;
     }
     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]);
+        chg = (whiteL[1] - whiteL[0]) / whiteL[1];
+        grayCR[0] = grayCR[1] * ( 1 + CHG * chg);
+        grayCL[0] = grayCL[1] * ( 1 + CHG * chg);
+        grayR[0] = grayR[1] * ( 1 + CHG * chg);
+        grayL[0] = grayL[1] * ( 1 + CHG * chg);
         ledCHG = 0b10;
         wait( 0.1);
         ledCHG = 0;
@@ -241,7 +222,7 @@
 void LED_blinky(){      //プログラムの切り替わり確認用
     for( int i = 0; i < 4; i++){
         ledCount = !ledCount;
-        wait(0.2);
+        wait(0.05);
     }
 }
 
@@ -335,32 +316,4 @@
             go_straight_p();
     }
     LED_blinky();
-}
-
-//        change_gray_ver2( i);
-/*        if( sensor[0] <= blackL){
-            turn_left_corner();
-//            change_gray();
-        } else if( sensor[3] <= blackR){
-            turn_right_corner();
-//            change_gray();
-        } else*/
-//            go_straight_p();
-//          go_straight_CR();
-//          go_straight_CL();
-/*以下メモ
-・段差
-        if( sensor[0] > 閾値){
-            wait(0.5);  //片輪上がるまで待ちたい
-            sensor[2] = read.sensorCR;
-            go_straight_CR();
-        } else if( sensor[3] > 閾値){
-            wait(0.5)   //片輪上がるまで待ちたい
-            sensor[1] = read.sensorCL;
-            go_straight_CL();
-        }
-
-・プログラムの切り替え
-回転動作をしたら（完了したら）カウント
-⇒回転を一発で決める必要有
-*/
+}
\ No newline at end of file