Added smoothing to UltrasonicSensor

Charles Weir · Charles Weir · commit cd509a1af067 · 2014-03-22T16:06:33.000Z
diff --git a/BrickPython/Sensor.py b/BrickPython/Sensor.py
@@ -103,14 +103,26 @@ class UltrasonicSensor(Sensor):
     '''
     #: The reading when no object is in sight:
     MAX_VALUE = 30
+    #: Round readings to nearest centimeters.
+    ROUND_TO = 5
+    #: How many readings to smooth over.
+    SMOOTHING_RANGE=10
 
     def __init__(self, port):
+        self.recentRawValues = [0]
         Sensor.__init__(self, port, Sensor.ULTRASONIC_CONT)
 
     def cookValue(self, rawValue):
-        result = int(5 * round(float(rawValue)/5))  # Round to nearest 5
+        self.recentRawValues.append( rawValue )
+        if len(self.recentRawValues) > UltrasonicSensor.SMOOTHING_RANGE:
+            del self.recentRawValues[0]
+        smoothedValue = min(self.recentRawValues)
+        result = int(self.ROUND_TO * round(float(smoothedValue)/self.ROUND_TO))  # Round to nearest 5
         return min(result, UltrasonicSensor.MAX_VALUE)
 
+#     def displayValue(self):
+#         return self.recentRawValues
+
 class LightSensor(Sensor):
     '''Represents my NXT color sensor.
     The BrickPi_Python COLOR_FULL setting didn't work for me at all - always has value 1.
diff --git a/ExamplePrograms/TrialApp.py b/ExamplePrograms/TrialApp.py
@@ -20,6 +20,7 @@ def __init__(self):
         self.root.wm_title("Trial running")
         for c in "ABCD":
             self.motor(c).zeroPosition()
+            self.addActionCoroutine(self.motor(c).runAtConstantSpeed(180))
         for c in settings:
             self.addSensorCoroutine(self.showChanges(c))
 
@@ -35,12 +36,6 @@ def showSensorValues(self, sensorId):
             for i in self.waitMilliseconds(1000): yield
             print sensor
 
-##        for c in "ABCD":
-##            motor= self.motor(c)
-##            for i in motor.moveTo(90*2): yield
-##            for i in motor.moveTo(0): yield
-##
-
 if __name__ == "__main__":
     logging.basicConfig(format='%(message)s', level=logging.DEBUG) # All log messages printed to console.
     logging.info( "Starting" )
diff --git a/test/TestSensor.py b/test/TestSensor.py
@@ -68,9 +68,17 @@ def testUltrasonicSensor(self):
         for input, output in {0:0, 2:0, 3:5, 4:5, 9:10, 11:10, 14:15, 16:15, 22:20, 23:25, 26:25,
                               255: UltrasonicSensor.MAX_VALUE
                               }.items():
-            sensor.updateValue( input )
+            for i in xrange(0,100):
+                sensor.updateValue( input )  # Remove effects of smoothing.
             self.assertEquals( sensor.value(), output )
 
+    def testUltrasonicSensorSmoothing(self):
+        sensor = UltrasonicSensor( '1' )
+        for input in [ 24,14,10,8,10,10,50,10,50,18,50]:
+            sensor.updateValue( input )
+        print sensor
+        self.assertEquals( sensor.value(), 10 )
+
     def testLightSensor(self):
         #Light is 680, dark about 800
         sensor = LightSensor('4')
