From 5f863adaf7431c10d7aff617e1d933c4f8a16c49 Mon Sep 17 00:00:00 2001 From: Peli de Halleux Date: Sat, 19 Mar 2016 19:15:20 -0700 Subject: [PATCH] better support for simulating gestures --- .vscode/tasks.json | 20 +++ sim/enums.ts | 19 ++- sim/libmbit.ts | 19 +-- sim/simsvg.ts | 37 ++--- sim/state.ts | 370 +++++++++++++++++++++++++++++++++++++++++++-- 5 files changed, 422 insertions(+), 43 deletions(-) create mode 100644 .vscode/tasks.json diff --git a/.vscode/tasks.json b/.vscode/tasks.json new file mode 100644 index 00000000..0ed9a5be --- /dev/null +++ b/.vscode/tasks.json @@ -0,0 +1,20 @@ +{ + "version": "0.1.0", + // Task runner is jake + "command": "kind", + // Need to be executed in shell / cmd + "isShellCommand": true, + "showOutput": "always", + "tasks": [ + { + // TS build command is local. + "taskName": "serve", + // Make this the default build command. + "isBuildCommand": true, + // Use the redefined Typescript output problem matcher. + "problemMatcher": [ + "$tsc" + ] + } + ] +} diff --git a/sim/enums.ts b/sim/enums.ts index 8a76a2c3..e4a98a63 100644 --- a/sim/enums.ts +++ b/sim/enums.ts @@ -78,5 +78,22 @@ namespace ks.rt.micro_bit { MICROBIT_ID_RADIO: number; MICROBIT_RADIO_EVT_DATAGRAM: number; MICROBIT_ID_GESTURE: number; - } + MICROBIT_ACCELEROMETER_REST_TOLERANCE: number; + MICROBIT_ACCELEROMETER_TILT_TOLERANCE: number; + MICROBIT_ACCELEROMETER_FREEFALL_TOLERANCE: number; + MICROBIT_ACCELEROMETER_SHAKE_TOLERANCE: number; + MICROBIT_ACCELEROMETER_3G_TOLERANCE: number; + MICROBIT_ACCELEROMETER_6G_TOLERANCE: number; + MICROBIT_ACCELEROMETER_8G_TOLERANCE: number; + MICROBIT_ACCELEROMETER_GESTURE_DAMPING: number; + MICROBIT_ACCELEROMETER_SHAKE_DAMPING: number; + MICROBIT_ACCELEROMETER_REST_THRESHOLD: number; + MICROBIT_ACCELEROMETER_FREEFALL_THRESHOLD: number; + MICROBIT_ACCELEROMETER_3G_THRESHOLD: number; + MICROBIT_ACCELEROMETER_6G_THRESHOLD: number; + MICROBIT_ACCELEROMETER_8G_THRESHOLD: number; + MICROBIT_ACCELEROMETER_SHAKE_COUNT_THRESHOLD: number; + MICROBIT_ACCELEROMETER_EVT_DATA_UPDATE: number; + MICROBIT_ID_ACCELEROMETER: number; + } } \ No newline at end of file diff --git a/sim/libmbit.ts b/sim/libmbit.ts index d77ada4e..17cf2181 100644 --- a/sim/libmbit.ts +++ b/sim/libmbit.ts @@ -237,6 +237,7 @@ namespace ks.rt.micro_bit { export function onGesture(gesture: number, handler: RefAction) { let ens = enums(); let b = board(); + b.accelerometer.activate(); if (gesture == 11 && !b.useShake) { // SAKE b.useShake = true; @@ -294,23 +295,19 @@ namespace ks.rt.micro_bit { export function getAcceleration(dimension: number): number { let b = board(); - if (!b.usesAcceleration) { - b.usesAcceleration = true; - runtime.queueDisplayUpdate(); - } - let acc = b.acceleration; + let acc = b.accelerometer; + acc.activate(); switch (dimension) { - case 0: return acc[0]; - case 1: return acc[1]; - case 2: return acc[2]; - default: return Math.sqrt(acc[0] * acc[0] + acc[1] * acc[1] + acc[2] * acc[2]); + case 0: return acc.getX(); + case 1: return acc.getY(); + case 2: return acc.getZ(); + default: return Math.floor(Math.sqrt(acc.instantaneousAccelerationSquared())); } } export function setAccelerometerRange(range : number) { let b = board(); - b.accelerometerRange = Math.max(1, Math.min(8, range)); - runtime.queueDisplayUpdate(); + b.accelerometer.setSampleRange(range); } export function lightLevel(): number { diff --git a/sim/simsvg.ts b/sim/simsvg.ts index 737fe65a..c478ac1a 100644 --- a/sim/simsvg.ts +++ b/sim/simsvg.ts @@ -436,15 +436,15 @@ namespace ks.rt.micro_bit { private updateTilt() { if (this.props.disableTilt) return; let state = this.board; - if (!state || !state.usesAcceleration) return; + if (!state || !state.accelerometer.isActive) return; - var acc = state.acceleration; - var af = 8 / 1023; - if(acc && !isNaN(acc[0]) && !isNaN(acc[1])) { - this.element.style.transform = "perspective(30em) rotateX(" + -acc[1]*af + "deg) rotateY(" + acc[0]*af +"deg)" - this.element.style.perspectiveOrigin = "50% 50% 50%"; - this.element.style.perspective = "30em"; - } + let x = state.accelerometer.getX(); + let y = state.accelerometer.getY(); + let af = 8 / 1023; + + this.element.style.transform = "perspective(30em) rotateX(" + y*af + "deg) rotateY(" + x*af +"deg)" + this.element.style.perspectiveOrigin = "50% 50% 50%"; + this.element.style.perspective = "30em"; } private buildDom() { @@ -557,23 +557,24 @@ namespace ks.rt.micro_bit { } this.element.addEventListener("mousemove", (ev: MouseEvent) => { let state = this.board; - if (!state.acceleration) return; + if (!state.accelerometer.isActive) return; + let ax = (ev.clientX - this.element.clientWidth / 2) / (this.element.clientWidth / 3); let ay = (ev.clientY - this.element.clientHeight / 2) / (this.element.clientHeight / 3); - state.acceleration[0] = Math.max(-1023, Math.min(1023, Math.floor(ax * 1023))); - state.acceleration[1] = Math.max(-1023, Math.min(1023, Math.floor(ay * 1023))); - state.acceleration[2] = Math.floor(Math.sqrt(1023*1023 - - state.acceleration[0] *state.acceleration[0] - - state.acceleration[1] *state.acceleration[1])); + + let x = - Math.max(- 1023, Math.min(1023, Math.floor(ax * 1023))); + let y = Math.max(- 1023, Math.min(1023, Math.floor(ay * 1023))); + let z2 = 1023*1023 - x * x - y * y; + let z = Math.floor((z2 > 0 ? -1 : 1)* Math.sqrt(Math.abs(z2))); + + state.accelerometer.update(x,y,z); this.updateTilt(); }, false); this.element.addEventListener("mouseleave", (ev: MouseEvent) => { let state = this.board; - if (!state.acceleration) return; + if (!state.accelerometer.isActive) return; - state.acceleration[0] = 0; - state.acceleration[1] = 0; - state.acceleration[2] = -1023; + state.accelerometer.update(0,0,-1023); this.updateTilt(); }, false); diff --git a/sim/state.ts b/sim/state.ts index e4c5a767..0ca1aef6 100644 --- a/sim/state.ts +++ b/sim/state.ts @@ -108,7 +108,7 @@ namespace ks.rt.micro_bit { setGroup(id: number) { this.groupId = id & 0xff; // byte only } - + setTransmitPower(power: number) { this.power = Math.max(0, Math.min(7, power)); } @@ -125,6 +125,351 @@ namespace ks.rt.micro_bit { } } + export enum BasicGesture { + GESTURE_NONE, + GESTURE_UP, + GESTURE_DOWN, + GESTURE_LEFT, + GESTURE_RIGHT, + GESTURE_FACE_UP, + GESTURE_FACE_DOWN, + GESTURE_FREEFALL, + GESTURE_3G, + GESTURE_6G, + GESTURE_8G, + GESTURE_SHAKE + }; + + interface AccelerometerSample { + x: number; + y: number; + z: number; + } + + interface ShakeHistory { + x: boolean; + y: boolean; + z: boolean; + count: number; + shaken: number; + timer: number; + } + + /** + * Co-ordinate systems that can be used. + * RAW: Unaltered data. Data will be returned directly from the accelerometer. + * + * SIMPLE_CARTESIAN: Data will be returned based on an easy to understand alignment, consistent with the cartesian system taught in schools. + * When held upright, facing the user: + * + * / + * +--------------------+ z + * | | + * | ..... | + * | * ..... * | + * ^ | ..... | + * | | | + * y +--------------------+ x--> + * + * + * NORTH_EAST_DOWN: Data will be returned based on the industry convention of the North East Down (NED) system. + * When held upright, facing the user: + * + * z + * +--------------------+ / + * | | + * | ..... | + * | * ..... * | + * ^ | ..... | + * | | | + * x +--------------------+ y--> + * + */ + export enum MicroBitCoordinateSystem { + RAW, + SIMPLE_CARTESIAN, + NORTH_EAST_DOWN + } + + export class Accelerometer { + private sigma: number = 0; // the number of ticks that the instantaneous gesture has been stable. + private lastGesture: BasicGesture = BasicGesture.GESTURE_NONE; // the last, stable gesture recorded. + private currentGesture: BasicGesture = BasicGesture.GESTURE_NONE; // the instantaneous, unfiltered gesture detected. + private sample: AccelerometerSample = { x: 0, y: 0, z: -1023 } + private shake: ShakeHistory = { x: false, y: false, z: false, count: 0, shaken: 0, timer: 0 }; // State information needed to detect shake events. + private pitch:number; + private roll:number; + private id: number; + public isActive = false; + public sampleRange = 2; + + constructor(public runtime: Runtime) { + this.id = (runtime.enums).MICROBIT_ID_ACCELEROMETER; + } + + public setSampleRange(range : number) { + this.activate(); + this.sampleRange = Math.max(1, Math.min(8, range)); + } + + public activate() { + if (!this.isActive) { + this.isActive = true; + this.runtime.queueDisplayUpdate(); + } + } + + /** + * Reads the acceleration data from the accelerometer, and stores it in our buffer. + * This is called by the tick() member function, if the interrupt is set! + */ + public update(x : number, y : number, z : number) { + // read MSB values... + this.sample.x = Math.floor(x); + this.sample.y = Math.floor(y); + this.sample.z = Math.floor(z); + + // Update gesture tracking + this.updateGesture(); + + // Indicate that a new sample is available + board().bus.queue(this.id, enums().MICROBIT_ACCELEROMETER_EVT_DATA_UPDATE) + } + + public instantaneousAccelerationSquared() { + // Use pythagoras theorem to determine the combined force acting on the device. + return this.sample.x * this.sample.x + this.sample.y * this.sample.y + this.sample.z * this.sample.z; + } + + /** + * Service function. Determines the best guess posture of the device based on instantaneous data. + * This makes no use of historic data (except for shake), and forms this input to the filter implemented in updateGesture(). + * + * @return A best guess of the current posture of the device, based on instantaneous data. + */ + private instantaneousPosture(): BasicGesture { + let ens = enums() + let force = this.instantaneousAccelerationSquared(); + let shakeDetected = false; + + // Test for shake events. + // We detect a shake by measuring zero crossings in each axis. In other words, if we see a strong acceleration to the left followed by + // a string acceleration to the right, then we can infer a shake. Similarly, we can do this for each acxis (left/right, up/down, in/out). + // + // If we see enough zero crossings in succession (MICROBIT_ACCELEROMETER_SHAKE_COUNT_THRESHOLD), then we decide that the device + // has been shaken. + if ((this.getX() < -ens.MICROBIT_ACCELEROMETER_SHAKE_TOLERANCE && this.shake.x) || (this.getX() > ens.MICROBIT_ACCELEROMETER_SHAKE_TOLERANCE && !this.shake.x)) { + shakeDetected = true; + this.shake.x = !this.shake.x; + } + + if ((this.getY() < -ens.MICROBIT_ACCELEROMETER_SHAKE_TOLERANCE && this.shake.y) || (this.getY() > ens.MICROBIT_ACCELEROMETER_SHAKE_TOLERANCE && !this.shake.y)) { + shakeDetected = true; + this.shake.y = !this.shake.y; + } + + if ((this.getZ() < -ens.MICROBIT_ACCELEROMETER_SHAKE_TOLERANCE && this.shake.z) || (this.getZ() > ens.MICROBIT_ACCELEROMETER_SHAKE_TOLERANCE && !this.shake.z)) { + shakeDetected = true; + this.shake.z = !this.shake.z; + } + + if (shakeDetected && this.shake.count < ens.MICROBIT_ACCELEROMETER_SHAKE_COUNT_THRESHOLD && ++this.shake.count == ens.MICROBIT_ACCELEROMETER_SHAKE_COUNT_THRESHOLD) + this.shake.shaken = 1; + + if (++this.shake.timer >= ens.MICROBIT_ACCELEROMETER_SHAKE_DAMPING) { + this.shake.timer = 0; + if (this.shake.count > 0) { + if (--this.shake.count == 0) + this.shake.shaken = 0; + } + } + + if (this.shake.shaken) + return BasicGesture.GESTURE_SHAKE; + + if (force < ens.MICROBIT_ACCELEROMETER_FREEFALL_THRESHOLD) + return BasicGesture.GESTURE_FREEFALL; + + if (force > ens.MICROBIT_ACCELEROMETER_3G_THRESHOLD) + return BasicGesture.GESTURE_3G; + + if (force > ens.MICROBIT_ACCELEROMETER_6G_THRESHOLD) + return BasicGesture.GESTURE_6G; + + if (force > ens.MICROBIT_ACCELEROMETER_8G_THRESHOLD) + return BasicGesture.GESTURE_8G; + + // Determine our posture. + if (this.getX() < (-1000 + ens.MICROBIT_ACCELEROMETER_TILT_TOLERANCE)) + return BasicGesture.GESTURE_LEFT; + + if (this.getX() > (1000 - ens.MICROBIT_ACCELEROMETER_TILT_TOLERANCE)) + return BasicGesture.GESTURE_RIGHT; + + if (this.getY() < (-1000 + ens.MICROBIT_ACCELEROMETER_TILT_TOLERANCE)) + return BasicGesture.GESTURE_DOWN; + + if (this.getY() > (1000 - ens.MICROBIT_ACCELEROMETER_TILT_TOLERANCE)) + return BasicGesture.GESTURE_UP; + + if (this.getZ() < (-1000 + ens.MICROBIT_ACCELEROMETER_TILT_TOLERANCE)) + return BasicGesture.GESTURE_FACE_UP; + + if (this.getZ() > (1000 - ens.MICROBIT_ACCELEROMETER_TILT_TOLERANCE)) + return BasicGesture.GESTURE_FACE_DOWN; + + return BasicGesture.GESTURE_NONE; + } + + updateGesture() { + let ens = enums() + // Determine what it looks like we're doing based on the latest sample... + let g = this.instantaneousPosture(); + + // Perform some low pass filtering to reduce jitter from any detected effects + if (g == this.currentGesture) { + if (this.sigma < ens.MICROBIT_ACCELEROMETER_GESTURE_DAMPING) + this.sigma++; + } + else { + this.currentGesture = g; + this.sigma = 0; + } + + // If we've reached threshold, update our record and raise the relevant event... + if (this.currentGesture != this.lastGesture && this.sigma >= ens.MICROBIT_ACCELEROMETER_GESTURE_DAMPING) { + this.lastGesture = this.currentGesture; + board().bus.queue(ens.MICROBIT_ID_GESTURE, this.lastGesture); + } + } + + /** + * Reads the X axis value of the latest update from the accelerometer. + * @param system The coordinate system to use. By default, a simple cartesian system is provided. + * @return The force measured in the X axis, in milli-g. + * + * Example: + * @code + * uBit.accelerometer.getX(); + * uBit.accelerometer.getX(RAW); + * @endcode + */ + public getX(system : MicroBitCoordinateSystem = MicroBitCoordinateSystem.SIMPLE_CARTESIAN): number { + this.activate(); + switch (system) { + case MicroBitCoordinateSystem.SIMPLE_CARTESIAN: + return -this.sample.x; + + case MicroBitCoordinateSystem.NORTH_EAST_DOWN: + return this.sample.y; + //case MicroBitCoordinateSystem.SIMPLE_CARTESIAN.RAW: + default: + return this.sample.x; + } + } + + /** + * Reads the Y axis value of the latest update from the accelerometer. + * @param system The coordinate system to use. By default, a simple cartesian system is provided. + * @return The force measured in the Y axis, in milli-g. + * + * Example: + * @code + * uBit.accelerometer.getY(); + * uBit.accelerometer.getY(RAW); + * @endcode + */ + public getY(system : MicroBitCoordinateSystem = MicroBitCoordinateSystem.SIMPLE_CARTESIAN): number { + this.activate(); + switch (system) { + case MicroBitCoordinateSystem.SIMPLE_CARTESIAN: + return -this.sample.y; + + case MicroBitCoordinateSystem.NORTH_EAST_DOWN: + return -this.sample.x; + //case RAW: + default: + return this.sample.y; + } + } + + /** + * Reads the Z axis value of the latest update from the accelerometer. + * @param system The coordinate system to use. By default, a simple cartesian system is provided. + * @return The force measured in the Z axis, in milli-g. + * + * Example: + * @code + * uBit.accelerometer.getZ(); + * uBit.accelerometer.getZ(RAW); + * @endcode + */ + public getZ(system : MicroBitCoordinateSystem = MicroBitCoordinateSystem.SIMPLE_CARTESIAN): number { + this.activate(); + switch (system) { + case MicroBitCoordinateSystem.NORTH_EAST_DOWN: + return -this.sample.z; + //case MicroBitCoordinateSystem.SIMPLE_CARTESIAN: + //case MicroBitCoordinateSystem.RAW: + default: + return this.sample.z; + } + } + + /** + * Provides a rotation compensated pitch of the device, based on the latest update from the accelerometer. + * @return The pitch of the device, in degrees. + * + * Example: + * @code + * uBit.accelerometer.getPitch(); + * @endcode + */ + public getPitch(): number { + this.activate(); + return Math.floor((360 * this.getPitchRadians()) / (2 * Math.PI)); + } + + getPitchRadians() : number { + this.recalculatePitchRoll(); + return this.pitch; + } + + /** + * Provides a rotation compensated roll of the device, based on the latest update from the accelerometer. + * @return The roll of the device, in degrees. + * + * Example: + * @code + * uBit.accelerometer.getRoll(); + * @endcode + */ + public getRoll(): number { + this.activate(); + return Math.floor((360 * this.getRollRadians()) / (2 * Math.PI)); + } + + getRollRadians(): number { + this.recalculatePitchRoll(); + return this.roll; + } + + /** + * Recalculate roll and pitch values for the current sample. + * We only do this at most once per sample, as the necessary trigonemteric functions are rather + * heavyweight for a CPU without a floating point unit... + */ + recalculatePitchRoll() { + let x = this.getX(MicroBitCoordinateSystem.NORTH_EAST_DOWN); + let y = this.getY(MicroBitCoordinateSystem.NORTH_EAST_DOWN); + let z = this.getZ(MicroBitCoordinateSystem.NORTH_EAST_DOWN); + + this.roll = Math.atan2(y, z); + this.pitch = Math.atan(-x / (y * Math.sin(this.roll) + z * Math.cos(this.roll))); + } + + } + export interface SimulatorEventBusMessage extends SimulatorMessage { id: number; eventid: number; @@ -164,11 +509,9 @@ namespace ks.rt.micro_bit { // serial serialIn: string[] = []; - // sensors - usesAcceleration = false; - acceleration = [0, 0, -1023]; - accelerometerRange = 2; - + // sensors + accelerometer : Accelerometer; + // gestures useShake = false; @@ -189,6 +532,7 @@ namespace ks.rt.micro_bit { this.animationQ = new AnimationQueue(runtime); this.bus = new EventBus(runtime); this.radio = new RadioBus(runtime); + this.accelerometer = new Accelerometer(runtime); let ens = enums(); this.buttons = [ new Button(ens.MICROBIT_ID_BUTTON_A), @@ -310,17 +654,17 @@ namespace ks.rt.micro_bit { } } public shiftLeft(cols: number) { - for(let x = 0; x < this.width;++x) - for(let y = 0; y < 5;++y) - this.set(x, y, x < this.width - cols ? this.get(x + cols,y) : 0); + for (let x = 0; x < this.width; ++x) + for (let y = 0; y < 5; ++y) + this.set(x, y, x < this.width - cols ? this.get(x + cols, y) : 0); } public shiftRight(cols: number) { - for(let x = this.width -1; x <=0;--x) - for(let y = 0; y < 5;++y) - this.set(x, y, x > cols ? this.get(x - cols,y) : 0); + for (let x = this.width - 1; x <= 0; --x) + for (let y = 0; y < 5; ++y) + this.set(x, y, x > cols ? this.get(x - cols, y) : 0); } - + public clear(): void { for (var i = 0; i < this.data.length; ++i) this.data[i] = 0;