Diff: ReadSensor.cpp

Revision:

0:e38b500d6e74

Child:

1:4808f55970e8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ReadSensor.cpp	Thu Apr 19 11:31:49 2018 +0000
@@ -0,0 +1,145 @@
+#include "mbed.h"
+#include "HCSR04.h"
+#include "AutomationElements.h"
+
+InterruptIn EchoL(PA_0);
+InterruptIn EchoF(PA_1);
+InterruptIn EchoR(PA_4);
+
+DigitalOut TriggerL(PB_3);
+DigitalOut TriggerF(PB_4);
+DigitalOut TriggerR(PB_5);
+
+HCSR04 sensor_Left(EchoL, TriggerL);
+HCSR04 sensor_Front(EchoF, TriggerF);
+HCSR04 sensor_Right(EchoR, TriggerR);
+
+//PwmOut& pwmLeft
+
+//float sampleTime = 0.5f;
+//PT1 filter(1, 2, sampleTime);
+//Ticker ticker_sens;
+float distance;
+float filteredDistance;
+int Glob_Position;
+
+void calc()
+{
+    switch(Glob_Position) {
+        case 1:
+            sensor_Left.startMeasurement();
+            break;
+
+        case 2:
+            sensor_Front.startMeasurement();
+            break;
+
+        case 3:
+            sensor_Right.startMeasurement();
+            break;
+
+        default:
+            break;
+    }
+}
+
+int ReadSensor(int Position)
+{
+    Glob_Position = Position;
+    //PT1 filter(1, 2, sampleTime);
+
+    sensor_Left.setRanges(2, 400);
+    sensor_Front.setRanges(2,400);
+    sensor_Right.setRanges(2, 400);
+    //ticker_sens.attach(callback(this, &ReadSensor::calc, sampleTime));
+    while(true) {
+        switch(Position) {
+            case 1:
+                while(!sensor_Left.isNewDataReady()) {
+                    // wait for new data
+                }
+                distance = sensor_Left.getDistance_cm();
+                break;
+
+            case 2:
+                while(!sensor_Front.isNewDataReady()) {
+                    // wait for new data
+                }
+                distance = sensor_Front.getDistance_cm();
+                break;
+
+            case 3:
+                while(!sensor_Right.isNewDataReady()) {
+                    // wait for new data
+                }
+                distance = sensor_Right.getDistance_cm();
+                break;
+
+            default:
+                return true;
+                break;
+        }
+
+        if(distance <= 9.00) {
+            //printf("t%f\n",filteredDistance);
+            printf("t%f\n",distance);
+            return 1;
+
+        } else {
+            //printf("f%f\n",filteredDistance);
+            printf("t%f\n",distance);
+            return 0;
+
+        }
+
+    }
+}
+
+float ReadSensorValue(int Position)
+{
+    Glob_Position = Position;
+    float distancetot = 0;
+    int i = 0;
+
+    sensor_Left.setRanges(2, 400);
+    sensor_Front.setRanges(2,400);
+    sensor_Right.setRanges(2, 400);
+    while(true) {
+        for(i = 0; i < 3; i++) {
+            switch(Position) {
+                case 1:
+                    while(!sensor_Left.isNewDataReady()) {
+                        // wait for new data
+                    }
+                    distance = sensor_Left.getDistance_cm();
+                    break;
+
+                case 2:
+                    while(!sensor_Front.isNewDataReady()) {
+                        // wait for new data
+                    }
+                    distance = sensor_Front.getDistance_cm();
+                    break;
+
+                case 3:
+                    while(!sensor_Right.isNewDataReady()) {
+                        // wait for new data
+                    }
+                    distance = sensor_Right.getDistance_cm();
+                    break;
+
+                default:
+                    return true;
+                    break;
+            }
+            distancetot = distancetot + distance;
+        }
+        //ticker_sens.detach();
+        distancetot = distancetot / i;
+        return distancetot;
+        //filter.in(distance);
+        //filteredDistance = filter.out();
+
+
+    }
+}
\ No newline at end of file