From ffb5cd63afe9b5f26a1491618c6a03aac4f44b0d Mon Sep 17 00:00:00 2001 From: Kevin Andersen Date: Wed, 23 Jan 2019 09:10:03 +0100 Subject: [PATCH] added zero-block for motor positioning. Added turn-for-rotation-block, but its WIP. Modified generateOutputCommand to allow for advanced motor commands. Changed motor position reporter to follow hardware instead of wrap-clamping. --- package-lock.json | 8 +- src/extensions/scratch3_boost/index.js | 149 +++++++++++++++++++++++-- 2 files changed, 145 insertions(+), 12 deletions(-) diff --git a/package-lock.json b/package-lock.json index 2c477c800..17616be59 100644 --- a/package-lock.json +++ b/package-lock.json @@ -2177,7 +2177,7 @@ }, "bl": { "version": "1.2.2", - "resolved": "https://registry.npmjs.org/bl/-/bl-1.2.2.tgz", + "resolved": "http://registry.npmjs.org/bl/-/bl-1.2.2.tgz", "integrity": "sha512-e8tQYnZodmebYDWGH7KMRvtzKXaJHx3BbilrgZCfvyLUYdKpK1t5PSPmpkny/SgiTSCnjfLW7v5rlONXVFkQEA==", "dev": true, "requires": { @@ -4950,7 +4950,7 @@ "dependencies": { "commander": { "version": "2.1.0", - "resolved": "https://registry.npmjs.org/commander/-/commander-2.1.0.tgz", + "resolved": "http://registry.npmjs.org/commander/-/commander-2.1.0.tgz", "integrity": "sha1-0SG7roYNmZKj1Re6lvVliOR8Z4E=", "dev": true } @@ -7617,7 +7617,7 @@ }, "magic-string": { "version": "0.22.5", - "resolved": "https://registry.npmjs.org/magic-string/-/magic-string-0.22.5.tgz", + "resolved": "http://registry.npmjs.org/magic-string/-/magic-string-0.22.5.tgz", "integrity": "sha512-oreip9rJZkzvA8Qzk9HFs8fZGF/u7H/gtrE8EN6RjKJ9kh2HlC+yQ2QezifqTZfGyiuAV0dRv5a+y/8gBb1m9w==", "dev": true, "requires": { @@ -11645,7 +11645,7 @@ "dependencies": { "minimist": { "version": "1.2.0", - "resolved": "https://registry.npmjs.org/minimist/-/minimist-1.2.0.tgz", + "resolved": "http://registry.npmjs.org/minimist/-/minimist-1.2.0.tgz", "integrity": "sha1-o1AIsg9BOD7sH7kU9M1d95omQoQ=", "dev": true } diff --git a/src/extensions/scratch3_boost/index.js b/src/extensions/scratch3_boost/index.js index 2c7c9089d..292fa5a20 100644 --- a/src/extensions/scratch3_boost/index.js +++ b/src/extensions/scratch3_boost/index.js @@ -200,6 +200,13 @@ class BoostMotor { */ this._position = 0; + /** + * This motor's current relative position + * @type {number} + * @private + */ + this._positionZero = 0; + /** * Is this motor currently moving? * @type {boolean} @@ -296,6 +303,21 @@ class BoostMotor { this._position = value; } + /** + * @return {int} - + */ + get positionZero () { + return this._positionZero; + } + + /** + * @param {int} value - + */ + set positionZero (value) { + // Todo: wrap around rotation to avoid extremely large numbers + this._positionZero = value; + } + /** * @return {boolean} - true if this motor is currently moving, false if this motor is off or braking. */ @@ -349,6 +371,33 @@ class BoostMotor { this._setNewTimeout(this.startBraking, milliseconds); } + /** + * Turn this motor on for a specific rotation in degrees. + * @param {number} degrees - run the motor for this amount of degrees. + */ + turnOnForDegrees (degrees) { + if (this._power === 0) return; + console.log(degrees) + degrees = Math.max(0, degrees); + /* TODO: Position parameter must be given as int32. Convert degrees to int32. + var f = new DataView() + var d = new Int32Array(degrees.data.buffer); + console.table(d) + */ + const cmd = this._parent.generateOutputCommand( + this._index, + 0x0B, + null, + [0,0,0x01,degrees, + this._power * this._direction, // power in range 0-100 + this._power * this._direction, + 0x00, + 0x00] + ); + + this._parent.send(BLECharacteristic, cmd); + } + /** * Start active braking on this motor. After a short time, the motor will turn off. * // TODO: rename this to coastAfter? @@ -689,7 +738,7 @@ class Boost { * @param {array} values - the list of values to write to the command. * @return {array} - a generated output command. */ - generateOutputCommand (connectID, subCommandID = 0x51, mode=0x00, values = null) { + generateOutputCommand (connectID, subCommandID = 0x51, mode=null, values = null) { let command = [0x00, BoostCommand.OUTPUT]; if (values) { command = command.concat( @@ -701,14 +750,15 @@ class Boost { if(subCommandID) { command = command.concat(subCommandID); } - command = command.concat(mode) - + if(mode) { + command = command.concat(mode) + } command = command.concat( values ); } command.unshift(command.length +1) - console.log(command) + console.log(buf2hex(command)) return command; } @@ -1007,6 +1057,26 @@ class Scratch3BoostBlocks { } } }, + { + opcode: 'motorOnForRotation', + text: formatMessage({ + id: 'boost.motorOnForRotation', + default: 'turn [MOTOR_ID] on for [ROTATION] rotations', + description: 'turn a motor on for rotation' + }), + blockType: BlockType.COMMAND, + arguments: { + MOTOR_ID: { + type: ArgumentType.STRING, + menu: 'MOTOR_ID', + defaultValue: BoostMotorLabel.A + }, + ROTATION: { + type: ArgumentType.NUMBER, + defaultValue: 1 + } + } + }, { opcode: 'motorOn', text: formatMessage({ @@ -1080,6 +1150,22 @@ class Scratch3BoostBlocks { } } }, + { + opcode: 'motorZero', + text: formatMessage({ + id: 'boost.motorZero', + default: 'zero [MOTOR_ID]', + description: 'set a motor\'s position to 0' + }), + blockType: BlockType.COMMAND, + arguments: { + MOTOR_ID: { + type: ArgumentType.STRING, + menu: 'MOTOR_ID', + defaultValue: BoostMotorLabel.A + } + } + }, { opcode: 'setLightHue', text: formatMessage({ @@ -1464,6 +1550,30 @@ class Scratch3BoostBlocks { }); } + /** + * Turn specified motor(s) on for a specified rotation in full rotations. + * @param {object} args - the block's arguments. + * @property {MotorID} MOTOR_ID - the motor(s) to activate. + * @property {int} ROTATION - the amount of full rotations to turn the motors. + * @return {Promise} - a promise which will resolve at the end of the duration. + */ + motorOnForRotation (args) { + // TODO: cast args.MOTOR_ID? + let degrees = Cast.toNumber(args.ROTATION) * 360; + degrees = MathUtil.clamp(degrees, 0, 36000); + return new Promise(resolve => { + this._forEachMotor(args.MOTOR_ID, motorIndex => { + const motor = this._peripheral.motor(motorIndex); + if (motor) { + motor.turnOnForDegrees(degrees); + } + }); + + // Run for some time even when no motor is connected + setTimeout(resolve, degrees); + }); + } + /** * Turn specified motor(s) on indefinitely. * @param {object} args - the block's arguments. @@ -1577,6 +1687,29 @@ class Scratch3BoostBlocks { }); } + /** + * Set the motor(s) position to 0. + * @param {object} args - the block's arguments. + * @property {MotorID} MOTOR_ID - the motor(s) to activate. + * @return {Promise} - a Promise that resolves after some delay. + */ + motorZero (args) { + // TODO: cast args.MOTOR_ID? + this._forEachMotor(args.MOTOR_ID, motorIndex => { + const motor = this._peripheral.motor(motorIndex); + if (motor) { + // TODO: Do this on the hardware, i.e. https://lego.github.io/lego-ble-wireless-protocol-docs/index.html#encoding-of-writedirectmodedata-0x81-0x51 + motor.positionZero = motor.position; + } + }); + + return new Promise(resolve => { + window.setTimeout(() => { + resolve(); + }, BLESendInterval); + }); + } + /** * Set the LED's hue. * @param {object} args - the block's arguments. @@ -1662,13 +1795,13 @@ class Scratch3BoostBlocks { switch(args.MOTOR_REPORTER_ID) { // TODO: Handle negative rotation. case BoostMotorLabel.A: - return MathUtil.wrapClamp(this._peripheral._motors[BoostPort.A].position, 0, 360); + return this._peripheral._motors[BoostPort.A].position - this._peripheral._motors[BoostPort.A].positionZero case BoostMotorLabel.B: - return MathUtil.wrapClamp(this._peripheral._motors[BoostPort.B].position, 0, 360); + return this._peripheral._motors[BoostPort.B].position - this._peripheral._motors[BoostPort.B].positionZero case BoostMotorLabel.C: - return MathUtil.wrapClamp(this._peripheral._motors[BoostPort.C].position, 0, 360); + return this._peripheral._motors[BoostPort.C].position - this._peripheral._motors[BoostPort.C].positionZero case BoostMotorLabel.D: - return MathUtil.wrapClamp(this._peripheral._motors[BoostPort.D].position, 0, 360); + return this._peripheral._motors[BoostPort.D].position - this._peripheral._motors[BoostPort.D].positionZero default: log.warn("Asked for a motor position that doesnt exist!") return false;