Revision 4:b81dc4282175, committed 2018-10-29

Comitter:

kamorei

Date:

Mon Oct 29 16:21:58 2018 +0000

Parent:

3:bc6c111b88da

Commit message:

10/29 ???A??? P?????????????

Changed in this revision

--- a/main.cpp	Fri Oct 26 16:54:54 2018 +0000
+++ b/main.cpp	Mon Oct 29 16:21:58 2018 +0000
@@ -1,3 +1,5 @@
+//直線だけP制御したら直線でブレなくなるかもしれないので作りました
+
 #include "mbed.h"
 
 DigitalOut ledL( PTB8);
@@ -17,40 +19,50 @@
 BusOut Mrighti(PTC0, PTC7);
 PwmOut Mrightp(PTA12);
 
-float white = 0.6, black = 0.08, gray = 0.3;   //値をぶち込む
+float white = 0.6, black = 0.08, gray = 0.1;   //値をぶち込む
 float whiteR = 0.02, blackR = 0.008, grayR = 0.015; //弱いセンサ用
 float sensor[4];    //sensor[0]:sensorL ... sensor[3]:sensorR
+float kp = 0.85; //Pゲイン
+float pr, pl;
 
-void turn_right(){
-    Mlefti = 2;
-    Mleftp = 0.7f;
-    Mrighti = 1;
-    Mrightp = 0.7f;
-    ledR = 1;
-    ledL = 0;
+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 turn_left(){ 
+void go_straight_check(){   //モータドライバの調子の確認用
     Mrighti = 2;
-    Mrightp = 1.0f;
-    Mlefti = 1;
-    Mleftp = 0.5f;    
-    ledR = 0;
-    ledL = 1;
-}
-
-void go_straight(){
-    //まっすぐ行こう
-    Mrighti = 2;
-    Mrightp = 1.0f;
+    Mrightp = 0.8f;
     Mlefti = 2;
-    Mleftp = 0.7f;
-    ledR = 0;
-    ledL = 0;
+    Mleftp = 0.8f;
 }
 
 void stop_point(){
-    //停止
+    Mrighti = 1;
+    Mrightp = 0.5f;
+    Mlefti = 1;
+    Mleftp = 0.5f;
+    wait(0.05);
     Mrighti = 0;
     Mlefti = 0;
     ledRR = 2;
@@ -58,7 +70,7 @@
 }
 
 int main() {
-    go_straight();
+    go_straight_check();
     wait(0.2);    
     while(1) {
         sensor[0] = sensorL.read();
@@ -66,15 +78,13 @@
         sensor[2] = sensorCR.read();
         sensor[3] = sensorR.read();
         
-        if( sensor[1] >= white && sensor[2] <= gray)
-            turn_right();
-        else if( sensor[1] <= black && sensor[2] >= white)
-            turn_left();
-        else
-            go_straight();
+        pr = (sensor[2] - black) / (white - black);
+        pl = (sensor[1] - black) / (white - black);
+        
+        go_straight_p();
         if( sensor[0] <= black && sensor[3] <= blackR){
             stop_point();
             break;
         }
     }
-}
+}
\ No newline at end of file