Diff: Xtra_Sensors/RangeFinder.cpp

Revision:

2:e7874762cc25

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Xtra_Sensors/RangeFinder.cpp	Mon Oct 21 17:16:11 2019 +0000
@@ -0,0 +1,94 @@
+/* Copyright (c) 2012 Nick Ryder, University of Oxford
+ * nick.ryder@physics.ox.ac.uk
+ *
+ *  MIT License
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy of this software 
+ * and associated documentation files (the "Software"), to deal in the Software without restriction, 
+ * including without limitation the rights to use, copy, modify, merge, publish, distribute, 
+ * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is 
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all copies or 
+ * substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING 
+ * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 
+ * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+ 
+ 
+ #include "RangeFinder.h"
+
+// 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() {
+}
+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;
+    }
+    return t / scale + offset;
+}