From 577b844ea27d7fccacc00712f7878dc3dfaceb2d Mon Sep 17 00:00:00 2001 From: Darko Date: Fri, 24 Mar 2017 02:16:09 +0100 Subject: [PATCH 1/5] Refactor polygon passing --- src/drivers/dynamixel/DynamixelDriver.js | 15 +++--- src/drivers/infrared/InfraredDriver.js | 56 +++++++++++--------- src/drivers/lidar/LidarDriver.js | 65 +++++++++++++++++++++--- src/drivers/motion/MotionDriver.js | 2 +- src/drivers/pin/PinDriver.js | 8 +++ src/misc/Point.js | 27 ++++++++-- src/misc/Polygon.js | 22 ++++++-- src/services/terrain/TerrainService.js | 14 ++--- 8 files changed, 153 insertions(+), 56 deletions(-) diff --git a/src/drivers/dynamixel/DynamixelDriver.js b/src/drivers/dynamixel/DynamixelDriver.js index 807916b..090baa3 100644 --- a/src/drivers/dynamixel/DynamixelDriver.js +++ b/src/drivers/dynamixel/DynamixelDriver.js @@ -66,7 +66,9 @@ class DynamixelDriver { constructor(name, config) { this.config = Object.assign({ id: 0xFE, - type: 'AX' // AX or RX + type: 'AX', // AX or RX + maxPosition: 1023, + minPosition: 0 }, config); this.name = name; @@ -148,8 +150,7 @@ class DynamixelDriver { } async getPosition() { - let position = await this._read(DynamixelDriver.AX_PRESENT_POSITION_L, true); - return ((position * (300 / 1023)) | 0); + return await this._read(DynamixelDriver.AX_PRESENT_POSITION_L, true); } getSpeed() { @@ -254,11 +255,11 @@ class DynamixelDriver { } setPosition(position) { - if (position > 300 || position < 1) { + if (position > this.config.maxPosition || position < this.config.minPosition) { throw Error(TAG, this.name, 'Position out of range!'); } - this._writeWord(DynamixelDriver.AX_GOAL_POSITION_L, (position * (1023 / 300)) | 0); + this._writeWord(DynamixelDriver.AX_GOAL_POSITION_L, position | 0); } setSpeed(speed) { @@ -271,11 +272,11 @@ class DynamixelDriver { } setCWAngleLimit(angle) { - this._writeWord(DynamixelDriver.AX_CW_ANGLE_LIMIT_L, (angle * (1023 / 300)) | 0); + this._writeWord(DynamixelDriver.AX_CW_ANGLE_LIMIT_L, angle | 0); } setCCWAngleLimit(angle) { - this._writeWord(DynamixelDriver.AX_CCW_ANGLE_LIMIT_L, (angle * (1023 / 300)) | 0); + this._writeWord(DynamixelDriver.AX_CCW_ANGLE_LIMIT_L, angle | 0); } setLED(on) { diff --git a/src/drivers/infrared/InfraredDriver.js b/src/drivers/infrared/InfraredDriver.js index d184e8f..550744c 100644 --- a/src/drivers/infrared/InfraredDriver.js +++ b/src/drivers/infrared/InfraredDriver.js @@ -11,7 +11,7 @@ const TAG = 'InfraredDriver'; /** * Uses data from infrared sensors to determine where is an enemy robot and other obstacles. * - * @memberof drivers.infrared + * @memberOf drivers.infrared * @author Darko Lukic * @fires drivers.infrared.InfraredDriver#obstacleDetected */ @@ -61,8 +61,9 @@ class InfraredDriver extends EventEmitter { sensorAngle: 90 }, config); this.name = name; - this.detected = false; - this.timeoutHandle = null; + this._detected = false; + this._timeoutHandle = null; + this._enabled = true; // Subscribe on communicator this.canDriver = Mep.getDriver(this.config['@dependencies'].communicator); @@ -82,33 +83,32 @@ class InfraredDriver extends EventEmitter { this.poi.rotate(new Point(this.config.sensorX, this.config.sensorY), - this.config.sensorAngle); this.polygon.rotate(new Point(this.config.sensorX, this.config.sensorY), - this.config.sensorAngle); - // Additional information - this.front = (this.config.sensorAngle > 0 && this.config.sensorAngle < 180); - Mep.Log.debug(TAG, name, 'Detects at', this.poi); + } + /** + * Enable sensor + */ + enable() { + this._enabled = true; + } - // Let's try to simulate - let infraredDriver = this; - if (name === 'InfraredAuto') { - Mep.Log.info(TAG, 'Is in testing mode'); - setTimeout(() => { - infraredDriver.processDetection(Buffer.from([0x01])); - }, 5000); - setTimeout(() => { - infraredDriver.processDetection(Buffer.from([0x00])); - }, 10 * 1000); - } + /** + * Disable sensor + */ + disable() { + this._enabled = false; } /** * Process detected obstacle - * * @private - * @param state {boolean} - Object is detected or not + * @param buffer {Boolean} - Object is detected or not */ processDetection(buffer) { - this.detected = !!(buffer.readInt8(0)); + if (this._enabled === false) return; + + this._detected = !!(buffer.readInt8(0)); /** * Obstacle detected event. @@ -119,15 +119,21 @@ class InfraredDriver extends EventEmitter { * @property {Boolean} - Is objected detected or not * @property {Object} - Additional information about detected object */ - this.emit('obstacleDetected', this.name, this.poi, this.polygon, this.detected); + this.emit( + 'obstacleDetected', + this.name, + this.poi.clone(), + this.polygon.clone(), + this._detected + ); // After `duration` publish obstacle detection again if object is still there - if (this.timeoutHandle !== null) { - clearTimeout(this.timeoutHandle); + if (this._timeoutHandle !== null) { + clearTimeout(this._timeoutHandle); } - if (this.detected === true) { + if (this._detected === true) { let infraredDriver = this; - this.timeoutHandle = setTimeout(() => { + this._timeoutHandle = setTimeout(() => { infraredDriver.processDetection(buffer); }, Mep.Config.get('obstacleMaxPeriod')); } diff --git a/src/drivers/lidar/LidarDriver.js b/src/drivers/lidar/LidarDriver.js index b0a736d..4198197 100644 --- a/src/drivers/lidar/LidarDriver.js +++ b/src/drivers/lidar/LidarDriver.js @@ -1,4 +1,5 @@ 'use strict'; +/** @namespace drivers.lidar */ const EventEmitter = require('events').EventEmitter; const Point = Mep.require('misc/Point'); @@ -6,23 +7,33 @@ const Polygon = Mep.require('misc/Polygon'); const TAG = 'LidarDriver'; +/** + * Provides an abstraction layer on top of lidar's firmware and algorithms to determine + * robot's position and obstacles + * @fires drivers.lidar.LidarDriver#obstacleDetected + * @memberOf drivers.lidar + * @author Darko Lukic + */ class LidarDriver extends EventEmitter { constructor(name, config) { super(); + // Merge configs this.config = Object.assign({ cid: 8000, tolerance: 400, volume: 230, - rpm: 300 + angle: 180, + inverted: true }, config); this.name = name; + // Set reference to `this` this._onDataReceived = this._onDataReceived.bind(this); this._addPointToPolyGenerator = this._addPointToPolyGenerator.bind(this); this._calculateRobotsLocation = this._calculateRobotsLocation.bind(this); - + // Initialize communication driver this.communicator = null; if (this.config._communicator !== undefined) { // For testing purposes only (experiments) @@ -32,6 +43,7 @@ class LidarDriver extends EventEmitter { } this.communicator.on('data_' + this.config.cid, this._onDataReceived); + // Set state variable this._readings = {}; this._poly = { minX: 0, @@ -41,7 +53,9 @@ class LidarDriver extends EventEmitter { polyPointsCount: 0, previousPoint: null }; + this._enabled = true; + // Set initial measurements for (let i = 0; i < 360; i++) { this._readings[i] = { distance: Infinity, @@ -50,7 +64,21 @@ class LidarDriver extends EventEmitter { } } - // Run this when robot stop + /** + * Enable lidar driver + */ + enable() { + this._enabled = true; + } + + /** + * Disable lidar driver + */ + disable() { + this._enabled = false; + } + + _calculateRobotsLocation() { let precision = 0.9; @@ -60,9 +88,17 @@ class LidarDriver extends EventEmitter { this.emit('positionChanged', this.config.name, new Point(0, 0), precision); } + /** + * Process a measurement and try to make an obstacle approximation. + * It uses bounding box algorithm to make an approximation of the obstacle + * @link https://en.wikipedia.org/wiki/Minimum_bounding_box + * @param angle {Number} - Angle of the measurement + * @param distance {Number} - Distance to the closest point at given angle + * @private + */ _addPointToPolyGenerator(angle, distance) { let point = new Point(0, distance); - point.rotate(new Point(0, 0), angle); + point.rotateAroundZero(angle); if (this._poly.polyPointsCount === 0) { if (distance < 2000) { @@ -93,6 +129,14 @@ class LidarDriver extends EventEmitter { let polygon = new Polygon(this.name, Mep.Config.get('obstacleMaxPeriod'), polyPoints); let poi = new Point((this._poly.minX + this._poly.maxX) / 2, (this._poly.minY + this._poly.maxY) / 2); + /** + * Position changed event. + * @event drivers.lidar.LidarDriver#obstacleDetected + * @property {String} driverName - Unique name of a driver + * @property {misc.Point} poi - Point which is part of obstacle + * @property {misc.Polygon} polygon - Approximation of the obstacle + * @property {Boolean} detected - True if obstacle is detected + */ this.emit('obstacleDetected', this.name, poi, @@ -106,15 +150,24 @@ class LidarDriver extends EventEmitter { this._poly.previousPoint = point; } + /** + * Process data from lidar + * @param data {Buffer} - Buffer from lidar + * @private + */ _onDataReceived(data) { - if (data.length !== 4) { + if (data.length !== 4 || this._enabled === false) { + console.log(TAG, data); return; } let angle = ((data.readUInt8(0) & 0xFF) << 8) | data.readUInt8(1); let distance = ((data.readUInt8(2) & 0xFF) << 8) | data.readUInt8(3); - let scaledAngle = (360 - angle + 180) % 360; + if (this.config.inverted === true) { + angle *= -1; + } + let scaledAngle = ((360 + angle) + this.config.angle) % 360; this._readings[scaledAngle] = { distance: distance, time: (new Date).getTime() diff --git a/src/drivers/motion/MotionDriver.js b/src/drivers/motion/MotionDriver.js index 1c61913..e39d770 100644 --- a/src/drivers/motion/MotionDriver.js +++ b/src/drivers/motion/MotionDriver.js @@ -361,7 +361,7 @@ class MotionDriver extends EventEmitter { * Position changed event. * @event drivers.motion.MotionDriver#positionChanged * @property {String} driverName - Unique name of a driver - * @property {Point} point - Position of the robot + * @property {misc.Point} point - Position of the robot */ this.emit('positionChanged', this.name, diff --git a/src/drivers/pin/PinDriver.js b/src/drivers/pin/PinDriver.js index 01fe644..14e3c5a 100644 --- a/src/drivers/pin/PinDriver.js +++ b/src/drivers/pin/PinDriver.js @@ -49,6 +49,10 @@ class PinDriver extends EventEmitter { } } + /** + * Read value of given pin + * @returns {Promise} + */ read() { return new Promise((resolve, reject) => { if (this.config.direction === 'input') { @@ -63,6 +67,10 @@ class PinDriver extends EventEmitter { }); } + /** + * Write value to given pin + * @param value {Number} - [0, 1] for digital pins or [0 - 255] for analog pins + */ write(value) { if (this.config.direction === 'output') { if (this.config.mode === 'digital' && value != 1 && value != 0) { diff --git a/src/misc/Point.js b/src/misc/Point.js index 95e50ef..43584d3 100644 --- a/src/misc/Point.js +++ b/src/misc/Point.js @@ -5,7 +5,7 @@ * Point in 2D space * * @author Darko Lukic - * @memberof misc + * @memberOf misc */ class Point { /** @@ -37,7 +37,7 @@ class Point { * @returns {boolean} */ equals(point) { - return point.getX() === this.getX() && point.getY() === this.getY(); + return (point.getX() === this.getX() && point.getY() === this.getY()); } /** @@ -52,8 +52,9 @@ class Point { /** * Rotate point around origin point - * @param originPoint - Origin point - * @param angle - Rotation angle + * @param originPoint {misc.Point} - Origin point + * @param angleDegrees {Number} - Rotation angle + * @returns {misc.Point} */ rotate(originPoint, angleDegrees) { let angle = angleDegrees * (Math.PI / 180); @@ -66,9 +67,25 @@ class Point { return this; } + /** + * Optimized algorithm for point rotation around coordinate beginning + * @param angleDegrees {Number} + * @returns {misc.Point} + */ + rotateAroundZero(angleDegrees) { + let angle = angleDegrees * (Math.PI / 180); + + let x = Math.cos(angle) * this.x - Math.sin(angle) * this.y; + let y = Math.sin(angle) * this.x + Math.cos(angle) * this.y; + this.x = x | 0; + this.y = y | 0; + + return this; + } + /** * Clone the point - * @return {Point} - Cloned point + * @return {misc.Point} - Cloned point */ clone() { return (new Point(this.x, this.y)); diff --git a/src/misc/Polygon.js b/src/misc/Polygon.js index 60e5cef..7649bb0 100644 --- a/src/misc/Polygon.js +++ b/src/misc/Polygon.js @@ -4,6 +4,8 @@ const Point = Mep.require('misc/Point'); +const TAG = 'Polygon'; + /** * Describes an polygon * @see https://en.wikipedia.org/wiki/Polygon @@ -41,7 +43,7 @@ class Polygon { } setId(id) { - this.id = id + this.id = id; return this; } @@ -105,6 +107,19 @@ class Polygon { return new Polygon(this.tag, this.duration, points); } + /** + * Optimized algorithm for polygon rotation around coordinate beginning + * @param angleDegrees {Number} + * @returns {misc.Polygon} + */ + rotateAroundZero(angleDegrees) { + for (let point of this.points) { + point.rotateAroundZero(angleDegrees) + } + return this; + } + + // TODO: Consider new algorithm: http://stackoverflow.com/a/2752754/1983050 isPointInside(point) { let minX = this.points[0].getX(); let maxX = this.points[0].getX(); @@ -118,10 +133,7 @@ class Polygon { if (point.getY() < minY) minY = this.points[i].getY(); } - if (point.getX() < minX || point.getX() > maxX || point.getY() < minY || point.getY() > maxY) { - return false; - } - return true; + return !(point.getX() < minX || point.getX() > maxX || point.getY() < minY || point.getY() > maxY); } } diff --git a/src/services/terrain/TerrainService.js b/src/services/terrain/TerrainService.js index 218811d..eee3243 100644 --- a/src/services/terrain/TerrainService.js +++ b/src/services/terrain/TerrainService.js @@ -47,9 +47,10 @@ class TerrainService extends EventEmitter { Mep.DriverManager.callMethodByGroup('terrain', 'on', ['obstacleDetected', this._processObstacleDetection.bind(this)]); } - _processObstacleDetection(source, centerPoint, relativePolygon, detected) { - let poi = centerPoint.clone(); - poi.rotate(new Point(0, 0), Mep.Position.getOrientation()); + _processObstacleDetection(source, poi, polygon, detected) { + //let poi = centerPoint.clone(); + //poi.rotate(new Point(0, 0), Mep.Position.getOrientation()); + poi.rotateAroundZero(Mep.Position.getOrientation()); poi.translate(Mep.Position.getPosition()); // Process only if obstacle is in terrain @@ -60,15 +61,14 @@ class TerrainService extends EventEmitter { return; } - let polygon = relativePolygon.clone(); - polygon.rotate(new Point(0, 0), Mep.Position.getOrientation()); + //let polygon = relativePolygon.clone(); + //polygon.rotate(new Point(0, 0), Mep.Position.getOrientation()); + polygon.rotateAroundZero(Mep.Position.getOrientation()); polygon.translate(Mep.Position.getPosition()); if (detected === true) { this.addObstacle(polygon); this.emit('obstacleDetected', poi, polygon); - } else { - // TODO: Remove an obstacle } } From c8a9f157d869e9a608326a0b8100a13342296632 Mon Sep 17 00:00:00 2001 From: Darko Date: Sat, 1 Apr 2017 22:57:16 +0200 Subject: [PATCH 2/5] Add LunarCollectorDriver --- config/big.json | 54 ++++++++++++++ src/drivers/can/CanDriver.js | 2 +- .../dynamixel/DynamixelDriver.experiment.js | 36 ++++++--- src/drivers/dynamixel/DynamixelDriver.js | 73 +++++++++---------- .../lunarcollector/LunarCollectorDriver.js | 65 +++++++++++++++++ strategies/default/DefaultScheduler.js | 2 + strategies/default/InitTask.js | 24 +++++- 7 files changed, 205 insertions(+), 51 deletions(-) create mode 100644 src/drivers/lunarcollector/LunarCollectorDriver.js diff --git a/config/big.json b/config/big.json index eb5d51b..5309fae 100644 --- a/config/big.json +++ b/config/big.json @@ -12,6 +12,60 @@ } }, + "ServoCollectorTrackLeft": { + "@class": "drivers/dynamixel/DynamixelDriver", + "@load": true, + "@dependencies": { + "communicator": "CanDriver" + }, + "id": 4, + "cid": 2000 + }, + + "ServoCollectorHandLeft": { + "@class": "drivers/dynamixel/DynamixelDriver", + "@load": true, + "@dependencies": { + "communicator": "CanDriver" + }, + "id": 2, + "cid": 2000 + }, + + "ServoCollectorTrackRight": { + "@class": "drivers/dynamixel/DynamixelDriver", + "@load": true, + "@dependencies": { + "communicator": "CanDriver" + }, + "id": 9, + "cid": 2000 + }, + + + + "ServoCollectorHandRight": { + "@class": "drivers/dynamixel/DynamixelDriver", + "@load": true, + "@dependencies": { + "communicator": "CanDriver" + }, + "id": 8, + "cid": 2000 + }, + + "LunarCollector": { + "@class": "drivers/lunarcollector/LunarCollectorDriver", + "@load": true, + "@dependencies": { + "leftTrack": "ServoCollectorTrackLeft", + "rightTrack": "ServoCollectorTrackRight", + "leftHand": "ServoCollectorHandLeft", + "rightHand": "ServoCollectorHandRight" + } + }, + + "InfraredAuto": { "@class": "drivers/infrared/InfraredDriver", "@load": false, diff --git a/src/drivers/can/CanDriver.js b/src/drivers/can/CanDriver.js index f72359d..b4d8759 100644 --- a/src/drivers/can/CanDriver.js +++ b/src/drivers/can/CanDriver.js @@ -51,7 +51,7 @@ class CanDriver extends EventEmitter { */ canDriver.emit('data', message.id, message.data); - //Mep.Log.debug(TAG, 'Message received', message); + Mep.Log.debug(TAG, 'Message received', message); }); this.channel.start(); diff --git a/src/drivers/dynamixel/DynamixelDriver.experiment.js b/src/drivers/dynamixel/DynamixelDriver.experiment.js index 7a50fa6..58b6150 100644 --- a/src/drivers/dynamixel/DynamixelDriver.experiment.js +++ b/src/drivers/dynamixel/DynamixelDriver.experiment.js @@ -6,25 +6,38 @@ global.Mep = require('../../Mep'); const AX12 = require('./DynamixelDriver'); const CAN = require('../can/CanDriver'); -let ax2 = new AX12('test', { +let ican = new CAN('CANTest', {}); + +let ax1 = new AX12('test', { id: 2, cid: 2000, - _communicator: new CAN('CANTest', {}) + _communicator: ican }); - -let ax1 = new AX12('test', { - id: 1, +let ax2 = new AX12('test', { + id: 4, cid: 2000, - _communicator: new CAN('CANTest', {}) + _communicator: ican }); -//ax1.setLED(false); -ax1.setSpeed(200); + +//ax1.setLED(true); +//ax1.setSpeed(1000, false); + +ax1.getPosition() + .then((pos) => { console.log(pos); }) + .catch(() => { console.log('fail'); }); +ax1.go(500).then(() => { + console.log('success'); +}).catch(() => { + console.log('fail'); +}); +//ax2.setSpeed(500); +//ax2.setPosition(1000); function move(pos) { - ax1.setSpeed(100); + ax1.setSpeed(200); pos = Math.abs(pos); ax1.go(pos, {timeout: 2000}).then(() => { move((pos + 50) % 200 + 1); @@ -34,9 +47,10 @@ function move(pos) { move((pos + 50) % 200 + 1); }); } -console.log('start'); +//move(); +//console.log('start'); //ax1.go(1).then(() => { console.log('asdasdasd'); }); -move(100); +//move(100); //ax2.setId(2); /* diff --git a/src/drivers/dynamixel/DynamixelDriver.js b/src/drivers/dynamixel/DynamixelDriver.js index 090baa3..8da09b7 100644 --- a/src/drivers/dynamixel/DynamixelDriver.js +++ b/src/drivers/dynamixel/DynamixelDriver.js @@ -66,7 +66,6 @@ class DynamixelDriver { constructor(name, config) { this.config = Object.assign({ id: 0xFE, - type: 'AX', // AX or RX maxPosition: 1023, minPosition: 0 }, config); @@ -90,51 +89,46 @@ class DynamixelDriver { } _onDataReceived(data) { - // Length === 1 means there is an error in communication (UART, Servo <--> AVR) - if (data.length === 1) { - switch(data.readInt8(0)) { - case 0x03: - Mep.Log.error(TAG, this.name, 'RX Timeout error'); - break; - - case 0x02: - Mep.Log.error(TAG, this.name, 'RX Data corrupted'); - break; - - case 0x04: - Mep.Log.error(TAG, this.name, 'TX transfer failed'); - break; - - case 0x05: - Mep.Log.error(TAG, this.name, 'TX transfer timeout'); - break; - - default: - Mep.Log.error(TAG, this.name, 'Unhandled error', data); - break; + if (data.readUInt8(0) === (this.config.id | 0)) { + + // Length === 1 means there is an error in communication (UART, Servo <--> AVR) + if (data.length === 2) { + switch (data.readInt8(1)) { + case 0x03: + Mep.Log.error(TAG, this.name, 'RX Timeout error'); + break; + + case 0x02: + Mep.Log.error(TAG, this.name, 'RX Data corrupted'); + break; + + case 0x04: + Mep.Log.error(TAG, this.name, 'TX transfer failed'); + break; + + case 0x05: + Mep.Log.error(TAG, this.name, 'TX transfer timeout'); + break; + + default: + Mep.Log.error(TAG, this.name, 'Unhandled error', data); + break; + } + return; } - return; - } - if (data.readUInt8(0) === (this.config.id | 0)) { if (this.uniqueDataReceivedCallback !== null) { this.uniqueDataReceivedCallback(data); } } } - init(callback) { - callback(); - // Check status - } - getTemperature() { return this._read(DynamixelDriver.AX_PRESENT_TEMPERATURE); } async getVoltage() { - let val = await this._read(DynamixelDriver.AX_PRESENT_VOLTAGE); - return val / 10; + return await this._read(DynamixelDriver.AX_PRESENT_VOLTAGE); } getLoad() { @@ -169,7 +163,7 @@ class DynamixelDriver { go(position, config) { let c = Object.assign({ pollingPeriod: 40, - tolerance: 3, + tolerance: 15, timeout: 3000, firmwareImplementation: false }, config); @@ -205,7 +199,7 @@ class DynamixelDriver { checkPosition(); } } - }); + }).catch(checkPosition); }, c.pollingPeriod); }; checkPosition(); @@ -258,16 +252,19 @@ class DynamixelDriver { if (position > this.config.maxPosition || position < this.config.minPosition) { throw Error(TAG, this.name, 'Position out of range!'); } - this._writeWord(DynamixelDriver.AX_GOAL_POSITION_L, position | 0); } - setSpeed(speed) { + setSpeed(speed, inverse = false) { if (speed > 1023 || speed < 0) { Mep.Log.error(TAG, this.name, 'Speed out of range!'); return; } + if (inverse === true) { + speed = (1 << 10) | speed; + } + this._writeWord(DynamixelDriver.AX_GOAL_SPEED_L, speed | 0); } @@ -325,6 +322,8 @@ class DynamixelDriver { return; } + + ax.uniqueDataReceivedCallback = (data) => { // Catch error code if (data.readUInt8(2) !== 0x00) { diff --git a/src/drivers/lunarcollector/LunarCollectorDriver.js b/src/drivers/lunarcollector/LunarCollectorDriver.js new file mode 100644 index 0000000..4102f12 --- /dev/null +++ b/src/drivers/lunarcollector/LunarCollectorDriver.js @@ -0,0 +1,65 @@ +'use strict'; + +/** @namespace drivers.lunarcollector */ + +const TAG = 'LunarCollector'; + + +/** + * @param config.leftTrack {drivers.dynamixel.DynamixelDriver} + * @param config.rightTrack {drivers.dynamixel.DynamixelDriver} + * @param config.leftHand {drivers.dynamixel.DynamixelDriver} + * @param config.rightHand {drivers.dynamixel.DynamixelDriver} + * @memberOf drivers.lunarcollector + * @author Darko Lukic + */ +class LunarCollectorDriver { + constructor(name, config) { + this.config = Object.assign({ + + }, config); + this.name = name; + + this._leftTrack = Mep.getDriver(this.config['@dependencies']['leftTrack']); + this._rightTrack = Mep.getDriver(this.config['@dependencies']['rightTrack']); + this._leftHand = Mep.getDriver(this.config['@dependencies']['leftHand']); + this._rightHand = Mep.getDriver(this.config['@dependencies']['rightHand']); + + this._leftHand.setSpeed(300); + this._rightHand.setSpeed(300); + } + + collect() { + this._leftTrack.setSpeed(1023, true); + this._rightTrack.setSpeed(1023); + let leftHandPromise = this._leftHand.go(500, { tolerance: 20 }); + let rightHandPromise = this._rightHand.go(520, { tolerance: 20 }); + + //return new Promise((resolve, reject) => setTimeout(resolve, 100000)); + + return Promise.all([ + leftHandPromise, + rightHandPromise + ]); + } + + standby() { + this._leftTrack.setSpeed(0); + this._rightTrack.setSpeed(0); + let leftHandPromise = this._leftHand.go(860); + let rightHandPromise = this._rightHand.go(160); + + return Promise.all([ + leftHandPromise, + rightHandPromise + ]); + } + dump() { + + } + getGroups() { + return []; + } +} + +module.exports = LunarCollectorDriver; \ No newline at end of file diff --git a/strategies/default/DefaultScheduler.js b/strategies/default/DefaultScheduler.js index b3a95f6..7fd0bcf 100644 --- a/strategies/default/DefaultScheduler.js +++ b/strategies/default/DefaultScheduler.js @@ -12,6 +12,8 @@ class DefaultScheduler extends Scheduler { ]; this.runTask(this.tasks[0]); + + this.lunarCollectorFull = false; } } diff --git a/strategies/default/InitTask.js b/strategies/default/InitTask.js index 76f1e5a..191eb10 100644 --- a/strategies/default/InitTask.js +++ b/strategies/default/InitTask.js @@ -7,6 +7,12 @@ const Delay = Mep.require('misc/Delay'); const TAG = 'InitTask'; class InitTask extends Task { + constructor(scheduler, weight, time, location) { + super(scheduler, weight, time, location); + + this.lunar = Mep.getDriver('LunarCollector'); + } + // Simplified functions for prompt go(x, y, config) { Mep.Motion.go(new TunedPoint(x, y), config); @@ -33,12 +39,26 @@ class InitTask extends Task { } async onRun() { + this.scheduler.lunarCollectorFull = true; + await starter.waitStartSignal(this); try { - await Mep.Motion.go(new TunedPoint(0, 0), { speed: 120, tolerance: -1, pf: true, rerouting: false }); + //await Mep.Motion.go(new TunedPoint(-1200, 0), { speed: 80, tolerance: -1, pf: false, rerouting: false }); + + //Mep.getDriver('MotionDriver').setSpeed(70); + + for (let i = 0; i < 4; i++) { + await this.lunar.collect(); + //await Mep.Motion.straight(-30); + + await this.lunar.standby(); + //await Mep.Motion.straight(30); + } + + - await this.home(); + // await this.home(); } catch (e) { Mep.Log.error(TAG, e); } From b98d85eea7218b70b4159fdfc02ed51ed52b2e20 Mon Sep 17 00:00:00 2001 From: Darko Date: Mon, 3 Apr 2017 19:18:51 +0200 Subject: [PATCH 3/5] Add H-Bridge driver --- config/big.json | 24 +++++++-- src/drivers/dynamixel/DynamixelDriver.js | 13 ++--- .../hbridge/HBridgeDriver.experiment.js | 27 ++++++++++ src/drivers/hbridge/HBridgeDriver.js | 51 +++++++++++++++++++ .../lunarcollector/LunarCollectorDriver.js | 30 +++++++---- src/services/motion/MotionService.js | 29 ++++++----- strategies/default/InitTask.js | 19 +++---- 7 files changed, 150 insertions(+), 43 deletions(-) create mode 100644 src/drivers/hbridge/HBridgeDriver.experiment.js create mode 100644 src/drivers/hbridge/HBridgeDriver.js diff --git a/config/big.json b/config/big.json index 5309fae..782ec0e 100644 --- a/config/big.json +++ b/config/big.json @@ -12,6 +12,16 @@ } }, + "ServoCollectorStopper": { + "@class": "drivers/dynamixel/DynamixelDriver", + "@load": true, + "@dependencies": { + "communicator": "CanDriver" + }, + "id": 6, + "cid": 2000 + }, + "ServoCollectorTrackLeft": { "@class": "drivers/dynamixel/DynamixelDriver", "@load": true, @@ -42,8 +52,6 @@ "cid": 2000 }, - - "ServoCollectorHandRight": { "@class": "drivers/dynamixel/DynamixelDriver", "@load": true, @@ -54,6 +62,15 @@ "cid": 2000 }, + "CollectorBigTrack": { + "@class": "drivers/hbridge/HBridgeDriver", + "@load": true, + "@dependencies": { + "communicator": "CanDriver" + }, + "cid": 1101 + }, + "LunarCollector": { "@class": "drivers/lunarcollector/LunarCollectorDriver", "@load": true, @@ -61,7 +78,8 @@ "leftTrack": "ServoCollectorTrackLeft", "rightTrack": "ServoCollectorTrackRight", "leftHand": "ServoCollectorHandLeft", - "rightHand": "ServoCollectorHandRight" + "rightHand": "ServoCollectorHandRight", + "bigTrack": "CollectorBigTrack" } }, diff --git a/src/drivers/dynamixel/DynamixelDriver.js b/src/drivers/dynamixel/DynamixelDriver.js index 8da09b7..009ab83 100644 --- a/src/drivers/dynamixel/DynamixelDriver.js +++ b/src/drivers/dynamixel/DynamixelDriver.js @@ -1,10 +1,12 @@ 'use strict'; /** @namespace drivers.dynamixel */ +const TaskError = Mep.require('strategy/TaskError'); -const TAG = 'Dynamixel'; +const TAG = 'DynamixelDriver'; + /** * Communicates with dynamixel servos (AX12 & RX24). * NOTE: This class doesn't send start bytes & checksum, please make custom `communicator` @@ -162,8 +164,8 @@ class DynamixelDriver { */ go(position, config) { let c = Object.assign({ - pollingPeriod: 40, - tolerance: 15, + pollingPeriod: 150, + tolerance: 20, timeout: 3000, firmwareImplementation: false }, config); @@ -176,7 +178,7 @@ class DynamixelDriver { // Apply time out setTimeout(() => { timeout = true; - reject(); + reject(new TaskError(TAG, 'timeout', 'Dynamixel cannot reach position in time')); }, c.timeout); if (c.firmwareImplementation === true) { @@ -317,8 +319,7 @@ class DynamixelDriver { return new Promise((resolve, reject) => { if (ax.config.id === 0xFE) { - Mep.Log.error(TAG, this.name, 'Cannot use broadcast ID for reading'); - reject(); + reject(new TaskError(TAG, 'broadcast', 'Cannot use broadcast ID for reading')); return; } diff --git a/src/drivers/hbridge/HBridgeDriver.experiment.js b/src/drivers/hbridge/HBridgeDriver.experiment.js new file mode 100644 index 0000000..4847a87 --- /dev/null +++ b/src/drivers/hbridge/HBridgeDriver.experiment.js @@ -0,0 +1,27 @@ +global.Mep = require('../../Mep'); +const HBridge = require('./HBridgeDriver'); +const CAN = require('../can/CanDriver'); + +let can = new CAN('CANTest', {}); + + + +let hbridge = new HBridge('TestHBridge', { + cid: 1101, + _communicator: can +}); + + +hbridge.start(100); + +setTimeout(() => { + hbridge.stop(); +}, 1000); + +setTimeout(() => { + hbridge.start(100, true); +}, 2000); + +setTimeout(() => { + hbridge.stop(); +}, 3000); \ No newline at end of file diff --git a/src/drivers/hbridge/HBridgeDriver.js b/src/drivers/hbridge/HBridgeDriver.js new file mode 100644 index 0000000..dc0cd4a --- /dev/null +++ b/src/drivers/hbridge/HBridgeDriver.js @@ -0,0 +1,51 @@ +'use strict'; + +/** @namespace drivers.hbridge */ + +const TAG = 'HBridgeDriver'; + + +/** + * @param {Object} config Additional parameters + * @param {Number} [config.cid] Communication ID + * @param {String} [config.@dependecies.communicator] Name of communication driver + * @memberOf drivers.hbridge + * @author Darko Lukic + */ +class HBridgeDriver { + constructor(name, config) { + this.config = Object.assign({ + + }, config); + this.name = name; + + + // Set up communicator + this.communicator = null; + if (this.config._communicator !== undefined) { + // For testing purposes only (experiments) + this.communicator = this.config._communicator; + } else { + this.communicator = Mep.getDriver(this.config['@dependencies'].communicator); + } + } + + start(speed, inverse = false) { + let buffer = Buffer.from([ + speed | 0, + (inverse === true) ? 1 : 0 + ]); + + this.communicator.send(this.config.cid, buffer); + } + + stop() { + this.start(0); + } + + getGroups() { + return []; + } +} + +module.exports = HBridgeDriver; \ No newline at end of file diff --git a/src/drivers/lunarcollector/LunarCollectorDriver.js b/src/drivers/lunarcollector/LunarCollectorDriver.js index 4102f12..9f6d72c 100644 --- a/src/drivers/lunarcollector/LunarCollectorDriver.js +++ b/src/drivers/lunarcollector/LunarCollectorDriver.js @@ -6,10 +6,10 @@ const TAG = 'LunarCollector'; /** - * @param config.leftTrack {drivers.dynamixel.DynamixelDriver} - * @param config.rightTrack {drivers.dynamixel.DynamixelDriver} - * @param config.leftHand {drivers.dynamixel.DynamixelDriver} - * @param config.rightHand {drivers.dynamixel.DynamixelDriver} + * @param config.leftTrack {String} - Name of Dynamixel driver which runs left track + * @param config.rightTrack {String} - Name of Dynamixel driver which runs right track + * @param config.leftHand {String} - Name of Dynamixel driver which runs left hand + * @param config.rightHand {String} - Name of Dynamixel driver which runs right hand * @memberOf drivers.lunarcollector * @author Darko Lukic */ @@ -24,18 +24,31 @@ class LunarCollectorDriver { this._rightTrack = Mep.getDriver(this.config['@dependencies']['rightTrack']); this._leftHand = Mep.getDriver(this.config['@dependencies']['leftHand']); this._rightHand = Mep.getDriver(this.config['@dependencies']['rightHand']); + this._bigTrack = Mep.getDriver(this.config['@dependencies']['bigTrack']); - this._leftHand.setSpeed(300); - this._rightHand.setSpeed(300); + this._leftHand.setSpeed(600); + this._rightHand.setSpeed(600); } collect() { this._leftTrack.setSpeed(1023, true); this._rightTrack.setSpeed(1023); + this._bigTrack.start(100); let leftHandPromise = this._leftHand.go(500, { tolerance: 20 }); let rightHandPromise = this._rightHand.go(520, { tolerance: 20 }); - //return new Promise((resolve, reject) => setTimeout(resolve, 100000)); + return Promise.all([ + leftHandPromise, + rightHandPromise + ]); + } + + prepare() { + this._leftTrack.setSpeed(0); + this._rightTrack.setSpeed(0); + let leftHandPromise = this._leftHand.go(600); + let rightHandPromise = this._rightHand.go(400); + this._bigTrack.start(100); return Promise.all([ leftHandPromise, @@ -46,6 +59,7 @@ class LunarCollectorDriver { standby() { this._leftTrack.setSpeed(0); this._rightTrack.setSpeed(0); + this._bigTrack.stop(); let leftHandPromise = this._leftHand.go(860); let rightHandPromise = this._rightHand.go(160); @@ -54,9 +68,7 @@ class LunarCollectorDriver { rightHandPromise ]); } - dump() { - } getGroups() { return []; } diff --git a/src/services/motion/MotionService.js b/src/services/motion/MotionService.js index 3a8fa3b..786a4c7 100644 --- a/src/services/motion/MotionService.js +++ b/src/services/motion/MotionService.js @@ -126,15 +126,15 @@ class MotionService extends EventEmitter { /** * Move the robot, set new position of the robot - * - * @param {TunedPoint} tunedPoint - Point that should be reached - * @param {Boolean} parameters.pf - Use terrain finding algorithm - * @param {Boolean} parameters.backward - Enable backward robot moving - * @param {Boolean} params.rerouting - Enable rerouting during the movement - * @param {Boolean} parameters.relative - Use relative to previous position - * @param {Number} parameters.tolerance - Position will consider as reached if Euclid's distance between current - * and required position is less than tolerance - * @param {Number} parameters.speed - Speed of the robot movement in range (0, 255) + * @param {TunedPoint} tunedPoint Point that should be reached + * @param {Object} [parameters] Configuration options. + * @param {Boolean} [parameters.pf] Use terrain finding algorithm. + * @param {Boolean} [parameters.backward] Set backward robot moving. + * @param {Boolean} [parameters.rerouting] Enable rerouting during the movement. + * @param {Boolean} [parameters.relative] Use relative to previous position. + * @param {Number} [parameters.tolerance] Position will consider as reached if Euclid's distance between current + * and required position is less than tolerance. + * @param {Number} [parameters.speed] Speed of the robot movement in range (0, 255). * @returns {Promise} */ go(tunedPoint, parameters) { @@ -187,10 +187,11 @@ class MotionService extends EventEmitter { /** * Go to single point without advanced features - * @param point {misc.Point} - Target point - * @param params.backward {Boolean} - Move robot backward - * @param params.tolerance {Number} - Max radius - * @param params.speed {Number} - Speed + * @param {misc.Point} point Target point + * @param {Object} params Additional options + * @param {Boolean} [params.backward] Move robot backward + * @param {Number} [params.tolerance] Max radius + * @param {Number} [params.speed] Speed * @return {Promise} * @private */ @@ -220,7 +221,7 @@ class MotionService extends EventEmitter { /** * Stop the robot - * @param softStop - If true robot will turn of motors + * @param {Boolean} softStop If true robot will turn of motors */ stop(softStop = false) { this.pause(); diff --git a/strategies/default/InitTask.js b/strategies/default/InitTask.js index 191eb10..8a689fa 100644 --- a/strategies/default/InitTask.js +++ b/strategies/default/InitTask.js @@ -28,7 +28,7 @@ class InitTask extends Task { } async home() { - await Mep.Motion.go(new TunedPoint(-1000, 0), { pf: true, tolerance: -1, speed: 100, backward: true }); + await Mep.Motion.go(new TunedPoint(-1300, 0), { pf: true, tolerance: -1, speed: 100, backward: true }); await Delay(200); await Mep.Motion.rotate(new TunedAngle(0)); console.log('Arrived to home'); @@ -39,23 +39,20 @@ class InitTask extends Task { } async onRun() { - this.scheduler.lunarCollectorFull = true; - await starter.waitStartSignal(this); try { - //await Mep.Motion.go(new TunedPoint(-1200, 0), { speed: 80, tolerance: -1, pf: false, rerouting: false }); - - //Mep.getDriver('MotionDriver').setSpeed(70); + //await Mep.Motion.go(new TunedPoint(0, 0), { speed: 80, tolerance: -1, pf: false, rerouting: false }); for (let i = 0; i < 4; i++) { await this.lunar.collect(); - //await Mep.Motion.straight(-30); - - await this.lunar.standby(); - //await Mep.Motion.straight(30); + await Delay(700); + await Mep.Motion.straight(-20); + await Delay(1000); + await this.lunar.prepare(); + await Mep.Motion.straight(20); } - + await this.lunar.standby(); // await this.home(); From 48ab18c9a0e6c6c3ac1daf7827e7467e17e2af68 Mon Sep 17 00:00:00 2001 From: Darko Date: Mon, 3 Apr 2017 19:52:45 +0200 Subject: [PATCH 4/5] Fix lidar angle --- src/drivers/can/CanDriver.js | 2 +- src/drivers/lidar/LidarDriver.js | 2 +- strategies/default/InitTask.js | 17 +++++++++++++---- 3 files changed, 15 insertions(+), 6 deletions(-) diff --git a/src/drivers/can/CanDriver.js b/src/drivers/can/CanDriver.js index b4d8759..f72359d 100644 --- a/src/drivers/can/CanDriver.js +++ b/src/drivers/can/CanDriver.js @@ -51,7 +51,7 @@ class CanDriver extends EventEmitter { */ canDriver.emit('data', message.id, message.data); - Mep.Log.debug(TAG, 'Message received', message); + //Mep.Log.debug(TAG, 'Message received', message); }); this.channel.start(); diff --git a/src/drivers/lidar/LidarDriver.js b/src/drivers/lidar/LidarDriver.js index 4198197..78b7a78 100644 --- a/src/drivers/lidar/LidarDriver.js +++ b/src/drivers/lidar/LidarDriver.js @@ -23,7 +23,7 @@ class LidarDriver extends EventEmitter { cid: 8000, tolerance: 400, volume: 230, - angle: 180, + angle: -90, inverted: true }, config); this.name = name; diff --git a/strategies/default/InitTask.js b/strategies/default/InitTask.js index 8a689fa..315f1f3 100644 --- a/strategies/default/InitTask.js +++ b/strategies/default/InitTask.js @@ -41,16 +41,25 @@ class InitTask extends Task { async onRun() { await starter.waitStartSignal(this); + + await Mep.Motion.go(new TunedPoint(0, 0)); + await this.home(); + + return; try { //await Mep.Motion.go(new TunedPoint(0, 0), { speed: 80, tolerance: -1, pf: false, rerouting: false }); for (let i = 0; i < 4; i++) { await this.lunar.collect(); await Delay(700); - await Mep.Motion.straight(-20); - await Delay(1000); - await this.lunar.prepare(); - await Mep.Motion.straight(20); + if (i !== 3) { + await Mep.Motion.straight(-20); + await Delay(1000); + this.lunar.prepare(); + await Mep.Motion.straight(20); + } else { + await Delay(1000); + } } await this.lunar.standby(); From 1227cf32a2d22ab9570e85345ca5b8d775946294 Mon Sep 17 00:00:00 2001 From: Darko Date: Mon, 3 Apr 2017 19:59:11 +0200 Subject: [PATCH 5/5] Update sinon --- package.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.json b/package.json index 4fc8219..225413e 100644 --- a/package.json +++ b/package.json @@ -28,7 +28,7 @@ "nconf": "^0.8.4", "require-new": "^1.1.0", "simplify-js": "^1.2.1", - "sinon": "^1.17.6", + "sinon": "^2.1.0", "socketcan": "^2.1.3", "termios": "^0.1.3", "usage": "^0.7.1"