Drift-compensated angle in gyro (#931)

* compute angle based on undrifted rate

* add is calibrating function

* fix integrator

* added example

* docs

* poll faster

libs/core/input.ts

@@ -87,7 +87,7 @@ namespace sensors.internal {
             this.devType = DAL.DEVICE_TYPE_NONE
             this.iicid = ''
             this.sensors = []
-            this.poller = new Poller(50, () => this.query(), (prev, curr) => this.update(prev, curr));
+            this.poller = new Poller(25, () => this.query(), (prev, curr) => this.update(prev, curr));
         }
 
         poke() {
@@ -121,7 +121,7 @@ namespace sensors.internal {
 
         powerMM = control.mmap("/dev/lms_power", 2, 0)
 
-        devPoller = new Poller(500, () => { return hashDevices(); },
+        devPoller = new Poller(250, () => { return hashDevices(); },
             (prev, curr) => {
                 detectDevices();
             });

libs/core/integrator.ts

@@ -0,0 +1,26 @@
+namespace control {
+    export class EulerIntegrator {
+        public value: number;
+        private t: number;
+        private v: number;
+
+        constructor() {
+            this.reset();
+        }
+
+        public integrate(derivative: number): void {
+            let now = control.millis();
+            let dt = (now -this.t) / 1000.0;
+            this.value += dt * (this.v + derivative) / 2;
+
+            this.t = now;
+            this.v = derivative;
+        }
+
+        public reset() {
+            this.value = 0;
+            this.v = 0;
+            this.t = control.millis();
+        }
+    }
+}

libs/core/output.ts

@@ -264,8 +264,9 @@ namespace motors {
             // allow 500ms for robot to settle
             if (this._brake && this._brakeSettleTime > 0)
                 pause(this._brakeSettleTime);
-            else
-                pause(1); // give a tiny breather
+            else {
+                pause(1);
+            }
         }
 
         protected pauseOnRun(stepsOrTime: number) {
@@ -275,7 +276,6 @@ namespace motors {
                 // allow robot to settle
                 this.settle();
             } else {
-                // give a breather to the event system in tight loops
                 pause(1);
             }
         }

libs/core/pxt.json

@@ -25,7 +25,8 @@
         "dal.d.ts",
         "icons.jres",
         "ns.ts",
-        "platform.h"
+        "platform.h",
+        "integrator.ts"
     ],
     "testFiles": [
         "test.ts"