Diff: main.cpp

Revision:

0:0abe8038e775

Child:

1:5a97a5e4ee44

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Oct 19 18:08:13 2018 +0000
@@ -0,0 +1,80 @@
+#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.5, black = 0.08, gray = 0.15;   //値をぶち込む
+float whiteR = 0.05, blackR = 0.008, grayR = 0.015; //弱いセンサ用
+float sensor[4];    //sensor[0]:sensorL ... sensor[3]:sensorR
+
+void turn_right(){
+    //左を正転，右を停止
+    Mlefti = 2;
+    Mleftp = 0.5f;
+    Mrighti = 1;
+    Mrightp = 0.5f;
+    ledR = 1;
+    ledL = 0;
+}
+
+void turn_left(){
+    //右を正転，左を停止 
+    Mrighti = 2;
+    Mrightp = 0.5f;
+    Mlefti = 1;
+    Mleftp = 0.5f;    
+    ledR = 0;
+    ledL = 1;
+}
+
+void go_straight(){
+    //まっすぐ行こう
+    Mrighti = 2;
+    Mrightp = 0.5f;
+    Mlefti = 2;
+    Mleftp = 0.5f;
+    ledR = 0;
+    ledL = 0;
+}
+
+void stop_point(){
+    //停止
+    Mrighti = 0;
+    Mlefti = 0;
+    ledRR = 2;
+    ledLL = 2;
+}
+
+int main() {
+    go_straight();
+    wait(3);    
+    while(1) {
+        sensor[0] = sensorL.read();
+        sensor[1] = sensorCL.read();
+        sensor[2] = sensorCR.read();
+        sensor[3] = sensorR.read();
+        
+        if( sensor[1] >= white && sensor[2] <= gray)
+            turn_right();
+        else if( sensor[1] <= gray && sensor[2] >= white)
+            turn_left();
+        else if( sensor[1] <= gray && sensor[2] <= gray)
+            go_straight();
+        //if( sensor[0] <= gray && sensor[3] <= grayR)
+        //    stop_point();
+    }
+}