From c4bcf16d486fe8bf0ee6428e0d722cbd6766c6b2 Mon Sep 17 00:00:00 2001
From: "jinyuan.weng" <jinyuan.weng@seeed.cc>
Date: Fri, 22 Sep 2017 14:30:56 +0800
Subject: [PATCH] Modify ultrasonic sensor distance calculation

Measure distance 5 times, ignore zero data, and calculate the average
distance.
---
 main.ts | 54 ++++++++++++++++++++++++++++++++++++++++++------------
 1 file changed, 42 insertions(+), 12 deletions(-)

diff --git a/main.ts b/main.ts
index a2f118b..8cc39d4 100644
--- a/main.ts
+++ b/main.ts
@@ -102,15 +102,30 @@ namespace grove {
     {
         let duration = 0;
         let RangeInCentimeters = 0;
+        let buf: number [] = [0, 0, 0, 0, 0];
+        let num = 0;
         
-        pins.digitalWritePin(pin, 0);
-        control.waitMicros(2);
-        pins.digitalWritePin(pin, 1);
-        control.waitMicros(5);
-        pins.digitalWritePin(pin, 0);
+        for(let i = 0; i < 5; i ++)
+        {
+            pins.digitalWritePin(pin, 0);
+            control.waitMicros(2);
+            pins.digitalWritePin(pin, 1);
+            control.waitMicros(5);
+            pins.digitalWritePin(pin, 0);
+            
+            buf[i] = pins.pulseIn(pin, PulseValue.High, 100000); // Max duration 100 ms
+        }
         
-        duration = pins.pulseIn(pin, PulseValue.High);
+        for(let i = 0; i < 5; i ++)
+        {
+            if(buf[i] != 0)
+            {
+                duration = duration + buf[i];
+                num ++;
+            }
+        }
         
+        duration = duration / num;
         RangeInCentimeters = duration * 153 / 29 / 2 / 100;
         
         return RangeInCentimeters;
@@ -125,15 +140,30 @@ namespace grove {
     {
         let duration = 0;
         let RangeInCentimeters = 0;
+        let buf: number [] = [0, 0, 0, 0, 0];
+        let num = 0;
         
-        pins.digitalWritePin(pin, 0);
-        control.waitMicros(2);
-        pins.digitalWritePin(pin, 1);
-        control.waitMicros(5);
-        pins.digitalWritePin(pin, 0);
+        for(let i = 0; i < 5; i ++)
+        {
+            pins.digitalWritePin(pin, 0);
+            control.waitMicros(2);
+            pins.digitalWritePin(pin, 1);
+            control.waitMicros(5);
+            pins.digitalWritePin(pin, 0);
+            
+            buf[i] = pins.pulseIn(pin, PulseValue.High, 100000); // Max duration 100 ms
+        }
         
-        duration = pins.pulseIn(pin, PulseValue.High);
+        for(let i = 0; i < 5; i ++)
+        {
+            if(buf[i] != 0)
+            {
+                duration = duration + buf[i];
+                num ++;
+            }
+        }
         
+        duration = duration / num;
         RangeInCentimeters = duration * 153 / 74 / 2 / 100;
         
         return RangeInCentimeters;