Diff: main.cpp

Revision:

11:99b63117ffdd

Parent:

10:80bac0698b04

--- a/main.cpp	Wed Nov 21 07:27:49 2018 +0000
+++ b/main.cpp	Mon Nov 26 05:20:20 2018 +0000
@@ -20,14 +20,14 @@
 BusOut Mrighti(PTC0, PTC7);
 PwmOut Mrightp(PTA12);
 
-float whiteCL = 0.27, grayCL = 0.15, blackCL = 0.03;
+float whiteCL = 0.265, grayCL = 0.15, blackCL = 0.03;
 float whiteCR = 0.235, grayCR = 0.13, blackCR = 0.03;
-float whiteL = 0.1, grayL = 0.06, blackL = 0.018;
-float whiteR = 0.1, grayR = 0.06, blackR = 0.018;
+float whiteL = 0.1, grayL = 0.06, blackL = 0.013;
+float whiteR = 0.1, grayR = 0.06, blackR = 0.013;
 float sensor[4];    //sensor[0]:sensorL ... sensor[3]:sensorR
 float pr, pl;
 
-void stop_point_ver2();
+void stop_point();
 void motor_check();
 
 void set_sensor(){      //センサ系
@@ -90,7 +90,20 @@
         sensor[3] = sensorR.read();
         turn_right();
     }
-    stop_point_ver2();
+    stop_point();
+//    turn_left();
+//    wait(0.3);
+}
+
+void turn_right_corner_ver2( float f){
+    motor_check();
+    wait(f);
+    sensor[2] = sensorCR.read();
+    while( sensor[2] >= blackCR){
+        sensor[2] = sensorCR.read();
+        turn_right();
+    }
+    stop_point();
 //    turn_left();
 //    wait(0.3);
 }
@@ -103,7 +116,20 @@
         sensor[0] = sensorL.read();
         turn_left();
     }
-    stop_point_ver2();
+    stop_point();
+//    turn_right();
+//    wait(0.3);
+}
+
+void turn_left_corner_ver2( float f){
+    motor_check();
+    wait(f);
+    sensor[1] = sensorCL.read();
+    while( sensor[1] >= blackCL){
+        sensor[1] = sensorCL.read();
+        turn_left();
+    }
+    stop_point();
 //    turn_right();
 //    wait(0.3);
 }
@@ -115,7 +141,7 @@
     Mleftp = 1.0f;
 }
 
-void stop_point_ver2(){
+void stop_point(){
     if( Mrighti == 1)
         Mrighti = 2;
     else if( Mrighti == 2)
@@ -143,10 +169,10 @@
 int main() {
     ledCheck = 1;
     LED_blinky();
-    while(1){      //スタート～最初の左折カーブ
+    while(1){      //スタート～段差～最初の左折カーブ
         set_sensor();
         if( sensor[3] <= blackR){
-            turn_right_corner( 0.2);
+            turn_right_corner_ver2( 0.2);
             break;
         } else
             go_straight_p();
@@ -157,7 +183,7 @@
     while(1){       //左折カーブ～段差～左折カーブ
         set_sensor();
         if( sensor[0] <= blackL){
-            turn_left_corner( 0.2);
+            turn_left_corner_ver2( 0.2);
             break;
         } else
             go_straight_p();
@@ -168,7 +194,7 @@
     while(1){       //左折カーブ～左折カーブ
         set_sensor();
         if( sensor[0] <= blackL){
-            turn_left_corner( 0.2);
+            turn_left_corner_ver2( 0.2);
             break;
         } else
             go_straight_p();
@@ -190,7 +216,7 @@
     while(1){       //トンネル手前のマーク～Ｔ字
         set_sensor();
         if( sensor[0] <= blackL && sensor[3] <= blackR){
-            turn_right_corner( 0.2);
+            turn_right_corner_ver2( 0.2);
             break;
         } else
             go_straight_p();
@@ -201,7 +227,7 @@
     while(1){       //Ｔ字～半円～鋭角カーブ
         set_sensor();
         if( sensor[0] <= blackL){
-            turn_left_corner( 0.5);
+            turn_left_corner_ver2( 0.5);
             break;
         } else
             go_straight_CR();
@@ -212,11 +238,11 @@
     while(1){       //鋭角カーブ終わり～ゴール
         set_sensor();
         if( sensor[0] <= blackL)
-            turn_left_corner( 0.2);
+            turn_left_corner_ver2( 0.2);
         else if( sensor[3] <= blackR)
-            turn_right_corner( 0.2);
+            turn_right_corner_ver2( 0.2);
         else if( sensor[0] <= grayL && sensor[3] <= grayR){
-            stop_point_ver2();
+            stop_point();
             break;
         } else
             go_straight_p();