Diff: source/IRSensor.cpp

Revision:

117:66d64dbd1b36

Parent:

113:c7afe49752b9

--- a/source/IRSensor.cpp	Wed May 10 08:52:04 2017 +0000
+++ b/source/IRSensor.cpp	Wed May 10 10:02:53 2017 +0000
@@ -39,17 +39,20 @@
     this->number = number;
 }
 
+
 /**
  * Deletes the IRSensor object.
  */
 IRSensor::~IRSensor() {}
 
 /**
-read voltage of all sensor for Lowpassfilter
-*/
-void IRSensor::voltage()
+ * Gets the distance measured with the IR sensor in [m].
+ * @return the distance, given in [m].
+ */
+float IRSensor::read()
 {
-
+    float avarage = 0;
+    static float mean[6] = {0};
     if(number == 3) {
         *bit0 = 0;
         *bit1 = 0;
@@ -60,57 +63,40 @@
         *bit2 = (number >> 2) & 1;
     }
 
+    float d = 0;
     float voltage = 0;
-    if(number == 4) {
-        voltage = distance2->read();
-    } else {
-        voltage = distance->read();
+    for(int i = 0; i < cycles; i++) {
+        if(number == 4) {
+            voltage = distance2->read();
+        } else {
+            voltage = distance->read();
+        }
+        if(voltage < 0.1f) voltage = 0.1;   // set voltage if not big enough
+        switch (number) {
+            case 0:
+                d  = 0.18f/pow((1.5f*voltage-0.1f),0.7f)-0.095f;
+                break;
+            case 1:
+                d  = 0.18f/pow((1.5f*voltage-0.1f),0.7f)-0.095f;
+                break;
+            case 2:
+                d  = 0.095f/pow((0.94f*voltage-0.05f),0.8f)-0.04f;
+                break;
+            case 3:
+                d  = 0.192f/pow((1.8f*voltage-0.13f),0.7f)-0.095f;
+                break;
+            case 4:
+                d  = 0.14f/pow((1.2f*voltage-0.09f),0.7f)-0.088f;
+                break;
+            case 5:
+                d  = 0.18f/pow((1.55f*voltage-0.12f),0.7f)-0.095f;
+                break;
+            default:
+                break;
+        }
+        avarage += d;
     }
-    
-    if (voltage < 0.1f) voltage = 0.1f;
-    if (voltage > 1.0f) voltage = 1.0f;
-        
-    
-    mean = 0.6f*mean + 0.4f*voltage;
-}
-
-
-/**
- * Gets the distance measured with the IR sensor in [m].
- * @return the distance, given in [m].
- */
-
-float IRSensor::read()
-{
-    float d = 0;
-    switch (number) {
-        case 0:
-            d  = 0.18f/pow((1.5f*mean-0.1f),0.7f)-0.095f;
-            break;
-        case 1:
-            d  = 0.18f/pow((1.5f*mean-0.1f),0.7f)-0.095f;
-            break;
-        case 2:
-            d  = 0.095f/pow((0.94f*mean-0.05f),0.8f)-0.04f;
-            break;
-        case 3:
-            d  = 0.192f/pow((1.8f*mean-0.13f),0.7f)-0.095f;
-            break;
-        case 4:
-            d  = 0.14f/pow((1.2f*mean-0.09f),0.7f)-0.088f;
-            break;
-        case 5:
-            d  = 0.18f/pow((1.55f*mean-0.12f),0.7f)-0.095f;
-            break;
-        default:
-            break;
-    }
-    /*
-    if (d > 1.0f || d < 0) {
-        d = 0.99f;
-    }*/
-        
-    return d;
+    return (avarage / cycles);
 }