Diff: main.cpp

Revision:

9:d989333191cb

Parent:

8:b47cebb6aeb8

Child:

10:80bac0698b04

--- a/main.cpp	Wed Nov 21 06:57:40 2018 +0000
+++ b/main.cpp	Wed Nov 21 07:10:01 2018 +0000
@@ -1,17 +1,8 @@
 #include "mbed.h"
 
 #define KP 5.8  //Pゲイン
-#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 graychange
+//#define blinky
 
 DigitalOut ledCheck( PTE2); //動作check
 DigitalOut ledCount( PTB11);    //プログラムの切り替え確認
@@ -29,30 +20,16 @@
 BusOut Mrighti(PTC0, PTC7);
 PwmOut Mrightp(PTA12);
 
-float whiteCL[2], whiteCR[2], grayCL[2], grayCR[2]; //[1]が初期設定の基準値，[0]が変動する閾値
-float whiteL[2], whiteR[2], grayL[2], grayR[2];
-float blackCL = 0.03, blackCR = 0.02;   //閾値
-float blackL = 0.018, blackR = 0.018;
-float stepL = 0.45, stepR = 0.38;
+float whiteCL = 0.27, 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 sensor[4];    //sensor[0]:sensorL ... sensor[3]:sensorR
-float pr, pl, chg;
+float pr, pl;
 
 void stop_point_ver2();
 void motor_check();
 
-void set_threshold(){   //閾値の初期設定
-    for( int i = 0; i < 2; i++){
-        whiteCL[i] = WHITE_CL;
-        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;
-    }
-}
-
 void set_sensor(){      //センサ系
     sensor[0] = sensorL.read();
     sensor[1] = sensorCL.read();
@@ -61,16 +38,16 @@
 }
 
 void go_straight_p(){   //P制御でトレース
-    pl = (sensor[1] - grayCL[0]) / (whiteCL[0] - grayCL[0]);
-    pr = (sensor[2] - grayCR[0]) / (whiteCR[0] - grayCR[0]);
+    pl = (sensor[1] - grayCL) / (whiteCL - grayCL);
+    pr = (sensor[2] - grayCR) / (whiteCR - grayCR);
     Mrighti = 2;
     if( pl >= 0.0)
-        Mrightp = (KP * pr) * 1.0f;
+        Mrightp = (KP * pr + 0.3) * 1.0f;
     else
         Mrightp = 1.0f;
     Mlefti = 2;
     if( pr >= 0.0)
-        Mleftp = (KP * pl) * 1.0f;
+        Mleftp = (KP * pl + 0.3) * 1.0f;
     else
         Mleftp = 1.0f;
     ledRR = 0b01;
@@ -78,11 +55,11 @@
 }
 
 void go_straight_CR(){  //CRのみでトレース
-    pr = (sensor[2] - grayCR[0]) / (whiteCR[0] - grayCR[0]);
+    pr = (sensor[2] - grayCR) / (whiteCR - grayCR);
     Mrighti = 2;
-    Mrightp = (KP * pr) * 1.0f;
+    Mrightp = (KP * pr + 0.5) * 1.0f;
     Mlefti = 2;
-    Mleftp = (1.0 - KP * pr) * 1.0f;
+    Mleftp = (1.5 - KP * pr) * 1.0f;
     ledRR = 0b11;
     ledLL = 0;
 }
@@ -109,7 +86,7 @@
     motor_check();
     wait(f);
     sensor[3] = sensorR.read();
-    while( sensor[3] >= blackR){   //sensorR > grayR[0]
+    while( sensor[3] >= blackR){
         sensor[3] = sensorR.read();
         turn_right();
     }
@@ -122,7 +99,7 @@
     motor_check();
     wait(f);
     sensor[0] = sensorL.read();
-    while( sensor[0] >= blackL){   //sensorL > grayL[0]
+    while( sensor[0] >= blackL){
         sensor[0] = sensorL.read();
         turn_left();
     }
@@ -156,31 +133,6 @@
     ledLL = 0b11;
 }
 
-void change_gray_ver2(int i){
-    if( i == 0){        //Rが白
-        whiteR[0] = sensorR.read();
-        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();
-        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;
-    }
-}
-
 void LED_blinky(){      //プログラムの切り替わり確認用
     for( int i = 0; i < 4; i++){
         ledCount = !ledCount;
@@ -189,24 +141,19 @@
 }
 
 int main() {
-    #ifdef graychange
-    int i = 1;      //i == 0ならRがwhite，i == 1ならLがwhite
-    #endif
     ledCheck = 1;
-    set_threshold();
     LED_blinky();
     while(1){      //スタート～最初の左折カーブ
         set_sensor();
-        #ifdef graychange
-        change_gray_ver2(i);
-        #endif
         if( sensor[3] <= blackR){
             turn_right_corner( 0.2);
             break;
         } else
             go_straight_p();
     }
+    #ifdef blinky
     LED_blinky();
+    #endif
     while(1){       //左折カーブ～段差～左折カーブ
         set_sensor();
         if( sensor[0] <= blackL){
@@ -215,30 +162,30 @@
         } else
             go_straight_p();
     }
+    #ifdef blinky
     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( 0.2);
             break;
         } else
             go_straight_p();
     }
+    #ifdef blinky
     LED_blinky();
+    #endif
     while(1){       //左折カーブ～トンネル手前のマーク
         set_sensor();
-        if( sensor[0] <= grayL[0] && sensor[3] <= grayR[0])
+        if( sensor[0] <= grayL && sensor[3] <= grayR)
             break;
         else
             go_straight_CR();
     }
+    #ifdef blinky
     LED_blinky();
+    #endif
     while(1){       //トンネル手前のマーク～Ｔ字
         set_sensor();
         if( sensor[0] <= blackL && sensor[3] <= blackR){
@@ -247,33 +194,33 @@
         } else
             go_straight_p();
     }
+    #ifdef blinky
     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( 0.5);
             break;
         } else
             go_straight_CR();
     }
+    #ifdef blinky
     LED_blinky();
+    #endif
     while(1){       //鋭角カーブ終わり～ゴール
         set_sensor();
         if( sensor[0] <= blackL)
             turn_left_corner( 0.2);
         else if( sensor[3] <= blackR)
             turn_right_corner( 0.2);
-        else if( sensor[0] <= grayL[0] && sensor[3] <= grayR[0]){
+        else if( sensor[0] <= grayL && sensor[3] <= grayR){
             stop_point_ver2();
             break;
         } else
             go_straight_p();
     }
+    #ifdef blinky
     LED_blinky();
+    #endif
 }
\ No newline at end of file