gyro boy improvements (#236)

gyro boy improvements

docs/examples/crane-labview.md

@@ -0,0 +1,51 @@
+# Crane LabView
+
+```blocks
+function INI() {
+    motors.largeB.setBrake(true)
+    motors.largeC.setBrake(true)
+    motors.mediumA.setBrake(true)
+    motors.largeB.setSpeed(-50)
+    pauseUntil(() => sensors.color3.light(LightIntensityMode.Reflected) > 25);
+    motors.largeB.stop();
+    motors.mediumA.setSpeed(30, 1, MoveUnit.Seconds);
+    motors.mediumA.setSpeed(-50, 90, MoveUnit.Degrees);
+    motors.largeC.setSpeed(50)
+    sensors.touch1.pauseUntil(TouchSensorEvent.Pressed);
+    motors.largeC.setSpeed(-50, 0.86, MoveUnit.Rotations);
+}
+
+INI()
+
+let down = false;
+loops.forever(function () {
+    brick.showImage(images.informationQuestionMark)
+    brick.setLight(BrickLight.OrangePulse);
+    pauseUntil(() => (down = brick.buttonDown.wasPressed()) || brick.buttonUp.wasPressed())
+    brick.setLight(BrickLight.Off)
+    music.playSoundEffect(sounds.mechanicalAirRelease)
+    brick.showImage(images.informationAccept)
+    if (down) {
+        brick.showImage(images.informationForward)
+        motors.largeC.setSpeed(65, 0.85, MoveUnit.Rotations);
+    } else {
+        brick.showImage(images.informationBackward)
+        motors.largeC.setSpeed(-65, 0.85, MoveUnit.Rotations);
+    }
+    motors.largeB.setSpeed(20, 275, MoveUnit.Degrees)
+    motors.mediumA.setSpeed(30, 1, MoveUnit.Seconds)
+    motors.largeB.setSpeed(-55)
+    pauseUntil(() => sensors.color3.light(LightIntensityMode.Reflected) > 25);
+    motors.largeB.stop();
+    if (down) {
+        motors.largeC.setSpeed(-65, 0.86, MoveUnit.Rotations);
+    } else {
+        motors.largeC.setSpeed(65, 0.85, MoveUnit.Rotations);
+    }
+    motors.largeB.setSpeed(20, 275, MoveUnit.Degrees);
+    motors.mediumA.setSpeed(-30, 90, MoveUnit.Degrees);
+    motors.largeB.setSpeed(-55)
+    pauseUntil(() => sensors.color3.light(LightIntensityMode.Reflected) > 25);
+    motors.largeB.stop()
+})
+```

docs/examples/gyro-boy-labview.md

@@ -0,0 +1,215 @@
+# Gyro Boy LabView
+
+```blocks
+let mSum = 0;
+let mPos = 0;
+let mSpd = 0;
+let mD = 0;
+let mDP1 = 0;
+let mDP2 = 0;
+let mDP3 = 0;
+let Crdv = 0;
+let cLo = 0;
+let gAng = 0;
+let ok = false;
+let pwr = 0;
+let Cstr = 0;
+let Cdrv = 0;
+let gMn = 0;
+let gMx = 0;
+let gSum = 0;
+let gyro = 0;
+let gOS = 0;
+let gSpd = 0;
+let tInt = 0.014;
+let lpwr = 0
+let rpwr = 0
+let tStart = 0
+let st = 0
+let oldDr = 0
+
+function RST() {
+    motors.largeA.reset()
+    motors.largeD.reset()
+    motors.largeA.setRegulated(false)
+    motors.largeD.setRegulated(false)
+    sensors.gyro2.reset()
+    sensors.gyro2.rate()
+    control.timer2.reset()
+    loops.pause(5000)
+    mSum = 0;
+    mPos = 0;
+    mD = 0;
+    mDP1 = 0;
+    mDP2 = 0;
+    mDP3 = 0;
+    Crdv = 0;
+    cLo = 0;
+    gAng = 0;
+    ok = false;
+    pwr = 0;
+    st = 0;
+    Cstr = 0;
+    Cdrv = 0;
+}
+
+function OS() {
+    // OSL
+    do {
+        gMn = 1000;
+        gMx = -100;
+        gSum = 0;
+        // gChk
+        for (let i = 0; i < 200; i++) {
+            gyro = sensors.gyro2.rate()
+            gSum = gyro;
+            gMx = Math.max(gMx, gyro)
+            gMn = Math.min(gMn, gyro)
+            loops.pause(4);
+        }
+    } while (gMx - gMn > 2);
+    gOS = gSum / 200;
+}
+
+function GT() {
+    if (cLo == 0) {
+        tInt = 0.014;
+        control.timer1.reset();
+    } else {
+        tInt = control.timer1.seconds() / cLo;
+    }
+    cLo++;
+}
+
+function GG() {
+    gyro = sensors.gyro2.rate();
+    gOS = 0.0005 * gyro + (1 - 0.0005) * gOS
+    gSpd = gyro - gOS;
+    gAng = gAng + tInt * gSpd;
+}
+
+function GM() {
+    let temp = mSum
+    mSum = motors.largeD.angle() + motors.largeA.angle();
+    mD = mSum - temp;
+    mPos = mPos + mD;
+    mSpd = ((mDP1 + mDP2 + mDP3 + mD) / 4) / tInt;
+    mDP3 = mDP2;
+    mDP2 = mDP1;
+    mDP1 = mD;
+}
+
+function EQ() {
+    mPos = mPos - Cdrv * tInt;
+    pwr = (0.8 * gSpd + 15 * gAng) + (0.095 * mSpd + 0.13 * mPos) - 0.01 * Cdrv
+    if (pwr > 100) pwr = 100
+    else if (pwr < -100) pwr = -100
+}
+
+function cntrl() {
+    mPos = mPos - tInt * Cdrv
+    lpwr = (pwr + Cstr * 0.1)
+    rpwr = (pwr - Cstr * 0.1)
+}
+
+function CHK() {
+    if (Math.abs(pwr) < 100)
+        control.timer2.reset();
+    if (control.timer2.seconds() > 2) {
+        ok = true;
+    }
+}
+
+// M
+loops.forever(function () {
+    RST();
+    brick.showImage(images.eyesSleeping)
+    OS()
+    gAng = -0.25;
+    music.playSoundEffect(sounds.movementsSpeedUp)
+    brick.showImage(images.eyesAwake)
+    st = 1;
+    // BALANCE
+    while (!ok) {
+        GT();
+        let t1 = control.timer1.millis()
+        GG();
+        GM();
+        EQ();
+        cntrl();
+        motors.largeA.setSpeed(lpwr)
+        motors.largeD.setSpeed(rpwr)
+        CHK()
+        let t2 = control.timer1.millis();
+        let p = 5 - (t2 - t1);
+        loops.pause(Math.max(1, p))
+    }
+    motors.stopAllMotors()
+    st = 0;
+    brick.setLight(BrickLight.RedPulse);
+    brick.showImage(images.eyesKnockedOut)
+    music.playSoundEffect(sounds.movementsSpeedDown)
+    sensors.touch3.pauseUntil(TouchSensorEvent.Pressed)
+    brick.setLight(BrickLight.Off);
+})
+
+// BHV
+loops.forever(function () {
+    switch (st) {
+        case 0:
+            Cdrv = 0;
+            Cstr = 0;
+            break;
+        case 1:
+            Cdrv = 40;
+            loops.pause(4000);
+            Cdrv = 0;
+            music.playTone(1000, 100);
+            st = 2;
+            break;
+        case 2:
+            switch (sensors.color1.color()) {
+                case ColorSensorColor.Red:
+                    music.playTone(2000, 100);
+                    Cdrv = 0;
+                    Cstr = 0;
+                    break;
+                case ColorSensorColor.Green:
+                    music.playTone(2000, 100);
+                    Cdrv = 150;
+                    Cstr = 0;
+                    break;
+                case ColorSensorColor.Blue:
+                    music.playTone(2000, 100);
+                    Cstr = 70;
+                    break;
+                case ColorSensorColor.Yellow:
+                    music.playTone(2000, 100);
+                    Cstr = -70;
+                    break;
+                case ColorSensorColor.White:
+                    music.playTone(2000, 100);
+                    Cdrv = -75;
+                    break;
+            }
+            if (sensors.ultrasonic4.distance() < 25) {
+                Cstr = 0;
+                oldDr = Cdrv;
+                Cdrv = -10;
+                motors.mediumC.setSpeed(30, 30, MoveUnit.Degrees);
+                motors.mediumC.setSpeed(-30, 60, MoveUnit.Degrees);
+                motors.mediumC.setSpeed(30, 30, MoveUnit.Degrees);
+                if (Math.randomRange(-1, 1) >= 1)
+                    Cstr = 70;
+                else
+                    Cstr = -70;
+                loops.pause(4000);
+                music.playTone(2000, 100)
+                Cstr = 0;
+                Cdrv = oldDr;
+            }
+            break;
+    }
+    loops.pause(80);
+})
+```

libs/core/_locales/core-jsdoc-strings.json

@@ -53,6 +53,8 @@
   "console.sendToScreen": "Sends the log messages to the brick screen and uses the brick up and down buttons to scroll.",
   "control": "Program controls and events.",
   "control.Timer.millis": "Gets the elapsed time in millis",
+  "control.Timer.pauseUntil": "Pauses until the timer reaches the given amount of milliseconds",
+  "control.Timer.pauseUntil|param|ms": "how long to pause for, eg: 5, 100, 200, 500, 1000, 2000",
   "control.Timer.reset": "Resets the timer",
   "control.Timer.seconds": "Gets the elapsed time in seconds",
   "control.allocateNotifyEvent": "Allocates the next user notification event",

libs/core/_locales/core-strings.json

@@ -48,6 +48,7 @@
   "console.sendToScreen|block": "send console to screen",
   "console|block": "console",
   "control.Timer.millis|block": "%timer|millis",
+  "control.Timer.pauseUntil|block": "%timer|pause until (ms) %ms",
   "control.Timer.reset|block": "%timer|reset",
   "control.Timer.seconds|block": "%timer|seconds",
   "control.raiseEvent|block": "raise event|from %src|with value %value",

libs/core/input.ts

@@ -312,6 +312,7 @@ namespace sensors.internal {
 
         reset() {
             if (this.isActive()) uartReset(this._port);
+            this.realmode = 0;
         }
     }
 

libs/core/timer.ts

@@ -30,6 +30,16 @@ namespace control {
         reset() {
             this.start = control.millis();
         }
+
+        /**
+         * Pauses until the timer reaches the given amount of milliseconds
+         * @param ms how long to pause for, eg: 5, 100, 200, 500, 1000, 2000
+         */
+        //% blockId=timerPauseUntil block="%timer|pause until (ms) %ms"
+        pauseUntil(ms: number) {
+            const remaining = this.millis() - ms;
+            loops.pause(Math.max(0, remaining));
+        }
     }
 
     //% whenUsed fixedInstance block="timer 1"

libs/datalog/_locales/datalog-jsdoc-strings.json

@@ -3,6 +3,9 @@
   "datalog.addValue": "Adds a cell to the row of data",
   "datalog.addValue|param|name": "name of the cell, eg: \"x\"",
   "datalog.addValue|param|value": "value of the cell, eg: 0",
+  "datalog.flush": "Commits any buffered row to disk",
+  "datalog.setEnabled": "Turns on or off datalogging",
   "datalog.setFile": "Starts a new data logger for the given file",
+  "datalog.setFile|param|filename": "the filename, eg: \"datalog.csv\"",
   "datalog.setStorage": "* @param storage custom storage solution"
 }

