Diff: main.cpp

Revision:

1:905436937f78

Parent:

0:8b8f5dba70e9

Child:

2:8748af75523f

--- a/main.cpp	Fri Oct 26 18:28:37 2018 +0000
+++ b/main.cpp	Tue Oct 30 05:08:42 2018 +0000
@@ -1,4 +1,10 @@
-//直線だけP制御したら直線でブレなくなるかもしれないので作りました
+/*
+タームB作戦（プログラムの流れ）
+・AからスタートしてB（トンネル手前）で一旦停止
+・閾値の再定義（トンネル内の明るさに）←あらかじめ用意しておいて切り替えてもいい?
+・トンネル抜けた地点で再定義（ゴール判定用が急激に変化したら再定義か？）
+・あとは流れでいいかな？
+*/
 
 #include "mbed.h"
 
@@ -22,7 +28,7 @@
 float white = 0.6, black = 0.08, gray = 0.3;   //値をぶち込む
 float whiteR = 0.02, blackR = 0.008, grayR = 0.015; //弱いセンサ用
 float sensor[4];    //sensor[0]:sensorL ... sensor[3]:sensorR
-float kp = 0.5; //Pゲイン
+float kp = 0.3; //Pゲイン
 float pr, pl;
 int i = 0, j = 0;
 
@@ -44,7 +50,7 @@
     ledL = 1;
 }
 
-void go_straight( float r, float l){
+void go_straight(){
     Mrighti = 2;
     Mrightp = (kp * pr + 0.5) * 1.0f;
     Mlefti = 2;
@@ -53,7 +59,39 @@
     ledL = 0;
 }
 
-void go_back( float r, float l){
+void go_straight_p(){
+    if( pr < (gray - black) / (white - black)){
+        Mrighti = 1;
+        Mrightp = (kp * pr + 0.3) * 1.0f;
+        Mlefti = 2;
+        Mleftp = (kp * pl + 0.3) * 1.0f;
+        ledR = 1;
+        ledL = 0;
+    } else if( pl < (gray - black) / (white - black)){
+        Mrighti = 2;
+        Mrightp = (kp * pr + 0.3) * 1.0f;
+        Mlefti = 1;
+        Mleftp = (kp * pl + 0.3) * 1.0f;
+        ledR = 0;
+        ledL = 1;
+    } else{
+        Mrighti = 2;
+        Mrightp = (kp * pr + 0.3) * 1.0f;
+        Mlefti = 2;
+        Mleftp = (kp * pl - 0.1) * 1.0f;
+        ledR = 0;
+        ledL = 0;
+    }
+}
+
+void go_straight_check(){   //モータドライバの調子の確認用
+    Mrighti = 2;
+    Mrightp = 0.8f;
+    Mlefti = 2;
+    Mleftp = 0.8f;
+}
+
+void go_back(){
     Mrighti = 1;
     Mrightp = (kp * pr + 0.5) * 1.0f;
     Mlefti = 1;
@@ -63,6 +101,11 @@
 } 
 
 void stop_point(){
+    Mrighti = 1;
+    Mrightp = 0.5f;
+    Mlefti = 1;
+    Mleftp = 0.5f;
+    wait(0.05);
     Mrighti = 0;
     Mlefti = 0;
     ledRR = 2;
@@ -70,7 +113,7 @@
 }
 
 int main() {
-    go_straight();
+    go_straight_check();
     wait(0.2); 
     while( i < 3){   
         while(1) {
@@ -82,25 +125,25 @@
             pr = (sensor[2] - black) / (white - black);
             pl = (sensor[1] - black) / (white - black);
         
-            if( sensor[2] <= gray)
+/*            if( sensor[2] <= gray)
                 turn_right();
             else if( sensor[1] <= gray)
                 turn_left();
             else if( sensor[1] <= gray && sensor[2] <= gray){
                 if(ledR == 1)
-                    turn_right;
+                    turn_right();
                 else
-                    turn_left;
+                    turn_left();
             }
-            else
-                go_straight( pr, pl);
+            else*/
+                go_straight_p();
             if( sensor[0] <= black && sensor[3] <= blackR){
                 stop_point();
                 break;
             }
         }
         i++;
-        if( i < 3){
+        if( i < 2){
 /*            while(1){
                 sensor[0] = sensorL.read();
                 sensor[1] = sensorCL.read();
@@ -115,7 +158,7 @@
                     break;
                 }
                 else
-                    go_back( pr, pl);
+                    go_back();
             }
 */
             for( j = 0; j < 2;){