Color calibration (#245)

* better handling of thresholds, color calibration strategy

* updating calibration parameters

libs/color-sensor/_locales/color-sensor-jsdoc-strings.json

@@ -1,5 +1,6 @@
 {
   "sensors.ColorSensor": "The color sensor is a digital sensor that can detect the color or intensity\nof light that enters the small window on the face of the sensor.",
+  "sensors.ColorSensor.calibrateLight": "Collects measurement of the light condition and adjusts the threshold to 10% / 90%.",
   "sensors.ColorSensor.color": "Get the current color from the color sensor.",
   "sensors.ColorSensor.colorMode": "Gets the current color mode",
   "sensors.ColorSensor.light": "Measures the ambient or reflected light value from 0 (darkest) to 100 (brightest).",

libs/color-sensor/_locales/color-sensor-strings.json

@@ -13,6 +13,7 @@
   "LightCondition.Dark|block": "dark",
   "LightIntensityMode.Ambient|block": "ambient light",
   "LightIntensityMode.Reflected|block": "reflected light",
+  "sensors.ColorSensor.calibrateLight|block": "calibrate|%sensor|for %mode|light",
   "sensors.ColorSensor.color|block": "%sensor| color",
   "sensors.ColorSensor.light|block": "%sensor|%mode",
   "sensors.ColorSensor.onColorDetected|block": "on %sensor|detected color %color",

libs/color-sensor/color.ts

@@ -39,9 +39,9 @@ const enum ColorSensorColor {
 
 enum LightCondition {
     //% block="dark"
-    Dark = sensors.internal.ThresholdState.Low,
+    Dark = sensors.ThresholdState.Low,
     //$ block="bright"
-    Bright = sensors.internal.ThresholdState.High
+    Bright = sensors.ThresholdState.High
 }
 
 namespace sensors {
@@ -52,12 +52,14 @@ namespace sensors {
      */
     //% fixedInstances
     export class ColorSensor extends internal.UartSensor {
-        thresholdDetector: sensors.internal.ThresholdDetector;
+        thresholdDetector: sensors.ThresholdDetector;
+        calibrating: boolean;
 
         constructor(port: number) {
             super(port)
             this._setMode(ColorSensorMode.None);
-            this.thresholdDetector = new sensors.internal.ThresholdDetector(this.id());
+            this.thresholdDetector = new sensors.ThresholdDetector(this.id());
+            this.calibrating = false;
         }
 
         _colorEventValue(value: number) {
@@ -95,6 +97,7 @@ namespace sensors {
         }
 
         _update(prev: number, curr: number) {
+            if (this.calibrating) return; // simply ignore data updates while calibrating
             if (this.mode == ColorSensorMode.Color)
                 control.raiseEvent(this._id, this._colorEventValue(curr));
             else
@@ -230,18 +233,65 @@ namespace sensors {
         //% blockId=colorSetThreshold block="set %sensor|%condition|to %value"
         //% group="Threshold" blockGap=8 weight=90
         //% value.min=0 value.max=100
+        //% sensor.fieldEditor="ports"
         setThreshold(condition: LightCondition, value: number) {
             if (condition == LightCondition.Dark)
                 this.thresholdDetector.setLowThreshold(value)
             else
                 this.thresholdDetector.setHighThreshold(value);
         }
+
+        /**
+         * Collects measurement of the light condition and adjusts the threshold to 10% / 90%.
+         */
+        //% blockId=colorCalibrateLight block="calibrate|%sensor|for %mode|light"
+        //% group="Threshold" weight=91 blockGap=8
+        //% sensor.fieldEditor="ports"
+        calibrateLight(mode: LightIntensityMode, deviation: number = 8) {
+            this.calibrating = true; // prevent events
+
+            this.light(mode); // trigger a read
+            pauseUntil(() => this.isActive()); // ensure sensor is live
+            
+
+            let vold = 0;
+            let vcount = 0;
+            let min = 200;
+            let max = -200;     
+            let k = 0;       
+            while(k++ < 1000 && vcount < 50) {
+                let v = this.light(mode);
+                min = Math.min(min, v);
+                max = Math.max(max, v);
+                // detect if nothing has changed and stop calibration
+                if (Math.abs(v - vold) <= 2)
+                    vcount ++;
+                else {
+                    vold = v;
+                    vcount = 1;
+                }
+
+                // wait a bit
+                loops.pause(50);                
+            }
+
+            // apply tolerance
+            const minDist = 10;
+            min = Math.max(minDist / 2, Math.min(min + deviation / 2, max - deviation / 2 - minDist / 2));
+            max = Math.min(100 - minDist / 2, Math.max(min + minDist, max - deviation / 2));
+        
+            // apply thresholds
+            this.thresholdDetector.setLowThreshold(min);
+            this.thresholdDetector.setHighThreshold(max);
+
+            this.calibrating = false;
+        }
     }
 
-    //% whenUsed block="color 3" weight=90 fixedInstance jres=icons.port3
+    //% whenUsed block="color 3" weight=95 fixedInstance jres=icons.port3
     export const color3: ColorSensor = new ColorSensor(3)
     
-    //% whenUsed block="color 1" weight=95 fixedInstance jres=icons.port1
+    //% whenUsed block="color 1" weight=90 fixedInstance jres=icons.port1
     export const color1: ColorSensor = new ColorSensor(1)
 
     //% whenUsed block="color 2" weight=90 fixedInstance jres=icons.port2

libs/core/_locales/core-strings.json

@@ -101,6 +101,7 @@
   "{id:category}Motors": "Motors",
   "{id:category}Output": "Output",
   "{id:category}Screen": "Screen",
+  "{id:category}Sensors": "Sensors",
   "{id:category}Serial": "Serial",
   "{id:group}Buttons": "Buttons",
   "{id:group}Counters": "Counters",

libs/core/input.ts

@@ -196,83 +196,6 @@ namespace sensors.internal {
         }
     }
 
-    export enum ThresholdState {
-        Normal = 1,
-        High = 2,
-        Low = 3,
-    }
-
-    export class ThresholdDetector {
-        public id: number;
-        private min: number;
-        private max: number;
-        private lowThreshold: number;
-        private highThreshold: number;
-        private level: number;
-        public state: ThresholdState;
-
-        constructor(id: number, min = 0, max = 100, lowThreshold = 20, highThreshold = 80) {
-            this.id = id;
-            this.min = min;
-            this.max = max;
-            this.lowThreshold = lowThreshold;
-            this.highThreshold = highThreshold;
-            this.level = Math.ceil((max - min) / 2);
-            this.state = ThresholdState.Normal;
-        }
-
-        public setLevel(level: number) {
-            if (this == null) return
-            this.level = this.clampValue(level);
-
-            if (this.level >= this.highThreshold) {
-                this.setState(ThresholdState.High);
-            }
-            else if (this.level <= this.lowThreshold) {
-                this.setState(ThresholdState.Low);
-            }
-            else {
-                this.setState(ThresholdState.Normal);
-            }
-        }
-
-        public setLowThreshold(value: number) {
-            this.lowThreshold = this.clampValue(value);
-            this.highThreshold = Math.max(this.lowThreshold + 1, this.highThreshold);
-        }
-
-        public setHighThreshold(value: number) {
-            this.highThreshold = this.clampValue(value);
-            this.lowThreshold = Math.min(this.highThreshold - 1, this.lowThreshold);
-        }
-
-        private clampValue(value: number) {
-            if (value < this.min) {
-                return this.min;
-            }
-            else if (value > this.max) {
-                return this.max;
-            }
-            return value;
-        }
-
-        private setState(state: ThresholdState) {
-            if (this.state == state) return;
-
-            this.state = state;
-            switch (state) {
-                case ThresholdState.High:
-                    control.raiseEvent(this.id, ThresholdState.High);
-                    break;
-                case ThresholdState.Low:
-                    control.raiseEvent(this.id, ThresholdState.Low);
-                    break;
-                case ThresholdState.Normal:
-                    break;
-            }
-        }
-    }
-
     export class UartSensor extends Sensor {
         protected mode: number // the mode user asked for
         protected realmode: number // the mode the hardware is in
@@ -499,3 +422,83 @@ namespace sensors.internal {
         TST_UART_WRITE = 0xc048740a,
     }
 }
+
+namespace sensors {
+    export enum ThresholdState {
+        Normal = 1,
+        High = 2,
+        Low = 3,
+    }
+
+    export class ThresholdDetector {
+        public id: number;
+        private min: number;
+        private max: number;
+        private lowThreshold: number;
+        private highThreshold: number;
+        private level: number;
+        public state: ThresholdState;
+
+        constructor(id: number, min = 0, max = 100, lowThreshold = 20, highThreshold = 80) {
+            this.id = id;
+            this.min = min;
+            this.max = max;
+            this.lowThreshold = lowThreshold;
+            this.highThreshold = highThreshold;
+            this.level = Math.ceil((max - min) / 2);
+            this.state = ThresholdState.Normal;
+        }
+
+        public setLevel(level: number) {
+            if (this == null) return
+            this.level = this.clampValue(level);
+
+            if (this.level >= this.highThreshold) {
+                this.setState(ThresholdState.High);
+            }
+            else if (this.level <= this.lowThreshold) {
+                this.setState(ThresholdState.Low);
+            }
+            else {
+                const interval = (this.highThreshold - this.lowThreshold) / 6;
+                if ((this.state == ThresholdState.High && this.level < this.highThreshold - interval) ||
+                    (this.state == ThresholdState.Low && this.level > this.lowThreshold + interval))
+                    this.setState(ThresholdState.Normal);
+            }
+        }
+
+        public setLowThreshold(value: number) {
+            this.lowThreshold = this.clampValue(value);
+            this.highThreshold = Math.max(this.lowThreshold + 1, this.highThreshold);
+        }
+
+        public setHighThreshold(value: number) {
+            this.highThreshold = this.clampValue(value);
+            this.lowThreshold = Math.min(this.highThreshold - 1, this.lowThreshold);
+        }
+
+        private clampValue(value: number) {
+            if (value < this.min) {
+                return this.min;
+            }
+            else if (value > this.max) {
+                return this.max;
+            }
+            return value;
+        }
+
+        private setState(state: ThresholdState) {
+            if (this.state == state) return;
+
+            this.state = state;
+            switch (state) {
+                case ThresholdState.High:
+                    control.raiseEvent(this.id, ThresholdState.High);
+                    break;
+                case ThresholdState.Low:
+                    control.raiseEvent(this.id, ThresholdState.Low);
+                    break;
+            }
+        }
+    }    
+}

libs/infrared-sensor/ir.ts

@@ -138,12 +138,12 @@ namespace sensors {
     //% fixedInstances
     export class InfraredSensor extends internal.UartSensor {
         private channel: IrRemoteChannel;
-        private proximityThreshold: sensors.internal.ThresholdDetector;
+        private proximityThreshold: sensors.ThresholdDetector;
 
         constructor(port: number) {
             super(port)
             this.channel = IrRemoteChannel.Ch0
-            this.proximityThreshold = new sensors.internal.ThresholdDetector(this._id, 0, 100, 10, 90);
+            this.proximityThreshold = new sensors.ThresholdDetector(this._id, 0, 100, 10, 90);
             irButton(0) // make sure buttons array is initalized
 
             // and set the mode, as otherwise button events won't work

libs/ultrasonic-sensor/ultrasonic.ts

@@ -2,21 +2,21 @@ enum UltrasonicSensorEvent {
     //% block="object detected"
     ObjectDetected = 10,
     //% block="object near"
-    ObjectNear = sensors.internal.ThresholdState.Low,
+    ObjectNear = sensors.ThresholdState.Low,
     //% block="object far"
-    ObjectFar = sensors.internal.ThresholdState.High
+    ObjectFar = sensors.ThresholdState.High
 }
 
 namespace sensors {
 
     //% fixedInstances
     export class UltraSonicSensor extends internal.UartSensor {
-        private promixityThreshold: sensors.internal.ThresholdDetector;
+        private promixityThreshold: sensors.ThresholdDetector;
         private movementThreshold: number;
 
         constructor(port: number) {
             super(port)
-            this.promixityThreshold = new sensors.internal.ThresholdDetector(this.id(), 0, 255, 10, 100); // range is 0..255cm
+            this.promixityThreshold = new sensors.ThresholdDetector(this.id(), 0, 255, 10, 100); // range is 0..255cm
             this.movementThreshold = 1;
         }