libs/datalog/_locales/datalog-strings.json

@@ -1,6 +1,7 @@
 {
   "datalog.addRow|block": "datalog add row",
   "datalog.addValue|block": "datalog add %name|=%value",
+  "datalog.setEnabled|block": "datalog %enabled",
   "datalog|block": "datalog",
   "{id:category}Datalog": "Datalog"
 }

libs/datalog/datalog.ts

@@ -3,13 +3,16 @@ namespace datalog {
     let _headers: string[] = undefined;
     let _headersLength: number;
     let _values: number[];
+    let _buffer: string = "";
     let _start: number;
-    let _filename = "data.csv";
+    let _filename = "datalog.csv";
     let _storage: storage.Storage = storage.temporary;
+    let _enabled = true;
 
     function clear() {
         _headers = undefined;        
         _values = undefined;
+        _buffer = "";
     }
 
     function init() {
@@ -31,7 +34,10 @@ namespace datalog {
                 _headersLength = _storage.size(_filename);
             }
             // commit row data
-            _storage.appendCSV(_filename, _values);
+            _buffer += storage.toCSV(_values, _storage.csvSeparator);
+            // buffered writes
+            if (_buffer.length > 1024)
+                flush();
         }
 
         // clear values
@@ -44,6 +50,8 @@ namespace datalog {
     //% weight=100
     //% blockId=datalogAddRow block="datalog add row"
     export function addRow(): void {
+        if (!_enabled) return;
+        
         commit();
         init();
         const s = (control.millis() - _start) / 1000;
@@ -65,16 +73,16 @@ namespace datalog {
             i = _headers.length - 1;
         }
         _values[i] = value;
-        if (i > 0) // 0 is time
-            console.logValue(name, value)
     }
 
     /**
      * Starts a new data logger for the given file
+     * @param filename the filename, eg: "datalog.csv"
      */
     //%
-    export function setFile(fn: string) {
+    export function setFile(filename: string) {
-        _filename = fn;
+        flush();
+        _filename = filename;
         clear();
     }
 
@@ -84,7 +92,31 @@ namespace datalog {
      */
     //%
     export function setStorage(storage: storage.Storage) {
+        flush();
         _storage = storage;
         clear();
     }
+
+    /**
+     * Commits any buffered row to disk
+     */
+    //%
+    export function flush() {
+        if (_buffer) {
+            const b = _buffer;
+            _buffer = "";
+            _storage.append(_filename, b);
+        }
+    }
+
+    /**
+     * Turns on or off datalogging
+     * @param enabled 
+     */
+    //% blockId=datalogEnabled block="datalog %enabled"
+    //% enabled.fieldEditor=fieldonoff
+    export function setEnabled(enabled: boolean) {
+        flush();
+        _enabled = enabled;
+    }
 }

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

@@ -1,5 +1,7 @@
 {
   "sensors.GyroSensor.angle": "Get the current angle from the gyroscope.",
+  "sensors.GyroSensor.drift": "Gets the computed rate drift",
   "sensors.GyroSensor.rate": "Get the current rotation rate from the gyroscope.",
-  "sensors.GyroSensor.reset": "Forces a calibration of the gyro. Must be called when the sensor is completely still."
+  "sensors.GyroSensor.reset": "Forces a calibration of the gyro. Must be called when the sensor is completely still.",
+  "sensors.GyroSensor.setDriftCorrection": "Enables or disable drift correction"
 }

libs/gyro-sensor/gyro.ts

@@ -8,16 +8,27 @@ namespace sensors {
     //% fixedInstances
     export class GyroSensor extends internal.UartSensor {
         private calibrating: boolean;
+        private _drift: number;
+        private _drifting: boolean;
         constructor(port: number) {
             super(port)
             this.calibrating = false;
+            this._drift = 0;
+            this._drifting = true;
+            this.setMode(GyroSensorMode.Rate);
         }
 
         _deviceType() {
             return DAL.DEVICE_TYPE_GYRO
         }
 
+        _query(): number {
+            return this.getNumber(NumberFormat.Int16LE, 0);
+        }
+
         setMode(m: GyroSensorMode) {
+            if (m == GyroSensorMode.Rate && this.mode != m)
+                this._drift = 0;
             this._setMode(m)
         }
 
@@ -37,8 +48,8 @@ namespace sensors {
             if (this.calibrating)
                 pauseUntil(() => !this.calibrating, 2000);
 
-            this.setMode(GyroSensorMode.Angle)
+            this.setMode(GyroSensorMode.Angle);
-            return this.getNumber(NumberFormat.Int16LE, 0)
+            return this._query();
         }
 
         /**
@@ -57,8 +68,14 @@ namespace sensors {
             if (this.calibrating)
                 pauseUntil(() => !this.calibrating, 2000);
 
-            this.setMode(GyroSensorMode.Rate)
+            this.setMode(GyroSensorMode.Rate);
-            return this.getNumber(NumberFormat.Int16LE, 0)
+            let curr = this._query();
+            if (Math.abs(curr) < 20) {
+                const p = 0.0005;
+                this._drift = (1 - p) * this._drift + p * curr;
+                curr -= this._drift;
+            }
+            return curr;
         }
 
         /**
@@ -76,24 +93,46 @@ namespace sensors {
             if (this.calibrating) return; // already in calibration mode
 
             this.calibrating = true;
-            // may be triggered by a button click, give time to settle
+            // may be triggered by a button click, 
-            loops.pause(500);
+            // give time for robot to settle
+            loops.pause(700);
             // send a reset command
             super.reset();
-            // we need to switch mode twice to perform a calibration
+            // switch back to the desired mode
-            if (this.mode == GyroSensorMode.Rate)
+            this.setMode(this.mode);
-                this.setMode(GyroSensorMode.Angle);
+            // wait till sensor is live
-            else
+            pauseUntil(() => this.isActive());
-                this.setMode(GyroSensorMode.Rate);
+            // give it a bit of time to init
-            // switch back and wait
+            loops.pause(1000)
-            if (this.mode == GyroSensorMode.Rate)
+            // compute drift
-                this.setMode(GyroSensorMode.Angle);
+            this._drift = 0;
-            else
+            if (this.mode == GyroSensorMode.Rate) {
-                this.setMode(GyroSensorMode.Rate);
+                for (let i = 0; i < 200; ++i) {
-            // give it more time to settle
+                    this._drift += this._query();
-            loops.pause(500);
+                    loops.pause(4);
+                }
+                this._drift /= 200;
+            }
+            // and we're done
             this.calibrating = false;
         }
+
+        /**
+         * Gets the computed rate drift
+         */
+        //%
+        drift(): number {
+            return this._drift;
+        }
+
+        /**
+         * Enables or disable drift correction
+         * @param enabled 
+         */
+        //%
+        setDriftCorrection(enabled: boolean) {
+            this._drifting = enabled;
+        }
     }
 
     //% fixedInstance whenUsed block="gyro 2" weight=95 jres=icons.port2

libs/storage/storage-core.ts

@@ -86,12 +86,7 @@ namespace storage {
          */
         //% blockId=storageAppendCSV block="storage %source|%filename|append CSV %data"
         appendCSV(filename: string, data: number[]) {
-            let s = ""
+            let s = toCSV(data, this.csvSeparator);
-            for (const d of data) {
-                if (s) s += this.csvSeparator;
-                s = s + d;
-            }
-            s += "\r\n"
             this.append(filename, s)
         }
 
@@ -167,6 +162,16 @@ namespace storage {
         }
     }
 
+    export function toCSV(data: number[], sep: string) {
+        let s = ""
+        for (const d of data) {
+            if (s) s += sep;
+            s = s + d;
+        }
+        s += "\r\n"
+        return s;
+    }
+
     class TemporaryStorage extends Storage {
         constructor() {
             super();

package.json

@@ -44,7 +44,7 @@
     "webfonts-generator": "^0.4.0"
   },
   "dependencies": {
-    "pxt-common-packages": "0.15.5",
+    "pxt-common-packages": "0.15.6",
     "pxt-core": "3.0.11"
   },
   "scripts": {