Revision 4:fe63b514d4ef, committed 2019-07-12

Comitter:

altb2

Date:

Fri Jul 12 15:19:53 2019 +0000

Parent:

2:89d7dd44ecfd

Commit message:

Extended to 4 Sensors which are read sequentially

Changed in this revision

--- a/RangeFinder.cpp	Fri Jun 21 08:57:37 2019 +0000
+++ b/RangeFinder.cpp	Fri Jul 12 15:19:53 2019 +0000
@@ -22,16 +22,71 @@
  
  #include "RangeFinder.h"
 
-RangeFinder::RangeFinder(PinName pin, int pulsetime, float scale, float offset, int time):
-    pio(pin), scale(scale), offset(offset), pulsetime(pulsetime), timeout(time)  {
+// Init with 1 Sensor:
+RangeFinder::RangeFinder(PinName pin0, int pulsetime, float scale, float offset, int time):
+    pio0(pin0), pio1(NC), pio2(NC), pio3(NC), scale(scale), offset(offset), pulsetime(pulsetime), timeout(time)  {
+    n_sensors = 1;
+    this->old_val[0] = 0.0;
+    current_sensor = 0;
+}
+
+// Init with 2 Sensors:
+RangeFinder::RangeFinder(PinName pin0, PinName pin1,int pulsetime, float scale, float offset, int time):
+    pio0(pin0), pio1(pin1), pio2(NC), pio3(NC), scale(scale), offset(offset), pulsetime(pulsetime), timeout(time)  {
+    n_sensors = 2;
+    for(uint8_t k=0;k<n_sensors;k++)
+        this->old_val[k] = 0.0;
+    current_sensor = 0;
+    
+}
+// Init with 3 Sensors:
+RangeFinder::RangeFinder(PinName pin0,PinName pin1, PinName pin2, int pulsetime, float scale, float offset, int time):
+    pio0(pin0), pio1(pin1), pio2(pin2), pio3(NC), scale(scale), offset(offset), pulsetime(pulsetime), timeout(time)  {
+    n_sensors = 3;
+    for(uint8_t k=0;k<n_sensors;k++)
+        this->old_val[k] = 0.0;
+    current_sensor = 0;
+}
+// Init with 4 Sensors:
+RangeFinder::RangeFinder(PinName pin0, PinName pin1, PinName pin2,PinName pin3, int pulsetime, float scale, float offset, int time):
+    pio0(pin0), pio1(pin1), pio2(pin2), pio3(pin3), scale(scale), offset(offset), pulsetime(pulsetime), timeout(time)  {
+    n_sensors = 4;
+    for(uint8_t k=0;k<n_sensors;k++)
+        this->old_val[k] = 0.0;
+    current_sensor = 0;
 }
 
 RangeFinder::~RangeFinder() {
 }
-
-float RangeFinder::read_m()  {
-    pio.write_us(1, pulsetime);
-    float t = (float) pio.read_high_us(timeout);
+void RangeFinder::read_and_filter(uint8_t i)  {
+val[i] = this->read_m(i);
+if(val[i] < 0)
+    val[i] = old_val[i];
+else{
+    old_val[i] = val[i];
+    }
+}
+// -----------------------------------------------------------------------------
+float RangeFinder::read_m(uint8_t i)  {
+    float t;
+    switch(i){
+        case 0:
+            pio0.write_us(1, pulsetime);
+            t = (float) pio0.read_high_us(timeout);
+            break;
+        case 1:
+            pio1.write_us(1, pulsetime);
+            t = (float) pio1.read_high_us(timeout);
+            break;
+        case 2:
+            pio2.write_us(1, pulsetime);
+            t = (float) pio2.read_high_us(timeout);
+            break;
+        case 3:
+            pio3.write_us(1, pulsetime);
+            t = (float) pio3.read_high_us(timeout);
+            break;
+        }
     if (t == -1.0)   {
         return -1.0;
     }

--- a/RangeFinder.h	Fri Jun 21 08:57:37 2019 +0000
+++ b/RangeFinder.h	Fri Jul 12 15:19:53 2019 +0000
@@ -37,15 +37,30 @@
         * @param timeout Time to wait for a pulse from the range finder before giving up.
         *        y = x/scale + offset
         */
-        RangeFinder(PinName pin, int pulsetime, float scale, float offset, int time);
+        RangeFinder(PinName pin0, int pulsetime, float scale, float offset, int time);      // 1 Sensor
+        RangeFinder(PinName pin0,PinName pin1, int pulsetime, float scale, float offset, int time); // 2 Sensors
+        RangeFinder(PinName pin0,PinName pin1,PinName pin2, int pulsetime, float scale, float offset, int time); // 3 Sensors
+        RangeFinder(PinName pin0,PinName pin1,PinName pin2,PinName pin3, int pulsetime, float scale, float offset, int time);   // 4 Sensors
         ~RangeFinder();
         /** Return the distance to the nearest object, or -1.0 if reading the pulse timed out.
         */
-        float read_m();
+        float read_m(uint8_t);
+        void read_and_filter(uint8_t);
+        float operator()(uint8_t i) {
+            return read_m(i);
+        }
+        float old_val[5];
+        float val[5];
+        uint8_t current_sensor;
+        uint8_t n_sensors;
     private:
-        PulseInOut pio;
+        PulseInOut pio0;
+        PulseInOut pio1;
+        PulseInOut pio2;
+        PulseInOut pio3;
         float scale, offset;
         int pulsetime, timeout;
+        
 };
 
 #endif
\ No newline at end of file