diff --git a/src/devices/absolutemotor.ts b/src/devices/absolutemotor.ts index e0700c0..7d89a0d 100644 --- a/src/devices/absolutemotor.ts +++ b/src/devices/absolutemotor.ts @@ -3,7 +3,7 @@ import { TachoMotor } from "./tachomotor"; import { IDeviceInterface } from "../interfaces"; import * as Consts from "../consts"; -import { mapSpeed } from "../utils"; +import { mapSpeed, normalizeAngle } from "../utils"; export class AbsoluteMotor extends TachoMotor { @@ -16,13 +16,13 @@ export class AbsoluteMotor extends TachoMotor { switch (mode) { case Mode.ABSOLUTE: - // const absolute = message.readInt32LE(this.isWeDo2SmartHub ? 2 : 4); - // /** - // * Emits when a the motors absolute position is changed. - // * @event AbsoluteMotor#absolute - // * @param {number} absolute - // */ - // this.emitGlobal("absolute", { absolute }); + const angle = normalizeAngle(message.readInt16LE(this.isWeDo2SmartHub ? 2 : 4)); + /** + * Emits when a the motors absolute position is changed. + * @event AbsoluteMotor#absolute + * @param {number} absolute + */ + this.emitGlobal("absolute", { angle }); break; default: super.receive(message); @@ -33,12 +33,12 @@ export class AbsoluteMotor extends TachoMotor { /** * Rotate a motor by a given angle. * @method AbsoluteMotor#gotoAbsolutePosition - * @param {number} position Absolute position the motor should go to (degrees from 0). + * @param {number} angle Absolute position the motor should go to (degrees from 0). * @param {number} [speed=100] For forward, a value between 1 - 100 should be set. For reverse, a value between -1 to -100. * @returns {Promise} Resolved upon successful completion of command (ie. once the motor is finished). */ - public gotoAbsolutePosition (position: [number, number] | number, speed: number = 100) { - if (!this.isVirtualPort && position instanceof Array) { + public gotoAbsoluteAngle (angle: [number, number] | number, speed: number = 100) { + if (!this.isVirtualPort && angle instanceof Array) { throw new Error("Only virtual ports can accept multiple positions"); } if (this.isWeDo2SmartHub) { @@ -50,16 +50,17 @@ export class AbsoluteMotor extends TachoMotor { speed = 100; } let message; - if (position instanceof Array) { + if (angle instanceof Array) { message = Buffer.from([0x81, this.portId, 0x11, 0x0e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, mapSpeed(speed), 0x64, 0x7f, 0x03]); - message.writeUInt32LE(position[0], 4); - message.writeUInt32LE(position[1], 8); + message.writeInt32LE(normalizeAngle(angle[0]), 4); + message.writeInt32LE(normalizeAngle(angle[1]), 8); } else { message = Buffer.from([0x81, this.portId, 0x11, 0x0d, 0x00, 0x00, 0x00, 0x00, mapSpeed(speed), 0x64, 0x7f, 0x03]); - message.writeUInt32LE(position, 4); + message.writeInt32LE(normalizeAngle(angle), 4); } this.send(message); this._finished = () => { + console.log("RESOLVE"); return resolve(); }; }); @@ -68,11 +69,11 @@ export class AbsoluteMotor extends TachoMotor { /** * (Re)set the knowledge of the absolute position to the current position. * @method AbsoluteMotor#resetAbsolutePosition - * @param {number} position Position to set (degrees from 0). + * @param {number} angle Position to set (degrees from 0). * @returns {Promise} Resolved upon successful completion of command (ie. once the motor is finished). */ - public resetAbsolutePosition (position: [number, number] | number) { - if (!this.isVirtualPort && position instanceof Array) { + public resetAbsoluteAngle (angle: [number, number] | number) { + if (!this.isVirtualPort && angle instanceof Array) { throw new Error("Only virtual ports can accept multiple positions"); } if (this.isWeDo2SmartHub) { @@ -81,13 +82,13 @@ export class AbsoluteMotor extends TachoMotor { return new Promise((resolve) => { this._busy = true; let message; - if (position instanceof Array) { + if (angle instanceof Array) { message = Buffer.from([0x81, this.portId, 0x11, 0x14, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00]); - message.writeUInt32LE(position[0], 4); - message.writeUInt32LE(position[0], 8); + message.writeInt32LE(normalizeAngle(angle[0]), 4); + message.writeInt32LE(normalizeAngle(angle[0]), 8); } else { message = Buffer.from([0x81, this.portId, 0x11, 0x51, 0x02, 0x00, 0x00, 0x00, 0x00]); - message.writeUInt32LE(position, 5); + message.writeInt32LE(normalizeAngle(angle), 5); } this.send(message); this._finished = () => { diff --git a/src/devices/device.ts b/src/devices/device.ts index fc34d28..2897abd 100644 --- a/src/devices/device.ts +++ b/src/devices/device.ts @@ -36,6 +36,7 @@ export class Device extends EventEmitter { } if (this.autoSubscribe) { if (this._modeMap[event] !== undefined) { + console.log(this._modeMap[event]); this.subscribe(this._modeMap[event]); } } diff --git a/src/devices/tachomotor.ts b/src/devices/tachomotor.ts index a675a5b..ffceeab 100644 --- a/src/devices/tachomotor.ts +++ b/src/devices/tachomotor.ts @@ -16,13 +16,13 @@ export class TachoMotor extends BasicMotor { switch (mode) { case Mode.ROTATION: - const rotation = message.readInt32LE(this.isWeDo2SmartHub ? 2 : 4); + const degrees = message.readInt32LE(this.isWeDo2SmartHub ? 2 : 4); /** * Emits when a rotation sensor is activated. * @event TachoMotor#rotate * @param {number} rotation */ - this.emitGlobal("rotate", { rotation }); + this.emitGlobal("rotate", { degrees }); break; } } @@ -68,18 +68,18 @@ export class TachoMotor extends BasicMotor { } /** - * Rotate a motor by a given angle. - * @method TachoMotor#rotateByAngle - * @param {number} angle How much the motor should be rotated (in degrees). + * Rotate a motor by a given amount of degrees. + * @method TachoMotor#rotateByDegrees + * @param {number} degrees How much the motor should be rotated (in degrees). * @param {number} [speed=100] For forward, a value between 1 - 100 should be set. For reverse, a value between -1 to -100. * @returns {Promise} Resolved upon successful completion of command (ie. once the motor is finished). */ - public rotateByAngle (angle: number, speed: [number, number] | number) { + public rotateByDegrees (degrees: number, speed: [number, number] | number) { if (!this.isVirtualPort && speed instanceof Array) { throw new Error("Only virtual ports can accept multiple speeds"); } if (this.isWeDo2SmartHub) { - throw new Error("Angle rotation is not available on the WeDo 2.0 Smart Hub"); + throw new Error("Rotation is not available on the WeDo 2.0 Smart Hub"); } return new Promise((resolve) => { this._busy = true; @@ -92,7 +92,7 @@ export class TachoMotor extends BasicMotor { } else { message = Buffer.from([0x81, this.portId, 0x11, 0x0b, 0x00, 0x00, 0x00, 0x00, mapSpeed(speed), 0x64, 0x7f, 0x03]); } - message.writeUInt32LE(angle, 4); + message.writeUInt32LE(degrees, 4); this.send(message); this._finished = () => { return resolve(); diff --git a/src/utils.ts b/src/utils.ts index 4bff8d5..4e330a4 100644 --- a/src/utils.ts +++ b/src/utils.ts @@ -29,3 +29,12 @@ export const decodeVersion = (version: number) => { export const decodeMACAddress = (address: Uint8Array) => { return Array.from(address).map((part) => toHex(part, 2)).join(":"); }; + +export const normalizeAngle = (angle: number) => { + if (angle >= 180) { + return angle - (360 * ((angle + 180) / 360)); + } else if (angle < -180) { + return angle + (360 * ((180 - angle) / 360)); + } + return angle; +};