Diff: main.cpp

Revision:

0:8b8f5dba70e9

Child:

1:905436937f78

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Oct 26 18:28:37 2018 +0000
@@ -0,0 +1,128 @@
+//直線だけP制御したら直線でブレなくなるかもしれないので作りました
+
+#include "mbed.h"
+
+DigitalOut ledL( PTB8);
+DigitalOut ledR( PTE5);
+BusOut ledLL( PTB8, PTB9);
+BusOut ledRR( PTE5, PTE4);  //わざと右から書いてます
+
+AnalogIn sensorR( PTB1);
+AnalogIn sensorL( PTB3);
+AnalogIn sensorCR( PTB0);
+AnalogIn sensorCL( PTB2);
+
+//モータ1
+BusOut Mlefti(PTA1, PTA2);
+PwmOut Mleftp(PTD4);
+//モータ2
+BusOut Mrighti(PTC0, PTC7);
+PwmOut Mrightp(PTA12);
+
+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 pr, pl;
+int i = 0, j = 0;
+
+void turn_right(){
+    Mlefti = 2;
+    Mleftp = 0.7f;
+    Mrighti = 1;
+    Mrightp = 0.7f;
+    ledR = 1;
+    ledL = 0;
+}
+
+void turn_left(){ 
+    Mrighti = 2;
+    Mrightp = 0.7f;
+    Mlefti = 1;
+    Mleftp = 0.7f;    
+    ledR = 0;
+    ledL = 1;
+}
+
+void go_straight( float r, float l){
+    Mrighti = 2;
+    Mrightp = (kp * pr + 0.5) * 1.0f;
+    Mlefti = 2;
+    Mleftp = (kp * pl + 0.5) * 1.0f;
+    ledR = 0;
+    ledL = 0;
+}
+
+void go_back( float r, float l){
+    Mrighti = 1;
+    Mrightp = (kp * pr + 0.5) * 1.0f;
+    Mlefti = 1;
+    Mleftp = (kp * pl + 0.5) * 1.0f;
+    ledR = 0;
+    ledL = 0;
+} 
+
+void stop_point(){
+    Mrighti = 0;
+    Mlefti = 0;
+    ledRR = 2;
+    ledLL = 2;
+}
+
+int main() {
+    go_straight();
+    wait(0.2); 
+    while( i < 3){   
+        while(1) {
+            sensor[0] = sensorL.read();
+            sensor[1] = sensorCL.read();
+            sensor[2] = sensorCR.read();
+            sensor[3] = sensorR.read();
+        
+            pr = (sensor[2] - black) / (white - black);
+            pl = (sensor[1] - black) / (white - black);
+        
+            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;
+                else
+                    turn_left;
+            }
+            else
+                go_straight( pr, pl);
+            if( sensor[0] <= black && sensor[3] <= blackR){
+                stop_point();
+                break;
+            }
+        }
+        i++;
+        if( i < 3){
+/*            while(1){
+                sensor[0] = sensorL.read();
+                sensor[1] = sensorCL.read();
+                sensor[2] = sensorCR.read();
+                sensor[3] = sensorR.read();
+                
+                pr = (sensor[2] - black) / (white - black);
+                pl = (sensor[1] - black) / (white - black);
+                
+                if( sensor[0] <= black && sensor[3] <= blackR){
+                    stop_point();
+                    break;
+                }
+                else
+                    go_back( pr, pl);
+            }
+*/
+            for( j = 0; j < 2;){
+                turn_left();
+                if( sensor[2] <= black)
+                    j++;
+            }
+        }
+    }
+}
\ No newline at end of file