From fe59c958c4c7eef29f27de053d73995ab881c6c8 Mon Sep 17 00:00:00 2001 From: Leo Bonnargent Date: Thu, 21 Nov 2019 21:08:26 +0100 Subject: [PATCH 1/3] Attempt to read differents device modes --- src/consts.ts | 28 +++++ src/hub.ts | 12 ++- src/lpf2hub.ts | 285 ++++++++++++++++++++++++++++++++++--------------- src/port.ts | 2 + src/utils.ts | 3 + 5 files changed, 240 insertions(+), 90 deletions(-) diff --git a/src/consts.ts b/src/consts.ts index 4a853e04..def520db 100644 --- a/src/consts.ts +++ b/src/consts.ts @@ -186,3 +186,31 @@ export enum BLECharacteristic { WEDO2_NAME_ID = "00001524-1212-efde-1523-785feabcd123", // "1524" LPF2_ALL = "00001624-1212-efde-1623-785feabcd123" } + +export enum ColorAndDistanceInputModes { + COLOR = 0, + PROX = 1, + COUNT = 2, + REFLT = 3, + AMBI = 4, + RGB_I = 6, + SPEC_1 = 8 +} + +export enum BoostTiltModes { + ANGLE = 0, + TILT = 1, + ORINT = 2, + IMPCT = 3, + ACCEL = 4, + OR_CF = 5, + IM_CF = 6, + CALIB = 7 +} + +export enum MotorModes { + POWER = 0, + SPEED = 1, + POS = 2, + APOS = 3 +} diff --git a/src/hub.ts b/src/hub.ts index 684fd5c6..723113a9 100644 --- a/src/hub.ts +++ b/src/hub.ts @@ -157,11 +157,15 @@ export class Hub extends EventEmitter { */ public subscribe (port: string, mode?: number) { return new Promise((resolve, reject) => { - let newMode = this._getModeForDeviceType(this._portLookup(port).type); + const { type, value } = this._portLookup(port); + + let newMode = this._getModeForDeviceType(type); if (mode !== undefined) { newMode = mode; } - this._activatePortDevice(this._portLookup(port).value, this._portLookup(port).type, newMode, 0x00, () => { + + this._ports[port].mode = newMode; + this._activatePortDevice(value, type, newMode, 0x00, () => { return resolve(); }); }); @@ -175,8 +179,8 @@ export class Hub extends EventEmitter { */ public unsubscribe (port: string) { return new Promise((resolve, reject) => { - const mode = this._getModeForDeviceType(this._portLookup(port).type); - this._deactivatePortDevice(this._portLookup(port).value, this._portLookup(port).type, mode, 0x00, () => { + const { type, value, mode } = this._portLookup(port); + this._deactivatePortDevice(value, type, mode || this._getModeForDeviceType(type), 0x00, () => { return resolve(); }); }); diff --git a/src/lpf2hub.ts b/src/lpf2hub.ts index d8393d11..8122986a 100644 --- a/src/lpf2hub.ts +++ b/src/lpf2hub.ts @@ -4,7 +4,7 @@ import { Hub } from "./hub"; import { Port } from "./port"; import * as Consts from "./consts"; -import { toBin, toHex } from "./utils"; +import { toBin, toDistance, toHex } from "./utils"; import Debug = require("debug"); const debug = Debug("lpf2hub"); @@ -456,90 +456,14 @@ export class LPF2Hub extends Hub { break; } case Consts.DeviceType.BOOST_DISTANCE: { - - /** - * Emits when a color sensor is activated. - * @event LPF2Hub#color - * @param {string} port - * @param {Color} color - */ - if (data[4] <= 10) { - this.emit("color", port.id, data[4]); - } - - let distance = data[5]; - const partial = data[7]; - - if (partial > 0) { - distance += 1.0 / partial; - } - - distance = Math.floor(distance * 25.4) - 20; - - this.emit("distance", port.id, distance); - - /** - * A combined color and distance event, emits when the sensor is activated. - * @event LPF2Hub#colorAndDistance - * @param {string} port - * @param {Color} color - * @param {number} distance Distance, in millimeters. - */ - if (data[4] <= 10) { - this.emit("colorAndDistance", port.id, data[4], distance); - } - break; - } - case Consts.DeviceType.WEDO2_TILT: { - const tiltX = data.readInt8(4); - const tiltY = data.readInt8(5); - this._lastTiltX = tiltX; - this._lastTiltY = tiltY; - /** - * Emits when a tilt sensor is activated. - * @event LPF2Hub#tilt - * @param {string} port If the event is fired from the Move Hub or Control+ Hub's in-built tilt sensor, the special port "TILT" is used. - * @param {number} x - * @param {number} y - * @param {number} z (Only available when using a Control+ Hub) - */ - this.emit("tilt", port.id, this._lastTiltX, this._lastTiltY, this._lastTiltZ); - break; - } - case Consts.DeviceType.BOOST_TACHO_MOTOR: { - const rotation = data.readInt32LE(4); - /** - * Emits when a rotation sensor is activated. - * @event LPF2Hub#rotate - * @param {string} port - * @param {number} rotation - */ - this.emit("rotate", port.id, rotation); - break; - } - case Consts.DeviceType.BOOST_MOVE_HUB_MOTOR: { - const rotation = data.readInt32LE(4); - this.emit("rotate", port.id, rotation); - break; - } - case Consts.DeviceType.CONTROL_PLUS_LARGE_MOTOR: { - const rotation = data.readInt32LE(4); - this.emit("rotate", port.id, rotation); + this._parseBoostColorAndDistance(port, data); break; } + case Consts.DeviceType.BOOST_TACHO_MOTOR: + case Consts.DeviceType.BOOST_MOVE_HUB_MOTOR: + case Consts.DeviceType.CONTROL_PLUS_LARGE_MOTOR: case Consts.DeviceType.CONTROL_PLUS_XLARGE_MOTOR: { - const rotation = data.readInt32LE(4); - this.emit("rotate", port.id, rotation); - break; - } - case Consts.DeviceType.CONTROL_PLUS_TILT: { - const tiltZ = data.readInt16LE(4); - const tiltY = data.readInt16LE(6); - const tiltX = data.readInt16LE(8); - this._lastTiltX = tiltX; - this._lastTiltY = tiltY; - this._lastTiltZ = tiltZ; - this.emit("tilt", "TILT", this._lastTiltX, this._lastTiltY, this._lastTiltZ); + this._parseMotor(port, data); break; } case Consts.DeviceType.CONTROL_PLUS_ACCELEROMETER: { @@ -557,12 +481,19 @@ export class LPF2Hub extends Hub { this.emit("accel", "ACCEL", accelX, accelY, accelZ); break; } - case Consts.DeviceType.BOOST_TILT: { - const tiltX = data.readInt8(4); - const tiltY = data.readInt8(5); + case Consts.DeviceType.CONTROL_PLUS_TILT: { + const tiltZ = data.readInt16LE(4); + const tiltY = data.readInt16LE(6); + const tiltX = data.readInt16LE(8); this._lastTiltX = tiltX; this._lastTiltY = tiltY; - this.emit("tilt", port.id, this._lastTiltX, this._lastTiltY, this._lastTiltZ); + this._lastTiltZ = tiltZ; + this.emit("tilt", "TILT", this._lastTiltX, this._lastTiltY, this._lastTiltZ); + break; + } + case Consts.DeviceType.WEDO2_TILT: + case Consts.DeviceType.BOOST_TILT: { + this._parseBoostTilt(port, data); break; } case Consts.DeviceType.POWERED_UP_REMOTE_BUTTON: { @@ -608,5 +539,187 @@ export class LPF2Hub extends Hub { } + private _parseBoostColorAndDistance(port: Port, data: Buffer) { + // store common data buffer indices + const commons: { [dataType: string]: number } = {}; + + switch (port.mode) { + case Consts.ColorAndDistanceInputModes.COLOR: { + commons.color = 4; + break; + } + + case Consts.ColorAndDistanceInputModes.PROX: { + commons.distance = 4; + break; + } + + case Consts.ColorAndDistanceInputModes.SPEC_1: { + commons.color = 4; + commons.distance = 5; + commons.partial = 7; + break; + } + } + + const color = commons.color ? data[commons.color] : undefined; + const partial = commons.partial ? data[commons.partial] : undefined; + const distance = commons.distance ? toDistance(data[commons.distance], partial) : undefined; + + if (color !== undefined) { + /** + * Emits when a color sensor is activated. + * @event LPF2Hub#color + * @param {string} port + * @param {Color} color + */ + this.emit("color", port.id, color); + } + + if (distance !== undefined) { + /** + * Emits when a distance sensor is activated. + * @event LPF2Hub#distance + * @param {string} port + * @param {number} distance Distance, in millimeters. + */ + this.emit("distance", port.id, distance); + } + + if (color !== undefined && distance !== undefined) { + /** + * A combined color and distance event, emits when the sensor is activated. + * @event LPF2Hub#colorAndDistance + * @param {string} port + * @param {Color} color + * @param {number} distance Distance, in millimeters. + */ + this.emit("colorAndDistance", port.id, color, distance); + } + + if (port.mode === Consts.ColorAndDistanceInputModes.COUNT) { + /** + * Emits when a count mode distance sensor is activated. + * @event LPF2Hub#count + * @param {string} port + * @param {number} count + */ + this.emit("count", port.id, data[4]); + } + + if (port.mode === Consts.ColorAndDistanceInputModes.REFLT) { + /** + * Emits when a reflect mode color sensor is activated. + * @event LPF2Hub#reflectivity + * @param {string} port + * @param {number} reflectivity + */ + this.emit("reflectivity", port.id, data[4]); + } + + if (port.mode === Consts.ColorAndDistanceInputModes.AMBI) { + /** + * Emits when a ambiant mode color sensor is activated. + * @event LPF2Hub#luminosity + * @param {string} port + * @param {number} luminosity + */ + this.emit("luminosity", port.id, data[4]); + } + + if (port.mode === Consts.ColorAndDistanceInputModes.RGB_I) { + /** + * Emits when a RGB mode color sensor is activated. + * @event LPF2Hub#rgb + * @param {string} port + * @param {number} r + * @param {number} g + * @param {number} b + */ + this.emit("rgb", port.id, data[4], data[6], data[8]); + } + } + + private _parseBoostTilt(port: Port, data: Buffer) { + const values: number[] = []; + let event = ''; + + switch (port.mode) { + case Consts.BoostTiltModes.ANGLE: { + values.push(data.readInt8(4)); + values.push(data.readInt8(5)); + event = 'angle'; + break; + } + + case Consts.BoostTiltModes.TILT: { + values.push(data[4]); + event = 'tilt'; + break; + } + + case Consts.BoostTiltModes.ORINT: { + values.push(data[4]); + event = 'orientation'; + break; + } + + case Consts.BoostTiltModes.IMPCT: { + values.push(data.readUInt32BE(4)); + event = 'impact'; + break; + } + + case Consts.BoostTiltModes.ACCEL: { + /** + * Emits when a tilt sensor is activated. + * @event LPF2Hub#tilt + * @param {string} port If the event is fired from the Move Hub or Control+ Hub's in-built tilt sensor, the special port "TILT" is used. + * @param {number} x + * @param {number} y + * @param {number} z + */ + values.push(data.readInt8(4)); + values.push(data.readInt8(5)); + values.push(data.readInt8(6)); + event = 'accel'; + break; + } + } + + this.emit(event, port.id, ...values); + } + + private _parseMotor(port: Port, data: Buffer) { + const values: number[] = []; + let event = ''; + + switch (port.mode) { + case Consts.MotorModes.POWER: { + values.push(data.readInt8(4)); + event = 'power'; + break; + } + + case Consts.MotorModes.SPEED: { + values.push(data.readInt8(4)); + event = 'speed'; + break; + } + + case Consts.MotorModes.POS: { + values.push(data.readInt32BE(4)); + event = 'rotate'; + break; + } + + case Consts.MotorModes.APOS: { + values.push(data.readInt16BE(4)); + event = 'absoluteRotate'; + break; + } + } + this.emit(event, port.id, ...values); + } } diff --git a/src/port.ts b/src/port.ts index 0a279cb2..4a72513a 100644 --- a/src/port.ts +++ b/src/port.ts @@ -7,6 +7,7 @@ export class Port { public id: string; public value: number; public type: Consts.DeviceType; + public mode: number | null; public connected: boolean = false; public busy: boolean = false; public finished: (() => void) | null = null; @@ -17,6 +18,7 @@ export class Port { this.id = id; this.value = value; this.type = Consts.DeviceType.UNKNOWN; + this.mode = null; } public cancelEventTimer () { diff --git a/src/utils.ts b/src/utils.ts index 83650aa1..860eb88c 100644 --- a/src/utils.ts +++ b/src/utils.ts @@ -7,3 +7,6 @@ export function toHex (value: number, length: number = 2) { export function toBin (value: number, length: number = 8) { return value.toString(2).padStart(length, "0"); } +export function toDistance(value: number, partial: number = 1) { + return Math.floor((value + 1 / Math.max(partial, 1)) * 25.4) - 20; +} From 3bc8404d61761c5a72800f1f2703c75738c995ce Mon Sep 17 00:00:00 2001 From: Leo Bonnargent Date: Fri, 22 Nov 2019 07:42:02 +0100 Subject: [PATCH 2/3] Reset port mode on detach --- src/hub.ts | 1 + 1 file changed, 1 insertion(+) diff --git a/src/hub.ts b/src/hub.ts index 723113a9..a1c2c946 100644 --- a/src/hub.ts +++ b/src/hub.ts @@ -283,6 +283,7 @@ export class Hub extends EventEmitter { } } else { port.type = Consts.DeviceType.UNKNOWN; + port.mode = null; debug(`Port ${port.id} disconnected`); /** * Emits when an attached motor or sensor is detached from the Hub. From 5f041e6faf561c2f1ac03ac336832ee4cda043db Mon Sep 17 00:00:00 2001 From: Leo Bonnargent Date: Fri, 22 Nov 2019 07:51:41 +0100 Subject: [PATCH 3/3] Fix rgb color reading --- src/lpf2hub.ts | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lpf2hub.ts b/src/lpf2hub.ts index 8122986a..0a45029e 100644 --- a/src/lpf2hub.ts +++ b/src/lpf2hub.ts @@ -636,7 +636,7 @@ export class LPF2Hub extends Hub { * @param {number} g * @param {number} b */ - this.emit("rgb", port.id, data[4], data[6], data[8]); + this.emit("rgb", port.id, data.readInt16LE(4), data.readInt16LE(6), data.readInt16LE(8)); } }