From 133ade7f486845762876975aef113086834260d7 Mon Sep 17 00:00:00 2001 From: Evelyn Eastmond Date: Fri, 13 Jul 2018 13:14:53 -0400 Subject: [PATCH] EV3 port and sensor polling updates (#1342) * Add motor rotate, without coasting yet. Fixed some todos. * Removing logging. * Implemented motor set position, motor set power, and beep note for time blocks. Changed labels on motor blocks. Tracking motor speeds for all ports. * Fixing motor ports to 0-3 and then output bit field when sending commands. * Added stop all motors with stop button. Added port masking function. Some cleanup. Moved beeping up an octave. * Combine device list polling and sensor polling into a single timeout call, device list requested every 20 times. * Don't reset sensors on device update. Don't store undefined sensor/motor values. Some comments. * Fixing some linting. --- src/extensions/scratch3_ev3/index.js | 676 +++++++++++++++------------ src/io/bleSession.js | 1 - src/io/btSession.js | 1 - 3 files changed, 385 insertions(+), 293 deletions(-) diff --git a/src/extensions/scratch3_ev3/index.js b/src/extensions/scratch3_ev3/index.js index 8bf20dca4..9522fb198 100644 --- a/src/extensions/scratch3_ev3/index.js +++ b/src/extensions/scratch3_ev3/index.js @@ -34,28 +34,22 @@ const BTCommand = { TIMESPEEDSYNC: 0xB1 }; -/** - * Array of accepted motor ports. - * @note These should not be translated as they correspond to labels on - * the EV3 hub. - * @type {array} - */ const MOTOR_PORTS = [ { name: 'A', - value: 1 + value: 0 }, { name: 'B', - value: 2 + value: 1 }, { name: 'C', - value: 4 + value: 2 }, { name: 'D', - value: 8 + value: 3 } ]; @@ -68,19 +62,19 @@ const MOTOR_PORTS = [ const SENSOR_PORTS = [ { name: '1', - value: 1 + value: 0 }, { name: '2', - value: 2 + value: 1 }, { name: '3', - value: 3 + value: 2 }, { name: '4', - value: 4 + value: 3 } ]; @@ -92,14 +86,16 @@ const EV_DEVICE_TYPES = { 16: 'touch', 8: 'mediumMotor', 7: 'largeMotor', - 126: 'none' + 126: 'none', + 125: 'none' }; // firmware pdf page 100? const EV_DEVICE_MODES = { touch: 0, color: 1, - ultrasonic: 1 + ultrasonic: 1, + none: 0 }; const EV_DEVICE_LABELS = { @@ -119,21 +115,26 @@ class EV3 { * @private */ this._runtime = runtime; + this._runtime.on('PROJECT_STOP_ALL', this._stopAllMotors.bind(this)); /** - * EV3 State + * State */ this.connected = false; - this.speed = 50; + this._sensorPorts = []; + this._motorPorts = []; this._sensors = { distance: 0, brightness: 0, buttons: [0, 0, 0, 0] }; - this._motorPositions = [0, 0, 0, 0]; - this._sensorPorts = []; - this._motorPorts = []; + this._motors = { + speeds: [50, 50, 50, 50], + positions: [0, 0, 0, 0], + busy: [0, 0, 0, 0] + }; this._pollingIntervalID = null; + this._pollingCounter = 0; /** * The Bluetooth connection session for reading/writing device data. @@ -144,7 +145,7 @@ class EV3 { this._runtime.registerExtensionDevice(extensionId, this); } - // TODO: keep here? + // TODO: keep here? / refactor /** * Called by the runtime when user wants to scan for a device. */ @@ -155,7 +156,7 @@ class EV3 { }, this._onSessionConnect.bind(this), this._onSessionMessage.bind(this)); } - // TODO: keep here? + // TODO: keep here? / refactor /** * Called by the runtime when user wants to connect to a certain device. * @param {number} id - the id of the device to connect to. @@ -164,25 +165,29 @@ class EV3 { this._bt.connectDevice(id); } - // TODO: keep here? + // TODO: keep here? / refactor /** * Called by the runtime when user wants to disconnect from the device. */ disconnectSession () { this._bt.disconnectSession(); window.clearInterval(this._pollingIntervalID); // TODO: window? - this.speed = 50; + this._sensorPorts = []; + this._motorPorts = []; this._sensors = { distance: 0, brightness: 0, buttons: [0, 0, 0, 0] }; - this._motorPositions = [0, 0, 0, 0]; - this._sensorPorts = []; - this._motorPorts = []; + this._motors = { + speeds: [50, 50, 50, 50], + positions: [0, 0, 0, 0], + busy: [0, 0, 0, 0] + }; this._pollingIntervalID = null; } + // TODO: keep here? / refactor /** * Called by the runtime to detect whether the device is connected. * @return {boolean} - the connected state. @@ -217,7 +222,7 @@ class EV3 { getMotorPosition (port) { if (!this.connected) return; - let value = this._motorPositions[port]; + let value = this._motors.positions[port]; value = value % 360; value = value < 0 ? value * -1 : value; @@ -230,13 +235,42 @@ class EV3 { return this._sensors.buttons[port]; } - beep () { + beep (freq, time) { if (!this.connected) return; + log.info('should be beeping'); + + const cmd = []; + cmd[0] = 15; // length + cmd[1] = 0; // 0x00 + cmd[2] = 0; // 0x00 + cmd[3] = 0; // 0x00 + cmd[4] = 128; // 0x80 + cmd[5] = 0; // 0x00 + cmd[6] = 0; // 0x00 + cmd[7] = 148; // 0x94 op sound + cmd[8] = 1; // 0x01 tone + cmd[9] = 129; // 0x81 volume following in 1 byte + cmd[10] = 2; // volume byte 1 + cmd[11] = 130; // 0x82 frequency following in 2 bytes + cmd[12] = freq; // frequency byte 1 + cmd[13] = freq >> 8; // frequency byte 2 + cmd[14] = 130; // 0x82 time following in 2 bytes + cmd[15] = time; // time byte 1 + cmd[16] = time >> 8; // time byte 2 + this._bt.sendMessage({ - message: 'DwAAAIAAAJQBgQKC6AOC6AM=', + message: Base64Util.arrayBufferToBase64(cmd), encoding: 'base64' }); + + // Yield for sound duration + // TODO: does this work? + return new Promise(resolve => { + setTimeout(() => { + resolve(); + }, time); + }); } motorTurnClockwise (port, time) { @@ -245,9 +279,9 @@ class EV3 { // Build up motor command const cmd = this._applyPrefix(0, this._motorCommand( BTCommand.TIMESPEED, - port, + this._portMask(port), time, - this.speed, + this._motors.speeds[port], BTCommand.LONGRAMP )); @@ -257,13 +291,16 @@ class EV3 { encoding: 'base64' }); + // Set motor to busy + // this._motors.busy[port] = 1; + // Send coast message - const coastTime = 100; setTimeout(() => { this.motorCoast(port); - }, time + coastTime); + }, time); // Yield for turn time + brake time + const coastTime = 100; // TODO: calculate coasting or set flag return new Promise(resolve => { setTimeout(() => { resolve(); @@ -277,9 +314,9 @@ class EV3 { // Build up motor command const cmd = this._applyPrefix(0, this._motorCommand( BTCommand.TIMESPEED, - port, + this._portMask(port), time, - this.speed * -1, + this._motors.speeds[port] * -1, BTCommand.LONGRAMP )); @@ -289,13 +326,16 @@ class EV3 { encoding: 'base64' }); + // Set motor to busy + // this._motors.busy[port] = 1; + // Send coast message - const coastTime = 100; setTimeout(() => { this.motorCoast(port); - }, time + coastTime); + }, time); // Yield for time + const coastTime = 100; // TODO: calculate coasting or set flag return new Promise(resolve => { setTimeout(() => { resolve(); @@ -304,32 +344,19 @@ class EV3 { } motorCoast (port) { + const cmd = []; - // MOTOR COAST - /* - 0x09 [ 9] length - 0x00 [ 0] length - 0x01 [ 1] - 0x00 [ 0] - 0x00 [ 0] - 0x00 [ 0] - 0x00 [ 0] - 0xA3 [163] Coast motor command - 0x00 [ 0] layer - 0x03 [ ] port - 0x00 [ 0] float = coast = 0 - */ - cmd[0] = 9; - cmd[1] = 0; - cmd[2] = 1; - cmd[3] = 0; - cmd[4] = 0; - cmd[5] = 0; - cmd[6] = 0; - cmd[7] = 163; - cmd[8] = 0; - cmd[9] = port; - cmd[10] = 0; + cmd[0] = 9; // length + cmd[1] = 0; // length + cmd[2] = 1; // 0x01 + cmd[3] = 0; // 0x00 + cmd[4] = 0; // 0x00 + cmd[5] = 0; // 0x00 + cmd[6] = 0; // 0x00 + cmd[7] = 163; // 0xA3 Motor brake/coast command + cmd[8] = 0; // layer + cmd[9] = this._portMask(port); // port output bit field + cmd[10] = 0; // float = coast = 0 this._bt.sendMessage({ message: Base64Util.uint8ArrayToBase64(cmd), @@ -340,25 +367,103 @@ class EV3 { motorRotate (port, degrees) { if (!this.connected) return; - // TODO: Build up motor command - log.info(`motor rotate port: ${port} and degrees: ${degrees}`); + // Build up motor command + const cmd = this._applyPrefix(0, this._motorCommand( + BTCommand.STEPSPEED, + this._portMask(port), + degrees, + this._motors.speeds[port], + BTCommand.LONGRAMP + )); + + // Send rotate message + this._bt.sendMessage({ + message: Base64Util.arrayBufferToBase64(cmd), + encoding: 'base64' + }); + + // Set motor to busy + // this._motors.busy[port] = 1; + + /* + // Yield for time + // TODO: calculate time? + return new Promise(resolve => { + setTimeout(() => { + resolve(); + }, time); + }); + */ } motorSetPosition (port, degrees) { if (!this.connected) return; - // TODO: Build up motor command - log.info(`motor set position port: ${port} and degrees: ${degrees}`); + // Calculate degrees to turn + let previousPos = this._motors.positions[port]; + previousPos = previousPos % 360; + previousPos = previousPos < 0 ? previousPos * -1 : previousPos; + const newPos = degrees % 360; + let degreesToTurn = 0; + let direction = 1; + if (previousPos <= newPos) { + degreesToTurn = newPos - previousPos; + } else { + degreesToTurn = previousPos - newPos; + direction = -1; + } + + // Build up motor command + const cmd = this._applyPrefix(0, this._motorCommand( + BTCommand.STEPSPEED, + this._portMask(port), + degreesToTurn, + this._motors.speeds[port] * direction, + BTCommand.LONGRAMP + )); + + // Send rotate message + this._bt.sendMessage({ + message: Base64Util.arrayBufferToBase64(cmd), + encoding: 'base64' + }); + + // Set motor to busy + // this._motors.busy[port] = 1; + + /* + // Yield for time + // TODO: calculate time? + return new Promise(resolve => { + setTimeout(() => { + resolve(); + }, time); + }); + */ } motorSetPower (port, power) { if (!this.connected) return; - // TODO: Build up motor command - log.info(`motor set power port: ${port} and degrees: ${power}`); + this._motors.speeds[port] = power; } + // ******* + // PRIVATE + // ******* + + _stopAllMotors () { + log.info('stop all motors'); + for (let i = 0; i < this._motorPorts.length; i++) { + if (this._motorPorts[i] !== 'none') { + this.motorCoast(i); + } + } + } + + // TODO: keep here? / refactor _applyPrefix (n, cmd) { + // TODO: document const len = cmd.length + 5; return [].concat( len & 0xFF, @@ -372,6 +477,7 @@ class EV3 { ); } + // TODO: keep here? / refactor /** * Generate a motor command in EV3 byte array format (CMD, LAYER, PORT, * SPEED, RAMP UP, RUN, RAMP DOWN, BREAKING TYPE) @@ -383,6 +489,7 @@ class EV3 { * @return {array} Byte array */ _motorCommand (command, port, n, speed, ramp) { + // TODO: document /** * Generate run values for a given input. * @param {number} run Run input @@ -446,230 +553,220 @@ class EV3 { ])); } + // TODO: keep here? / refactor _onSessionConnect () { this.connected = true; - // GET EV3 SENSOR LIST - /* - 0B [ 11] - 00 [ 0] - 01 [ 1] - 00 [ 0] - 00 [ 0] - 21 [ 33] - 00 [ 0] - 98 [152] opInput_Device_List - 81 [129] LENGTH - 21 [ 33] ARRAY - 60 [ 96] CHANGED - E1 [225] size of global var? - 20 [ 32] global var index - */ - this._bt.sendMessage({ - message: 'CwABAAAhAJiBIWDhIA==', // [11, 0, 1, 0, 0, 33, 0, 152, 129, 33, 96, 225, 32] - encoding: 'base64' - }).then( - x => { - log.info(`get device list resolved: ${x}`); - }, - e => { - log.info(`get device list rejected: ${e}`); - } - ); + // start polling + // TODO: window? + this._pollingIntervalID = window.setInterval(this._getSessionData.bind(this), 150); } + // TODO: keep here? / refactor _getSessionData () { if (!this.connected) { window.clearInterval(this._pollingIntervalID); return; } - // GET EV3 DISTANCE PORT 0 - /* - 99 [153] input device - 1D [ 29] ready si - 00 [ 0] layer (this brick) - 00 [ 0] sensor port 0 - 00 [ 0] do not change type - 01 [ 1] mode 1 = EV3-Ultrasonic-Inch - 01 [ 1] one data set - 60 [ 96] global var index - */ - /* - this._bt.sendMessage({ - message: 'DQAAAAAEAJkdAAAAAQFg', // [13, 0, 0, 0, 0, 4, 0, 153, 29, 0, 0, 0, 1, 1, 96] - encoding: 'base64' - }); - */ + const cmd = []; // a compound command - // GET EV3 BRIGHTNESS PORT 1 - /* - 0x99 [153] input device - 0x1D [ 29] ready si - 0x00 [ 0] layer (this brick) - 0x01 [ 1] sensor port 1 - 0x00 [ 0] do not change type - 0x01 [ 1] mode 1 = EV3-Color-Ambient - 0x01 [ 1] one data set - 0x60 [ 96] global var index - */ - /* - this._bt.sendMessage({ - message: 'DQAAAAAEAJkdAAEAAQFg', // [13, 0, 0, 0, 0, 4, 0, 153, 29, 0, 1, 0, 1, 1, 96] - encoding: 'base64' - }); - */ + // HEADER + cmd[0] = null; // calculate length later + cmd[1] = 0; // ... + cmd[2] = 1; // message counter // TODO: ????? + cmd[3] = 0; // message counter // TODO: ????? + cmd[4] = 0; // command type: direct command + cmd[5] = null; // calculate vars length later + cmd[6] = 0; // ... + let sensorCount = 0; + // Either request device list or request sensor data ?? + if (this._pollingCounter % 20 === 0) { + // GET DEVICE LIST + cmd[7] = 152; // 0x98 op: get device list + cmd[8] = 129; // 0x81 LENGTH // TODO: ????? + cmd[9] = 33; // 0x21 ARRAY // TODO: ????? + cmd[10] = 96; // 0x60 CHANGED // TODO: ????? + cmd[11] = 225; // 0xE1 size of global var - 1 byte to follow + cmd[12] = 32; // 0x20 global var index "0" 0b00100000 - // COMPOUND COMMAND FOR READING sensors0x27 command size - // 0x?? [ ] command size - // 0x00 [ 0] command size - // 0x01 [ 1] message counter - // 0x00 [ 0] message counter - // 0x00 [ 0] command type - // 0x?? [ ] result payload size of global/local vars - // 0x00 [ 0] result payload size of global/local vars - const compoundCommand = []; - compoundCommand[0] = 0; // calculate length later - compoundCommand[1] = 0; // command size - compoundCommand[2] = 1; // message counter // TODO: ????? - compoundCommand[3] = 0; // message counter - compoundCommand[4] = 0; // command type: direct command - compoundCommand[5] = 0; // global/local vars - compoundCommand[6] = 0; // global/local vars - let compoundCommandIndex = 7; - let sensorCount = -1; + // Command and payload lengths + cmd[0] = cmd.length - 2; + cmd[5] = 33; - // Read from available sensors - for (let i = 0; i < this._sensorPorts.length; i++) { - if (this._sensorPorts[i] !== 'none') { - sensorCount++; - // make up sensor command array - // 0x9D [ 157] op: get sensor value - // 0x00 [ 0] layer - // 0x02 [ ] port - // 0x00 [ 0] do not change type - // 0x00 [ ] mode - // 0xE1 [ 225] - // 0x0C [ ] global index - compoundCommand[compoundCommandIndex + 0] = 157; - compoundCommand[compoundCommandIndex + 1] = 0; - compoundCommand[compoundCommandIndex + 2] = i; - compoundCommand[compoundCommandIndex + 3] = 0; - compoundCommand[compoundCommandIndex + 4] = EV_DEVICE_MODES[this._sensorPorts[i]]; - compoundCommand[compoundCommandIndex + 5] = 225; - compoundCommand[compoundCommandIndex + 6] = sensorCount * 4; - compoundCommandIndex += 7; + // log.info(`REQUEST DEVICE LIST: ${compoundCommand}`); + // Clear sensor data + this._updateDevices = true; + this._sensorPorts = []; + this._motorPorts = []; + // TODO: figure out when/how to clear out sensor data + + } else { + + let index = 7; + + // GET SENSOR VALUES + // eslint-disable-next-line no-undefined + if (!this._sensorPorts.includes(undefined)) { + for (let i = 0; i < 4; i++) { + if (this._sensorPorts[i] !== 'none') { + cmd[index + 0] = 157; // 0x9D op: get sensor value + cmd[index + 1] = 0; // layer + cmd[index + 2] = i; // port + cmd[index + 3] = 0; // do not change type + cmd[index + 4] = EV_DEVICE_MODES[this._sensorPorts[i]]; // mode + cmd[index + 5] = 225; // 0xE1 one byte to follow + cmd[index + 6] = sensorCount * 4; // global index + index += 7; + } + sensorCount++; + } } + + // GET MOTOR POSITION VALUES + // eslint-disable-next-line no-undefined + if (!this._motorPorts.includes(undefined)) { + for (let i = 0; i < 4; i++) { + cmd[index + 0] = 179; // 0XB3 op: get motor position value + cmd[index + 1] = 0; // layer + cmd[index + 2] = i; // port + cmd[index + 3] = 225; // 0xE1 byte following + cmd[index + 4] = sensorCount * 4; // global index + index += 5; + sensorCount++; + } + } + + // Command and payload lengths + cmd[0] = cmd.length - 2; + cmd[5] = sensorCount * 4; } - // Read from available motors - // let motorCount = 0; + + // GET MOTOR BUSY STATES + /* for (let i = 0; i < this._motorPorts.length; i++) { if (this._motorPorts[i] !== 'none') { sensorCount++; - // make up sensor command array - // 0xB3 [ 179] op: get motor position value - // 0x00 [ 0] layer - // 0x02 [ ] output bit fields ?? - // 0xE1 [ 225] - // 0x?? [ 0] global index - compoundCommand[compoundCommandIndex + 0] = 179; - compoundCommand[compoundCommandIndex + 1] = 0; - compoundCommand[compoundCommandIndex + 2] = i; - compoundCommand[compoundCommandIndex + 3] = 225; - compoundCommand[compoundCommandIndex + 4] = sensorCount * 4; + compoundCommand[compoundCommandIndex + 0] = 169; // 0xA9 op: test if output port is busy + compoundCommand[compoundCommandIndex + 1] = 0; // layer + compoundCommand[compoundCommandIndex + 2] = this._portMask(i); // output bit field + compoundCommand[compoundCommandIndex + 3] = 225; // 0xE1 1 byte following + compoundCommand[compoundCommandIndex + 4] = sensorCount * 4; // global index compoundCommandIndex += 5; - // motorCount++; } } + */ - - // Calculate compound command length - compoundCommand[0] = compoundCommand.length - 2; - // Calculate global var payload length needed - compoundCommand[5] = (sensorCount + 1) * 4; - // console.log('compound command to send: ' + compoundCommand); this._bt.sendMessage({ - message: Base64Util.uint8ArrayToBase64(compoundCommand), + message: Base64Util.uint8ArrayToBase64(cmd), encoding: 'base64' }); - // TODO: Read from available motor ports + this._pollingCounter++; } + // TODO: rename and document better _onSessionMessage (params) { const message = params.message; const array = Base64Util.base64ToUint8Array(message); + // log.info(`received array: ${array}`); - if (this._sensorPorts.length === 0) { - // SENSOR LIST - // JAABAAIefn5+fn5+fn5+fn5+fn5+Bwd+fn5+fn5+fn5+fn5+fgA= - log.info(`device array: ${array}`); - this._sensorPorts[0] = EV_DEVICE_TYPES[array[5]]; - this._sensorPorts[1] = EV_DEVICE_TYPES[array[6]]; - this._sensorPorts[2] = EV_DEVICE_TYPES[array[7]]; - this._sensorPorts[3] = EV_DEVICE_TYPES[array[8]]; - this._motorPorts[0] = EV_DEVICE_TYPES[array[21]]; - this._motorPorts[1] = EV_DEVICE_TYPES[array[22]]; - this._motorPorts[2] = EV_DEVICE_TYPES[array[23]]; - this._motorPorts[3] = EV_DEVICE_TYPES[array[24]]; + if (this._updateDevices) { + // READ DEVICE LIST + this._sensorPorts[0] = EV_DEVICE_TYPES[array[5]] ? EV_DEVICE_TYPES[array[5]] : 'none'; + this._sensorPorts[1] = EV_DEVICE_TYPES[array[6]] ? EV_DEVICE_TYPES[array[6]] : 'none'; + this._sensorPorts[2] = EV_DEVICE_TYPES[array[7]] ? EV_DEVICE_TYPES[array[7]] : 'none'; + this._sensorPorts[3] = EV_DEVICE_TYPES[array[8]] ? EV_DEVICE_TYPES[array[8]] : 'none'; + this._motorPorts[0] = EV_DEVICE_TYPES[array[21]] ? EV_DEVICE_TYPES[array[21]] : 'none'; + this._motorPorts[1] = EV_DEVICE_TYPES[array[22]] ? EV_DEVICE_TYPES[array[22]] : 'none'; + this._motorPorts[2] = EV_DEVICE_TYPES[array[23]] ? EV_DEVICE_TYPES[array[23]] : 'none'; + this._motorPorts[3] = EV_DEVICE_TYPES[array[24]] ? EV_DEVICE_TYPES[array[24]] : 'none'; log.info(`sensor ports: ${this._sensorPorts}`); log.info(`motor ports: ${this._motorPorts}`); - - // Now ready to read from assigned _sensors - // Start reading sensor data - // TODO: window? - this._pollingIntervalID = window.setInterval(this._getSessionData.bind(this), 100); - } else { - // log.info(`received compound command result: ${array}`); - let offset = 5; - for (let i = 0; i < this._sensorPorts.length; i++) { - if (this._sensorPorts[i] !== 'none') { - const value = this._array2float([ - array[offset], - array[offset + 1], - array[offset + 2], - array[offset + 3] - ]); - // log.info(`sensor at port ${i} ${this._sensorPorts[i]} value: ${value}`); - if (EV_DEVICE_LABELS[this._sensorPorts[i]] === 'button') { - this._sensors.buttons[i] = value; - } else { - this._sensors[EV_DEVICE_LABELS[this._sensorPorts[i]]] = value; - } - offset += 4; + this._updateDevices = false; + // eslint-disable-next-line no-undefined + } else if (!this._sensorPorts.includes(undefined) && !this._motorPorts.includes(undefined)) { + // READ SENSOR VALUES + let offset = 5; // start reading sensor values at byte 5 + for (let i = 0; i < 4; i++) { + const value = this._array2float([ + array[offset], + array[offset + 1], + array[offset + 2], + array[offset + 3] + ]); + if (EV_DEVICE_LABELS[this._sensorPorts[i]] === 'button') { + // Read a button value per port + this._sensors.buttons[i] = value ? value : 0; + } else if (EV_DEVICE_LABELS[this._sensorPorts[i]]) { // if valid + // Read brightness / distance values and set to 0 if null + this._sensors[EV_DEVICE_LABELS[this._sensorPorts[i]]] = value ? value : 0; } + log.info(`${JSON.stringify(this._sensors)}`); + offset += 4; } - for (let i = 0; i < this._motorPorts.length; i++) { - if (this._motorPorts[i] !== 'none') { - let value = this._tachoValue([ - array[offset], - array[offset + 1], - array[offset + 2], - array[offset + 3] - ]); - if (value > 0x7fffffff) { - value = value - 0x100000000; - } - // log.info(`motor at port ${i} ${this._motorPorts[i]} value: ${value}`); - this._motorPositions[i] = value; - offset += 4; + // READ MOTOR POSITION VALUES + for (let i = 0; i < 4; i++) { + let value = this._tachoValue([ // from Paula + array[offset], + array[offset + 1], + array[offset + 2], + array[offset + 3] + ]); + if (value > 0x7fffffff) { // from Paula + value = value - 0x100000000; } + if (value) { + this._motors.positions[i] = value; + } + log.info(`motor positions: ${this._motors.positions}`); + offset += 4; } - // const sensorValue = this._array2float([array[5], array[6], array[7], array[8]]); - // log.info('receiving port array?: ' + array); - // log.info('receiving port sensorValue?: ' + sensorValue); - // this._sensors.distance = distance; } + /* + // READ MOTOR BUSY STATES + /* + for (let i = 0; i < this._motorPorts.length; i++) { + if (this._motorPorts[i] !== 'none') { + const busy = array[offset]; + if (busy === 0 && this._motors.busy[i]) { + this.motorCoast(i); // always set to coast for now, but really should only do for recently moved + this._motors.busy[i] = 0; // reset busy + } + // this._motors.positions[i] = value; + log.info(`motor ${i} busy: ${busy}`); + offset += 1; + } + } + */ } + // TODO: keep here? / refactor + _portMask (port) { + // TODO: convert to enum or lookup + let p = null; + if (port === 0) { + p = 1; + } else if (port === 1) { + p = 2; + } else if (port === 2) { + p = 4; + } else if (port === 3) { + p = 8; + } + + return p; + } + + // TODO: keep here? / refactor _tachoValue (list) { const value = list[0] + (list[1] * 256) + (list[2] * 256 * 256) + (list[3] * 256 * 256 * 256); return value; } - // TODO: put elsewhere + // TODO: keep here? / refactor _array2float (list) { const buffer = new Uint8Array(list).buffer; const view = new DataView(buffer); @@ -711,19 +808,19 @@ class Scratch3Ev3Blocks { getInfo () { return { id: Scratch3Ev3Blocks.EXTENSION_ID, - name: 'LEGO MINDSTORMS EV3', + name: 'LEGO EV3', blockIconURI: blockIconURI, showStatusButton: true, blocks: [ { opcode: 'motorTurnClockwise', - text: 'motor [PORT] turn clockwise for [TIME] seconds', + text: 'motor [PORT] turn this way for [TIME] seconds', blockType: BlockType.COMMAND, arguments: { PORT: { type: ArgumentType.STRING, menu: 'motorPorts', - defaultValue: MOTOR_PORTS[0].value + defaultValue: 'A' }, TIME: { type: ArgumentType.NUMBER, @@ -733,13 +830,13 @@ class Scratch3Ev3Blocks { }, { opcode: 'motorTurnCounterClockwise', - text: 'motor [PORT] turn counter for [TIME] seconds', + text: 'motor [PORT] turn that way for [TIME] seconds', blockType: BlockType.COMMAND, arguments: { PORT: { type: ArgumentType.STRING, menu: 'motorPorts', - defaultValue: MOTOR_PORTS[0].value + defaultValue: 'A' }, TIME: { type: ArgumentType.NUMBER, @@ -747,7 +844,7 @@ class Scratch3Ev3Blocks { } } }, - { + /* { opcode: 'motorRotate', text: 'motor [PORT] rotate [DEGREES] degrees', blockType: BlockType.COMMAND, @@ -755,7 +852,7 @@ class Scratch3Ev3Blocks { PORT: { type: ArgumentType.STRING, menu: 'motorPorts', - defaultValue: MOTOR_PORTS[0].value + defaultValue: 'A' }, DEGREES: { type: ArgumentType.NUMBER, @@ -771,14 +868,14 @@ class Scratch3Ev3Blocks { PORT: { type: ArgumentType.STRING, menu: 'motorPorts', - defaultValue: MOTOR_PORTS[0].value + defaultValue: 'A' }, DEGREES: { type: ArgumentType.NUMBER, defaultValue: 90 } } - }, + }, */ { opcode: 'motorSetPower', text: 'motor [PORT] set power [POWER] %', @@ -787,7 +884,7 @@ class Scratch3Ev3Blocks { PORT: { type: ArgumentType.STRING, menu: 'motorPorts', - defaultValue: MOTOR_PORTS[0].value + defaultValue: 'A' }, POWER: { type: ArgumentType.NUMBER, @@ -803,7 +900,7 @@ class Scratch3Ev3Blocks { PORT: { type: ArgumentType.STRING, menu: 'motorPorts', - defaultValue: MOTOR_PORTS[0].value + defaultValue: 'A' } } }, @@ -865,8 +962,18 @@ class Scratch3Ev3Blocks { }, { opcode: 'beep', - text: 'beep', - blockType: BlockType.COMMAND + text: 'beep note [NOTE] for [TIME] secs', + blockType: BlockType.COMMAND, + arguments: { + NOTE: { + type: ArgumentType.NUMBER, + defaultValue: 60 + }, + TIME: { + type: ArgumentType.NUMBER, + defaultValue: 0.5 + } + } } ], menus: { @@ -876,6 +983,7 @@ class Scratch3Ev3Blocks { }; } + // TODO: redo? /** * Create data for a menu in scratch-blocks format, consisting of an array of objects with text and * value properties. The text is a translated string, and the value is one-indexed. @@ -887,45 +995,23 @@ class Scratch3Ev3Blocks { return info.map((entry, index) => { const obj = {}; obj.text = entry.name; - obj.value = String(index + 1); + obj.value = String(index); return obj; }); } motorTurnClockwise (args) { - const port = Cast.toNumber(args.PORT); // TODO: fix + const port = Cast.toNumber(args.PORT); const time = Cast.toNumber(args.TIME) * 1000; - let p = null; - if (port === 1) { - p = 1; - } else if (port === 2) { - p = 2; - } else if (port === 3) { - p = 4; - } else if (port === 4) { - p = 8; - } - - return this._device.motorTurnClockwise(p, time); + return this._device.motorTurnClockwise(port, time); } motorTurnCounterClockwise (args) { - const port = Cast.toNumber(args.PORT); // TODO: fix + const port = Cast.toNumber(args.PORT); const time = Cast.toNumber(args.TIME) * 1000; - let p = null; - if (port === 1) { - p = 1; - } else if (port === 2) { - p = 2; - } else if (port === 3) { - p = 4; - } else if (port === 4) { - p = 8; - } - - return this._device.motorTurnCounterClockwise(p, time); + return this._device.motorTurnCounterClockwise(port, time); } motorRotate (args) { @@ -948,18 +1034,20 @@ class Scratch3Ev3Blocks { const port = Cast.toNumber(args.PORT); const power = Cast.toNumber(args.POWER); - this._device.motorSetPower(port, power); + const value = Math.max(-100, Math.min(power, 100)); + + this._device.motorSetPower(port, value); return; } getMotorPosition (args) { - const port = Cast.toNumber(args.PORT - 1); // TODO: Fix MOTOR_PORTS + const port = Cast.toNumber(args.PORT); return this._device.getMotorPosition(port); } whenButtonPressed (args) { - const port = Cast.toNumber(args.PORT - 1); // TODO: Fix SENSOR PORTS + const port = Cast.toNumber(args.PORT); return this._device.isButtonPressed(port); } @@ -977,7 +1065,7 @@ class Scratch3Ev3Blocks { } buttonPressed (args) { - const port = Cast.toNumber(args.PORT - 1); // TODO: fix SENSOR_PORTS + const port = Cast.toNumber(args.PORT); return this._device.isButtonPressed(port); } @@ -990,8 +1078,14 @@ class Scratch3Ev3Blocks { return this._device.brightness; } - beep () { - return this._device.beep(); + beep (args) { + const note = Cast.toNumber(args.NOTE); + const time = Cast.toNumber(args.TIME * 1000); + + // https://en.wikipedia.org/wiki/MIDI_tuning_standard#Frequency_values + const freq = Math.pow(2, ((note - 69 + 12) / 12)) * 440; + + return this._device.beep(freq, time); } } diff --git a/src/io/bleSession.js b/src/io/bleSession.js index a77f6298a..dfe74fa6f 100644 --- a/src/io/bleSession.js +++ b/src/io/bleSession.js @@ -50,7 +50,6 @@ class BLESession extends JSONRPCWebSocket { connectDevice (id) { this.sendRemoteRequest('connect', {peripheralId: id}) .then(() => { - log.info('should have connected'); this._connected = true; this._runtime.emit(this._runtime.constructor.PERIPHERAL_CONNECTED); this._connectCallback(); diff --git a/src/io/btSession.js b/src/io/btSession.js index 08df20085..b6029a10c 100644 --- a/src/io/btSession.js +++ b/src/io/btSession.js @@ -52,7 +52,6 @@ class BTSession extends JSONRPCWebSocket { connectDevice (id) { this.sendRemoteRequest('connect', {peripheralId: id}) .then(() => { - log.info('should have connected'); this._connected = true; this._runtime.emit(this._runtime.constructor.PERIPHERAL_CONNECTED); this._connectCallback();