Diff: sensor.cpp

Revision:

0:e9af471e2a2b

diff -r 000000000000 -r e9af471e2a2b sensor.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sensor.cpp	Thu Nov 16 06:15:58 2017 +0000
@@ -0,0 +1,131 @@
+#include "sensor.h"
+#define threshold1 0.005
+#define threshold2 0.005
+/*
+AnalogIn sensorfl(PTB0);
+AnalogIn sensorfr(PTB1);
+AnalogIn sensorbl(PTB2);
+AnalogIn sensorbr(PTB3);
+*/
+AnalogIn sensor1(PTB0);  //左から数えて1,2,3,4
+AnalogIn sensor2(PTB1);
+AnalogIn sensor3(PTB2);
+AnalogIn sensor4(PTB3);
+
+DigitalIn sensorl(PTD6);
+DigitalIn sensorr(PTD7);
+
+Timer t;
+
+static float th1 = 0.0;
+static float th2 = 0.0;
+static float th3 = 0.0;
+static float th4 = 0.0;
+
+void sensorInit(){
+    float th1max = sensor1;
+    float th1min = sensor1;
+    float th2max = sensor2;
+    float th2min = sensor2;
+    float th3max = sensor3;
+    float th3min = sensor3;
+    float th4max = sensor4;
+    float th4min = sensor4;
+    handle.pulsewidth(0.001);
+    t.start();
+    while(1){
+        if(t.read() > 1.0){
+            break;
+        }
+        if(th1max < sensor1){
+            th1max = sensor1;
+        }
+        if(th1min > sensor1){
+            th1min = sensor1;
+        }
+        if(th2max < sensor2){
+            th2max = sensor2;
+        }
+        if(th2min > sensor2){
+            th2min = sensor2;
+        }
+        if(th3max < sensor3){
+            th3max = sensor3;
+        }
+        if(th3min > sensor3){
+            th3min = sensor3;
+        }
+        if(th4max < sensor4){
+            th4max = sensor4;
+        }
+        if(th4min > sensor4){
+            th4min = sensor4;
+        }
+    }
+    //wait(1.0);
+    t.stop();
+    t.reset();
+    handle.pulsewidth(0.0021);
+    t.start();
+    while(1){
+        if(t.read() > 1.0){
+            break;
+        }
+        if(th1max < sensor1){
+            th1max = sensor1;
+        }
+        if(th1min > sensor1){
+            th1min = sensor1;
+        }
+        if(th2max < sensor2){
+            th2max = sensor2;
+        }
+        if(th2min > sensor2){
+            th2min = sensor2;
+        }
+        if(th3max < sensor3){
+            th3max = sensor3;
+        }
+        if(th3min > sensor3){
+            th3min = sensor3;
+        }
+        if(th4max < sensor4){
+            th4max = sensor4;
+        }
+        if(th4min > sensor4){
+            th4min = sensor4;
+        }
+    }
+    th1 = (th1max + th1min) / 2.0;
+    th2 = (th2max + th2min) / 2.0;
+    th3 = (th3max + th3min) / 2.0;
+    th4 = (th4max + th4min) / 2.0;
+    t.stop();
+    //wait(1.0);
+    handle.pulsewidth(0.00155);
+    wait(0.5);
+}
+
+int getReaction(){
+    int tmp = 0;
+    if(sensor1 < th1){
+        tmp += 0b100000;
+    }
+    
+    if(sensor2 < th2){
+        //tmp += 0b010000;
+        tmp += 0b000000;
+    }
+    if(sensor3 < th3){
+        tmp += 0b000000;
+        //死んでる
+    }
+    
+    if(sensor4 < th4){
+        tmp += 0b000100;
+    }
+    
+    tmp += 0b000010 * sensorl;
+    tmp += 0b000001 * sensorr;
+    return tmp;
+}
\ No newline at end of file