Drift-compensated angle in gyro (#931)

* compute angle based on undrifted rate

* add is calibrating function

* fix integrator

* added example

* docs

* poll faster

docs/tutorials/drifter.md

@@ -0,0 +1,34 @@
+# Drifter
+
+Use this program to try out the gyro sensor and the effect of drifting.
+
+```typescript
+// this loop shows the rate, angle and drift of the robot
+forever(() => {
+    brick.showValue("rate", sensors.gyro2.rate(), 1)
+    brick.showValue("angle", sensors.gyro2.angle(), 2)
+    brick.showValue("drift", sensors.gyro2.drift(), 3)
+})
+// this loop shows wheter the sensor is calibrating
+forever(() => {
+    brick.showString(sensors.gyro2.isCalibrating() ? "calibrating..." : "", 4)
+})
+// instructions on how to use the buttons
+brick.showString("ENTER: calibrate", 7)
+brick.showString("       (reset+drift)", 8)
+brick.showString("LEFT: reset", 9)
+brick.showString("RIGHT: compute drift", 10)
+
+// enter -> calibrate
+brick.buttonEnter.onEvent(ButtonEvent.Pressed, function () {
+    sensors.gyro2.calibrate()
+})
+// right -> compute drift
+brick.buttonRight.onEvent(ButtonEvent.Pressed, function () {
+    sensors.gyro2.computeDrift()
+})
+// left -> reset
+brick.buttonLeft.onEvent(ButtonEvent.Pressed, function () {
+    sensors.gyro2.reset()
+})
+```

docs/tutorials/gyro.md

@@ -15,5 +15,11 @@
   "cardType": "tutorial",
   "url":"/tutorials/move-straight-with-gyro",
   "imageUrl":"/static/tutorials/move-straight-with-gyro.png"
+}, {
+  "name": "Drifter",
+  "description": "Explore how the gyro is drifting",
+  "cardType": "example",
+  "url":"/tutorials/drifter",
+  "imageUrl":"/static/tutorials/drifter.png"
 }]
 ```

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
+            else {
-                pause(1); // give a tiny breather
+                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"

libs/gyro-sensor/docs/reference/sensors/gyro/pause-until-rotated.md

@@ -0,0 +1,21 @@
+# Pause Until Rotated
+
+Pauses the program until the gyro sensors detect that the desired amount of rotation
+has been acheived.
+
+```
+sensors.gyro2.pauseUntilRotated(90)
+```
+
+## Example
+
+This program performs a square turn left, then right.
+
+```blocks
+sensors.gyro2.calibrate()
+motors.largeBC.steer(200, 10)
+sensors.gyro2.pauseUntilRotated(90)
+motors.largeBC.steer(-200, 10)
+sensors.gyro2.pauseUntilRotated(-90)
+motors.largeBC.stop()
+```

libs/gyro-sensor/gyro.ts

@@ -7,12 +7,15 @@ const enum GyroSensorMode {
 namespace sensors {
     //% fixedInstances
     export class GyroSensor extends internal.UartSensor {
-        private calibrating: boolean;
+        private _calibrating: boolean;
         private _drift: number;
+        private _angle: control.EulerIntegrator;
         constructor(port: number) {
             super(port)
-            this.calibrating = false;
+            this._calibrating = false;
             this._drift = 0;
+            this._angle = new control.EulerIntegrator();
+            this._setMode(GyroSensorMode.Rate);
             this.setMode(GyroSensorMode.Rate);
         }
 
@@ -21,13 +24,17 @@ namespace sensors {
         }
 
         _query(): number {
-            return this.getNumber(NumberFormat.Int16LE, 0);
+            const v = this.getNumber(NumberFormat.Int16LE, 0);
+            this._angle.integrate(v - this._drift);
+            return v;
         }
 
         setMode(m: GyroSensorMode) {
-            if (m == GyroSensorMode.Rate && this.mode != m)
+            // decrecated
-                this._drift = 0;
+        }
-            this._setMode(m)
+
+        isCalibrating(): boolean {
+            return this._calibrating;
         }
 
         /**
@@ -40,15 +47,14 @@ namespace sensors {
         //% parts="gyroscope"
         //% blockNamespace=sensors
         //% this.fieldEditor="ports"
-        //% weight=64
+        //% weight=64 blockGap=8
         //% group="Gyro Sensor"
         angle(): number {
             this.poke();
-            if (this.calibrating)
+            if (this._calibrating)
-                pauseUntil(() => !this.calibrating, 2000);
+                pauseUntil(() => !this._calibrating, 2000);
 
-            this.setMode(GyroSensorMode.Angle);
+            return Math.round(this._angle.value);
-            return this._query();
         }
 
         /**
@@ -65,10 +71,8 @@ namespace sensors {
         //% group="Gyro Sensor"
         rate(): number {
             this.poke();
-            if (this.calibrating)
+            if (this._calibrating)
-                pauseUntil(() => !this.calibrating, 2000);
+                pauseUntil(() => !this._calibrating, 2000);
-
-            this.setMode(GyroSensorMode.Rate);
             return this._query() - this._drift;
         }
 
@@ -85,12 +89,12 @@ namespace sensors {
         //% weight=51 blockGap=8
         //% group="Gyro Sensor"
         calibrate(): void {
-            if (this.calibrating) return; // already in calibration mode
+            if (this._calibrating) return; // already in calibration mode
 
             const statusLight = brick.statusLight(); // save current status light
             brick.setStatusLight(StatusLight.Orange);
 
-            this.calibrating = true;
+            this._calibrating = true;
             // may be triggered by a button click,
             // give time for robot to settle
             pause(700);
@@ -104,7 +108,8 @@ namespace sensors {
                 brick.setStatusLight(statusLight); // resture previous light
 
                 // and we're done
-                this.calibrating = false;
+                this._angle.reset();
+                this._calibrating = false;
                 return;
             }
 
@@ -116,22 +121,22 @@ namespace sensors {
             // wait till sensor is live
             pauseUntil(() => this.isActive(), 7000);
             // mode toggling
-            this.setMode(GyroSensorMode.Rate);
+            this._setMode(GyroSensorMode.Rate);
-            this.setMode(GyroSensorMode.Angle);
+            this._setMode(GyroSensorMode.Angle);
+            this._setMode(GyroSensorMode.Rate);
 
             // check sensor is ready
             if (!this.isActive()) {
                 brick.setStatusLight(StatusLight.RedFlash); // didn't work
                 pause(2000);
                 brick.setStatusLight(statusLight); // restore previous light
-                this.calibrating = false;
+                this._angle.reset();
+                this._calibrating = false;
                 return;
             }
 
             // switch to rate mode
             this.computeDriftNoCalibration();
-            // switch back to the desired mode
-            this.setMode(this.mode);
 
             // and done
             brick.setStatusLight(StatusLight.Green); // success
@@ -139,7 +144,8 @@ namespace sensors {
             brick.setStatusLight(statusLight); // resture previous light
 
             // and we're done
-            this.calibrating = false;
+            this._angle.reset();
+            this._calibrating = false;
         }
 
         /**
@@ -154,13 +160,37 @@ namespace sensors {
         //% weight=50 blockGap=8
         //% group="Gyro Sensor"
         reset(): void {
-            if (this.calibrating) return; // already in calibration mode
+            if (this._calibrating) return; // already in calibration mode
+
+            this._calibrating = true;
+            const statusLight = brick.statusLight(); // save current status light
+            brick.setStatusLight(StatusLight.Orange);
 
-            this.calibrating = true;
             // send a reset command
             super.reset();
+            this._drift = 0;
+            this._angle.reset();
+            pauseUntil(() => this.isActive(), 7000);
+
+            // check sensor is ready
+            if (!this.isActive()) {
+                brick.setStatusLight(StatusLight.RedFlash); // didn't work
+                pause(2000);
+                brick.setStatusLight(statusLight); // restore previous light
+                this._angle.reset();
+                this._calibrating = false;
+                return;
+            }
+
+            this._setMode(GyroSensorMode.Rate);
+
             // and done
-            this.calibrating = false;
+            brick.setStatusLight(StatusLight.Green); // success
+            pause(1000);
+            brick.setStatusLight(statusLight); // resture previous light
+            // and done
+            this._angle.reset();
+            this._calibrating = false;
         }
 
         /**
@@ -190,15 +220,34 @@ namespace sensors {
         //% weight=10 blockGap=8
         //% group="Gyro Sensor"
         computeDrift() {
-            if (this.calibrating)
+            if (this._calibrating)
-                pauseUntil(() => !this.calibrating, 2000);
+                pauseUntil(() => !this._calibrating, 2000);
             pause(1000); // let the robot settle
             this.computeDriftNoCalibration();
         }
 
+        /**
+         * Pauses the program until the gyro detected
+         * that the angle changed by the desired amount of degrees.
+         * @param degrees the degrees to turn 
+         */
+        //% help=sensors/gyro/pause-until-rotated
+        //% block="pause **gyro** %this|until rotated %degrees|degrees"
+        //% blockId=gyroPauseUntilRotated
+        //% parts="gyroscope"
+        //% blockNamespace=sensors
+        //% this.fieldEditor="ports"
+        //% weight=63
+        //% group="Gyro Sensor"
+        pauseUntilRotated(degrees: number, timeOut?: number): void {
+            let a = this.angle();
+            const end = a + degrees;
+            const direction = (end - a) > 0 ? 1 : -1;
+            pauseUntil(() => (end - this.angle()) * direction <= 0, timeOut);
+        }
+
         private computeDriftNoCalibration() {
             // clear drift
-            this.setMode(GyroSensorMode.Rate);
             this._drift = 0;
             const n = 10;
             let d = 0;
@@ -207,23 +256,18 @@ namespace sensors {
                 pause(20);
             }
             this._drift = d / n;
+            this._angle.reset();
         }
 
         _info(): string {
-            if (this.calibrating)
+            if (this._calibrating)
                 return "cal...";
 
-            switch (this.mode) {
-                case GyroSensorMode.Angle:
-                    return `${this._query()}>`;
-                case GyroSensorMode.Rate:
             let r = `${this._query()}r`;
             if (this._drift != 0)
                 r += `-${this._drift | 0}`;
             return r;
         }
-            return "";
-        }
     }
 
     //% fixedInstance whenUsed block="2" weight=95 jres=icons.port2

sim/state/gyro.ts

@@ -8,7 +8,6 @@ namespace pxsim {
     export class GyroSensorNode extends UartSensorNode {
         id = NodeType.GyroSensor;
 
-        private angle: number = 0;
         private rate: number = 0;
 
         constructor(port: number) {
@@ -19,14 +18,6 @@ namespace pxsim {
             return DAL.DEVICE_TYPE_GYRO;
         }
 
-        setAngle(angle: number) {
-            angle = angle | 0;
-            if (this.angle != angle) {
-                this.angle = angle;
-                this.setChangedState();
-            }
-        }
-
         setRate(rate: number) {
             rate = rate | 0;
             if (this.rate != rate) {
@@ -35,9 +26,12 @@ namespace pxsim {
             }
         }
 
+        getRate() {
+            return this.rate;
+        }
+
         getValue() {
-            return this.mode == GyroSensorMode.Angle ? this.angle :
+            return this.getRate();
-                this.mode == GyroSensorMode.Rate ? this.rate : 0;
         }
     }
 }

sim/visuals/controls/rotationSlider.ts

@@ -2,12 +2,11 @@
 
 namespace pxsim.visuals {
     const MAX_RATE = 40;
-    const MAX_ANGLE = 360;
 
     export class RotationSliderControl extends ControlView<GyroSensorNode> {
         private group: SVGGElement;
         private slider: SVGGElement;
-        private text: SVGTextElement;
+        private rateText: SVGTextElement;
 
         private static SLIDER_WIDTH = 70;
         //private static SLIDER_HEIGHT = 78;
@@ -26,8 +25,8 @@ namespace pxsim.visuals {
             pxsim.svg.child(this.slider, "circle", { 'cx': 9, 'cy': 50, 'r': 13, 'style': 'fill: #f12a21' });
             pxsim.svg.child(this.slider, "circle", { 'cx': 9, 'cy': 50, 'r': 12.5, 'style': 'fill: none;stroke: #b32e29' });
 
-            this.text = pxsim.svg.child(this.group, "text", {
+            this.rateText = pxsim.svg.child(this.group, "text", {
-                'x': RotationSliderControl.SLIDER_WIDTH / 2, 
+                'x': this.getInnerWidth() / 2,
                 'y': RotationSliderControl.SLIDER_WIDTH * 1.2,
                 'text-anchor': 'middle', 'dominant-baseline': 'middle',
                 'style': 'font-size: 16px',
@@ -72,17 +71,10 @@ namespace pxsim.visuals {
                 return;
             }
             const node = this.state;
-            let percentage = 50;
+            const rate = node.getRate();
-            if (node.getMode() == GyroSensorMode.Rate) {
+            this.rateText.textContent = `${rate}°/s`
-                const rate = node.getValue();
-                this.text.textContent = `${rate}°/s`
             // cap rate at 40deg/s
-                percentage = 50 + Math.sign(rate) * Math.min(MAX_RATE, Math.abs(rate)) / MAX_RATE * 50;
+            const percentage = 50 + Math.sign(rate) * Math.min(MAX_RATE, Math.abs(rate)) / MAX_RATE * 50;
-            } else {  //angle
-                const angle = node.getValue();
-                this.text.textContent = `${angle}°`
-                percentage = 50 + Math.sign(angle) * Math.min(MAX_ANGLE, Math.abs(angle)) / MAX_ANGLE * 50;
-            }
             const x = RotationSliderControl.SLIDER_WIDTH * percentage / 100;
             const y = Math.abs((percentage - 50) / 50) * 10;
             this.slider.setAttribute("transform", `translate(${x}, ${y})`);
@@ -97,11 +89,7 @@ namespace pxsim.visuals {
             t = -(t - 0.5) * 2; // [-1,1]
 
             const state = this.state;
-            if (state.getMode() == GyroSensorMode.Rate) {
             state.setRate(MAX_RATE * t);
-            } else {
-                state.setAngle(MAX_ANGLE * t)
-            }
         }
     }