src/drivers/motion/MotionDriver.js
'use strict';
/** @namespace drivers.motion */
const Point = Mep.require("misc/geometry/Point");
const EventEmitter = require('events');
const Buffer = require('buffer').Buffer;
const TaskError = Mep.require('strategy/TaskError');
const TunedPoint = Mep.require('strategy/TunedPoint');
const TunedAngle = Mep.require('strategy/TunedAngle');
const Delay = Mep.require('misc/Delay');
const TAG = 'MotionDriver';
/**
* Driver enables communication with Memristor's motion driver.
* @memberof drivers.motion
* @author Darko Lukic <lukicdarkoo@gmail.com>
* @fires drivers.motion.MotionDriver#positionChanged
* @fires drivers.motion.MotionDriver#orientationChanged
* @fires drivers.motion.MotionDriver#stateChanged
*/
class MotionDriver extends EventEmitter {
static get STATE_IDLE() { return 'I'.charCodeAt(0); }
static get STATE_STUCK() { return 'S'.charCodeAt(0); }
static get STATE_MOVING() { return 'M'.charCodeAt(0); }
static get STATE_ROTATING() { return 'R'.charCodeAt(0); }
static get STATE_ERROR() { return 'E'.charCodeAt(0); }
static get STATE_UNDEFINED() { return 'U'.charCodeAt(0); }
static get STATE_BREAK() { return 'B'.charCodeAt(0); }
static get DIRECTION_FORWARD() { return 1; }
static get DIRECTION_UNDEFINED() { return 0; }
static get DIRECTION_BACKWARD() { return -1; }
static get CONFIG_DISTANCE_REGULATOR() { return 0; }
static get CONFIG_ROTATION_REGULATOR() { return 1; }
static get CONFIG_ENABLE_STUCK() { return 2; }
static get CONFIG_STUCK() { return 3; }
static get CONFIG_DEBUG() { return 4; }
static get CONFIG_STATUS_CHANGE_REPORT() { return 5; }
static get CONFIG_KEEP_COUNT() { return 6; }
static get CONFIG_TMR() { return 7; }
static get CONFIG_STUCK_DISTANCE_JUMP() { return 8; }
static get CONFIG_STUCK_ROTATION_JUMP() { return 9; }
static get CONFIG_STUCK_DISTANCE_MAX_FAIL_COUNT() { return 10; }
static get CONFIG_STUCK_ROTATION_MAX_FAIL_COUNT() { return 11; }
static get CONFIG_MOTOR_SPEED_LIMIT() { return 12; }
static get CONFIG_MOTOR_RATE_OF_CHANGE() { return 13; }
static get CONFIG_SEND_STATUS_INTERVAL() { return 15; }
static get CONFIG_WHEEL_DISTANCE() { return 15; }
static get CONFIG_WHEEL_R1() { return 16; }
static get CONFIG_WHEEL_R2() { return 17; }
static get CONFIG_PID_D_P() { return 18; }
static get CONFIG_PID_D_D() { return 19; }
static get CONFIG_PID_R_P() { return 20; }
static get CONFIG_PID_R_D() { return 21; }
static get CONFIG_VMAX() { return 22; }
static get CONFIG_OMEGA() { return 23; }
static get CONFIG_ACCEL() { return 24; }
static get CONFIG_ALPHA() { return 25; }
static get CONFIG_SLOWDOWN() { return 26; }
static get CONFIG_ANGLE_SPEEDUP() { return 27; }
/**
* @param {String} name Unique driver name
* @param {Object} config Configuration presented as an associative array
* @param {strategy.TunedPoint} [config.startPosition] X coordinate for start position
* @param {strategy.TunedAngle} [config.startOrientation] Start orientation
* @param {Number} [config.startSpeed] Initial speed
* @param {Number} [config.refreshDataPeriod] Get state from motion driver every `refreshDataPeriod` in ms
* @param {Number} [config.connectionTimeout] Connection timeout in ms
* @param {Number} [config.ackTimeout] Acknowledgment timeout
*/
constructor(name, config) {
super();
this.name = name;
this.config = Object.assign({
startOrientation: 0,
startSpeed: 100,
refreshDataPeriod: 100,
connectionTimeout: 4000,
ackTimeout: 150,
commandTimeout: (10 * 1000),
commandStartTimeout: (2 * 1000)
}, config);
this.positon = new TunedPoint(...this.config.startPosition).getPoint();
this.orientation = new TunedAngle(...config.startOrientation).getAngle();
this._activeSpeed = config.startSpeed;
this._breaking = false;
this._direction = MotionDriver.DIRECTION_UNDEFINED;
this.communicator = Mep.DriverManager.getDriver(config['@dependencies'].communicator);
this._waitACKQueue = {};
if(this.config.cid === undefined) {
this.communicator.on('data', this._onDataReceived.bind(this));
} else {
this.communicator.on('data_' + this.config.cid, this._onDataReceivedCAN.bind(this));
}
}
_waitACK(type) {
let motionDriver = this;
setTimeout(() => {
if (this._waitACKQueue[type] !== undefined) {
Mep.Log.error(TAG, 'Error sending a command', type);
motionDriver._sendCommand(this._waitACKQueue[type]);
}
}, this.config.ackTimeout);
}
_sendCommand(buff) {
if(this.config.cid === undefined) {
// using UART
let type = buff.readUInt8(0);
this._waitACKQueue[type] = buff;
this._waitACK(type);
this.communicator.send(buff);
} else {
// using CAN
this.communicator.send(this.config.cid, buff);
}
}
getDirection() {
return this._direction;
}
/**
* Check driver by checking checking communication with motion driver board
*/
async init() {
let motionDriver = this;
this.reset();
this.setPositionAndOrientation(
this.positon.getX(),
this.positon.getY(),
this.orientation
);
this.setConfig(MotionDriver.CONFIG_SEND_STATUS_INTERVAL, this.config.refreshDataPeriod, 0);
this.setConfig(28, 3.0, 2);
this.setConfig(29, 1.2, 1);
this.setConfig(30, 0.2, 1);
//this.setConfig(27, 10, 0);
this.requestRefreshData();
return new Promise((resolve) => {
let driverChecker = setInterval(() => {
if (motionDriver.getState() !== MotionDriver.STATE_UNDEFINED) {
clearInterval(driverChecker);
Mep.Log.info(TAG, 'Communication is validated');
resolve();
}
}, 100);
setTimeout(() => {
if (motionDriver.getState() === MotionDriver.STATE_UNDEFINED) {
throw Error(TAG, 'No response from motion driver');
}
}, this.config.connectionTimeout);
})
}
/**
* Finish `moveToCurvilinear` command and prepare robot for another one
*/
finishCommand() {
this._sendCommand(Buffer.from(['i'.charCodeAt(0)]));
Mep.Log.debug(TAG, 'Finish command sent');
}
/**
* Reset all settings in motion driver
*/
reset() {
this._sendCommand(Buffer.from(['R'.charCodeAt(0)]));
}
/**
* Request state, position and orientation from motion driver
*/
requestRefreshData() {
this._sendCommand(Buffer.from(['P'.charCodeAt(0)]));
}
/**
* Reset position and orientation
* @param x {Number} - New X coordinate relative to start position of the robot
* @param y {Number} - New Y coordinate relative to start position of the robot
* @param orientation {Number} - New robot's orientation
*/
setPositionAndOrientation(x, y, orientation) {
this._sendCommand(Buffer.from([
'I'.charCodeAt(0),
x >> 8,
x & 0xFF,
y >> 8,
y & 0xFF,
orientation >> 8,
orientation & 0xFF
]));
}
setConfig(key, value, exp = 3) {
let fixedValue = (value * Math.pow(10, exp)) | 0;
let buffer = Buffer.from([
'c'.charCodeAt(0),
key & 0xFF,
(fixedValue >> 20) & 0x0F,
(fixedValue >> 12) & 0xFF,
(fixedValue >> 4) & 0xFF,
((fixedValue & 0x0F) << 4) | (exp & 0x0F)
]);
this._sendCommand(buffer);
}
getConfig(key) {
let buffer = Buffer.from([
'C'.charCodeAt(0),
key & 0xFF
]);
this._sendCommand(buffer);
}
_onCReceived(buffer) {
let value = buffer.readUInt8(0) << 20;
value |= buffer.readUInt8(1) << 12;
value |= buffer.readUInt8(2) << 4;
value |= buffer.readUInt8(3) >> 4;
let exp = buffer.readUInt8(3) & 0x0F;
let result = value / Math.pow(10, exp);
Mep.Log.info(TAG, 'Value =', result)
}
/**
* Rotate robot to given angle
* @param {Number} angle Angle
* @returns {Promise}
*/
rotateTo(angle) {
this._direction = MotionDriver.DIRECTION_UNDEFINED;
this._sendCommand(Buffer.from([
'A'.charCodeAt(0),
angle >> 8,
angle & 0xFF
]));
this._breaking = false;
return this._promiseToStateChanged();
}
/**
* Rotate for given angle
* @param {Number} angle
* @returns {Promise}
*/
rotateFor(angle) {
this._direction = MotionDriver.DIRECTION_UNDEFINED;
this._sendCommand(Buffer.from([
'T'.charCodeAt(0),
angle >> 8,
angle & 0xFF
]));
this._breaking = false;
return this._promiseToStateChanged();
}
_promiseToStateChanged() {
let motionDriver = this;
let timeout = setTimeout(() => {
motionDriver.emit('stateChanged', MotionDriver.STATE_STUCK);
}, this.config.commandTimeout);
let startTimeout = setTimeout(() => {
motionDriver.emit('stateChanged', MotionDriver.STATE_STUCK);
}, this.config.commandStartTimeout);
return new Promise((resolve, reject) => {
let stateListener = (name, state) => {
switch (state) {
case MotionDriver.STATE_IDLE:
Mep.Log.debug(TAG, 'Resolved');
clearTimeout(timeout);
clearTimeout(startTimeout);
resolve();
motionDriver.removeListener('stateChanged', stateListener);
break;
case MotionDriver.STATE_STUCK:
clearTimeout(timeout);
clearTimeout(startTimeout);
reject(new TaskError(TAG, 'stuck', 'Robot is stacked'));
motionDriver.removeListener('stateChanged', stateListener);
break;
case MotionDriver.STATE_ERROR:
clearTimeout(timeout);
clearTimeout(startTimeout);
reject(new TaskError(TAG, 'error', 'Unknown moving error'));
motionDriver.removeListener('stateChanged', stateListener);
break;
case MotionDriver.STATE_BREAK:
clearTimeout(timeout);
clearTimeout(startTimeout);
reject(new TaskError(TAG, 'break', 'Command is broken by another one'));
motionDriver.removeListener('stateChanged', stateListener);
break;
default:
clearTimeout(startTimeout);
break;
}
};
this.on('stateChanged', stateListener);
});
}
/**
* Move robot forward or backward depending on sign
* @param {Number} millimeters
* @deprecated
*/
goForward(millimeters) {
this._direction = (millimeters > 0) ?
MotionDriver.DIRECTION_FORWARD : MotionDriver.DIRECTION_BACKWARD;
this._sendCommand(Buffer.from([
'D'.charCodeAt(0),
millimeters >> 8,
millimeters & 0xFF,
0
]));
return this._promiseToStateChanged();
}
/**
* Stop the robot.
*/
stop() {
this._direction = MotionDriver.DIRECTION_UNDEFINED;
this._breaking = true;
this.state = MotionDriver.STATE_IDLE;
this.emit('stateChanged', this.name, MotionDriver.STATE_BREAK);
this._sendCommand(Buffer.from(['S'.charCodeAt(0)]));
return new Promise((resolve, reject) => {
setTimeout(resolve, 700);
});
}
/**
* Stop robot by turning off motors.
*/
softStop() {
this._direction = MotionDriver.DIRECTION_UNDEFINED;
this._breaking = true;
this.emit('stateChanged', this.name, MotionDriver.STATE_BREAK);
this._sendCommand(Buffer.from(['s'.charCodeAt(0)]));
return new Promise((resolve, reject) => {
resolve();
});
}
/**
* Set default speed of the robot
* @param {Number} speed Speed (0 - 255)
*/
setSpeed(speed) {
this._activeSpeed = speed | 0;
this._sendCommand(Buffer.from([
'V'.charCodeAt(0),
this._activeSpeed | 0
]));
}
/**
* Move robot to absolute position
* @param {misc.Point} position Required position of the robot
* @param {Number} direction Direction, can be MotionDriver.DIRECTION_FORWARD or
* MotionDriver.DIRECTION_BACKWARD
*/
moveToPosition(position, direction) {
this._direction = direction;
this._sendCommand(Buffer.from([
'G'.charCodeAt(0),
position.getX() >> 8,
position.getX() & 0xff,
position.getY() >> 8,
position.getY() & 0xff,
0,
direction
]));
this._breaking = false;
return this._promiseToStateChanged();
}
/**
* Move robot to absolute position but robot make curves to speed up motion. This
* command requires `finishCommand()` before next motion command.
*
* @param {misc.Point} position Required position of the robot
* @param {Number} direction Direction, can be MotionDriver.DIRECTION_FORWARD or MotionDriver.DIRECTION_BACKWARD
* @param {Number} radius
* @param {Number} tolerance
*/
moveToCurvilinear(position, direction, radius, tolerance) {
let motionDriver = this;
this._direction = direction;
this._sendCommand(Buffer.from([
'N'.charCodeAt(0),
position.getX() >> 8,
position.getX() & 0xff,
position.getY() >> 8,
position.getY() & 0xff,
direction,
radius >> 8,
radius & 0xff,
]));
this._breaking = false;
return new Promise((resolve, reject) => {
let positionListener = (name, currentPosition) => {
if (currentPosition.getDistance(position) <= tolerance) {
motionDriver.state = MotionDriver.STATE_IDLE;
motionDriver.finishCommand();
motionDriver.emit('stateChanged', motionDriver.name, motionDriver.state);
//motionDriver.removeListener('positionChanged', positionListener);
//resolve();
}
};
this.on('positionChanged', positionListener);
this._promiseToStateChanged()
.then(() => {
motionDriver.removeListener('positionChanged', positionListener);
resolve();
})
.catch((e) => {
motionDriver.removeListener('positionChanged', positionListener);
reject(e);
});
});
}
/**
* Packet type P is received
* @param {Buffer} buffer Payload
* @private
*/
_onPReceived(buffer) {
// Ignore garbage
let state = buffer.readInt8(0);
let position = new Point(
(buffer.readInt8(1) << 8) | (buffer.readInt8(2) & 0xFF),
(buffer.readInt8(3) << 8) | (buffer.readInt8(4) & 0xFF)
);
let orientation = (buffer.readInt8(5) << 8) | (buffer.readInt8(6) & 0xFF);
// let speed = buffer.readInt8(7);
// Checks
if ([MotionDriver.STATE_MOVING,
MotionDriver.STATE_IDLE,
MotionDriver.STATE_ROTATING,
MotionDriver.STATE_ERROR,
MotionDriver.STATE_STUCK].indexOf(state) < 0) {
return;
}
if (orientation < -180 || orientation > 180) return;
if (position.getX() > 2000 || position.getX() < -2000) return;
if (position.getY() > 2000 || position.getY() < -2000) return;
if (this.positon.equals(position) === false) {
this.positon = position;
/**
* Position changed event.
* @event drivers.motion.MotionDriver#positionChanged
* @property {String} driverName Unique name of a driver
* @property {misc.Point} point Position of the robot
*/
this.emit('positionChanged',
this.name,
this.positon,
this.config.precision
);
}
// If state is changed
if (state !== this.state) {
if (this._breaking === false) {
this.state = state;
/**
* State change event.
* @event drivers.motion.MotionDriver#stateChanged
* @property {Number} state New state
*/
this.emit('stateChanged', this.name, this.state);
Mep.Log.debug(TAG, 'State', String.fromCharCode(this.state));
}
}
if (orientation !== this.orientation) {
this.orientation = orientation;
/**
* Orientation change event.
* @event drivers.motion.MotionDriver#orientationChanged
* @property {String} driverName Unique name of a driver
* @property {Number} orientation New orientation
*/
this.emit('orientationChanged',
this.name,
orientation,
this.config.precision
);
}
}
_onAReceived(buffer) {
delete this._waitACKQueue[buffer.readUInt8()];
}
_onDataReceivedCAN(buffer) {
if(buffer.length == 0) return;
let type = buffer.readUInt8(0);
let buf = buffer.slice(1);
if (type == 'P'.charCodeAt(0) && buf.length === 7) {
this._onPReceived(buf);
}
else if (type == 'A'.charCodeAt(0) && buf.length === 0) {
this._onAReceived(buf);
}
else if (type == 'C'.charCodeAt(0) && buf.length === 4) {
this._onCReceived(buf);
}
}
/**
* Callback will be called when new packet is arrived and it will dispatch event to new
* callback depending on packet type
* @param {Buffer} buffer Payload
* @param {String} type Packet type
* @private
*/
_onDataReceived(buffer, type) {
if (type == 'P'.charCodeAt(0) && buffer.length === 8) {
this._onPReceived(buffer);
}
else if (type == 'A'.charCodeAt(0) && buffer.length === 1) {
this._onAReceived(buffer);
}
else if (type == 'C'.charCodeAt(0) && buffer.length === 4) {
this._onCReceived(buffer);
}
}
/**
* Get position of the robot
* @return {misc.Point} Position of the robot
*/
getPosition() {
return this.positon;
}
getState() {
return this.state;
}
getGroups() {
return ['position'];
}
getOrientation() {
return this.orientation;
}
getActiveSpeed() {
return this._activeSpeed;
}
}
module.exports = MotionDriver;