Uart, Motion and Task lot of minor fixes after real world tests

config/default.json

@@ -43,7 +43,7 @@
             "startOrientation": 0,
             "startSpeed": 100,
             "precision": 2,
-            "refreshDataPeriod": 300
+            "refreshDataPeriod": 80
         },
 
         "MotionDriverController": {

src/drivers/motion/MotionDriver.js

@@ -18,6 +18,7 @@ const TAG = 'MotionDriver';
 class MotionDriver extends EventEmitter  {
     static get DIRECTION_FORWARD() { return 1; }
     static get DIRECTION_BACKWARD() { return -1; }
+
     static get STATE_IDLE() { return 1; }
     static get STATE_STUCK() { return 2; }
     static get STATE_MOVING() { return 3; }
@@ -39,11 +40,10 @@ class MotionDriver extends EventEmitter  {
             startOrientation: 0,
             startSpeed: 100,
             refreshDataPeriod: 100,
-            connectionTimeout: 500
+            connectionTimeout: 4000
         }, config);
 
         this.positon = new Point(config.startX, config.startY);
-        this.direction = MotionDriver.DIRECTION_FORWARD;
         this.state = MotionDriver.STATE_UNDEFINED;
         this.stateChar = 'U';
         this.orientation = config.startOrientation;
@@ -55,21 +55,26 @@ class MotionDriver extends EventEmitter  {
     init(finishedCallback) {
         let motionDriver = this;
 
+        this.reset();
         this.setPositionAndOrientation(
             this.config.startX,
             this.config.startY,
             this.config.startOrientation
         );
+        this.setRefreshInterval(this.config.refreshDataPeriod);
         this.requestRefreshData();
 
-
+        let driverChecker = setInterval(() => {
+            if (motionDriver.getState() !== MotionDriver.STATE_UNDEFINED) {
+                clearInterval(driverChecker);
+                Mep.Log.info(TAG, 'Communication is validated');
+                finishedCallback();
+            }
+        }, 100);
         setTimeout(() => {
             if (motionDriver.getState() === MotionDriver.STATE_UNDEFINED) {
                 throw Error(TAG, 'No response from motion driver');
-            } else {
-                Mep.Log.info(TAG, 'Communication is validated');
             }
-            finishedCallback();
         }, this.config.connectionTimeout);
     }
 
@@ -78,11 +83,15 @@ class MotionDriver extends EventEmitter  {
         Mep.Log.debug(TAG, 'Finish command sent');
     }
 
+    reset(callback) {
+        this.communicator.send(Buffer.from(['R'.charCodeAt(0)]), callback);
+    }
+
     /**
      * Request state, position and orientation from motion driver
      */
-    requestRefreshData() {
-        this.communicator.send(Buffer.from(['P'.charCodeAt(0)]));
+    requestRefreshData(callback) {
+        this.communicator.send(Buffer.from(['P'.charCodeAt(0)]), callback);
     }
 
     /**
@@ -91,7 +100,7 @@ class MotionDriver extends EventEmitter  {
      * @param y {Number} - New Y coordinate relative to start position of the robot
      * @param orientation {Number} - New robot's orientation
      */
-    setPositionAndOrientation(x, y, orientation) {
+    setPositionAndOrientation(x, y, orientation, callback) {
         let data = Buffer.from([
             'I'.charCodeAt(0),
             x >> 8,
@@ -102,42 +111,60 @@ class MotionDriver extends EventEmitter  {
             orientation & 0xFF
         ]);
 
-        this.communicator.send(data);
+        this.communicator.send(data, callback);
     }
 
-    rotateTo(angle) {
+    rotateTo(angle, callback) {
         let data = Buffer.from([
             'A'.charCodeAt(0),
             angle >> 8,
             angle & 0xFF
         ]);
-        this.communicator.send(data);
+        this.communicator.send(data, callback);
+    }
+
+    goForward(millimeters, callback) {
+        let data = Buffer.from([
+            'D'.charCodeAt(0),
+            millimeters >> 8,
+            millimeters & 0xFF,
+            0
+        ]);
+        this.communicator.send(data, callback);
     }
 
     /**
      * Stop the robot.
      */
-    stop() {
-        this.communicator.send('S');
+    stop(callback) {
+        this.communicator.send(Buffer.from(['S'.charCodeAt(0)]), callback);
     }
 
     /**
      * Stop robot by turning off motors.
      */
-    softStop() {
-        this.communicator.send('s');
+    softStop(callback) {
+        this.communicator.send(Buffer.from(['s'.charCodeAt(0)]), callback);
+    }
+
+    setRefreshInterval(interval) {
+        this.communicator.send(Buffer.from([
+            'p'.charCodeAt(0),
+            interval >> 8,
+            interval & 0xff,
+        ]));
     }
 
 
     /**
      * Set default speed of the robot
      * @param speed {Number} - Speed (0 - 255)
      */
-    setSpeed(speed) {
+    setSpeed(speed, callback) {
         this.communicator.send(Buffer.from([
             'V'.charCodeAt(0),
             speed
-        ]));
+        ]), callback);
     }
 
     /**
@@ -146,7 +173,7 @@ class MotionDriver extends EventEmitter  {
      * @param positionY {Number} - Y coordinate relative to start position of the robot
      * @param direction {Number} - Direction, can be MotionDriver.DIRECTION_FORWARD or MotionDriver.DIRECTION_BACKWARD
      */
-    moveToPosition(x, y, direction) {
+    moveToPosition(x, y, direction, callback) {
         this.communicator.send(Buffer.from([
             'G'.charCodeAt(0),
             x >> 8,
@@ -155,18 +182,18 @@ class MotionDriver extends EventEmitter  {
             y & 0xff,
             0,
             direction
-        ]));
+        ]), callback);
     }
 
-    moveToCurvilinear(x, y, direction) {
+    moveToCurvilinear(x, y, direction, callback) {
         this.communicator.send(Buffer.from([
             'N'.charCodeAt(0),
             x >> 8,
             x & 0xff,
             y >> 8,
             y & 0xff,
             direction
-        ]));
+        ]), callback);
     }
 
 
@@ -185,8 +212,6 @@ class MotionDriver extends EventEmitter  {
     }
 
     _onDataReceived(buffer, type) {
-        if (buffer.length < 9) return;
-
         // Ignore garbage
         let stateChar = String.fromCharCode(buffer.readInt8(0));
         let position = new Point(
@@ -244,7 +269,7 @@ class MotionDriver extends EventEmitter  {
         }
 
         // Read data again
-        setInterval(this.requestRefreshData.bind(this), this.config.refreshDataPeriod);
+        // setInterval(this.requestRefreshData.bind(this), this.config.refreshDataPeriod);
     }
 
     /**
@@ -255,10 +280,6 @@ class MotionDriver extends EventEmitter  {
         return this.positon;
     }
 
-    getDirection() {
-        return this.direction;
-    }
-
     getState() {
         return this.state;
     }

src/drivers/uart/Uart.js

@@ -66,8 +66,6 @@ class Uart extends EventEmitter {
     _onDataReceived(chunkBuffer) {
         this.in.pause();
 
-        //console.log(chunkBuffer);
-
         if (this.protocol === null) {
             this.emit('data', chunkBuffer);
         } else {
@@ -84,15 +82,23 @@ class Uart extends EventEmitter {
      * @param type {Number} - Type of packet, will be ignored if protocol doesn't support
      */
     send(buffer, callback, type) {
-        let uart = this;
-
         if (buffer.length === 0) {
-            callback('Buffer length cannot be 0');
+            Mep.Log.error('Buffer length cannot be 0');
             return;
         }
 
+        let packetizedBuffer;
+        if (type === undefined && this.protocol !== null) {
+            packetizedBuffer = this.protocol.generate(buffer.slice(1), buffer.readUInt8(0));
+        }
+        else if (type !== undefined && this.protocol !== null) {
+            packetizedBuffer = this.protocol.generate(buffer, type);
+        } else {
+            packetizedBuffer = buffer;
+        }
+
         this.out.write(
-            (this.protocol === null) ? buffer : this.protocol.generate(buffer, type),
+            packetizedBuffer,
             null,
             callback
         );

src/services/position/PositionService.js

@@ -16,9 +16,15 @@ const TAG = 'PositionService';
  * @author Darko Lukic <lukicdarkoo@gmail.com>
  */
 class PositionService extends EventEmitter {
+    get DIRECTION_FORWARD() { return 1; }
+    get DIRECTION_BACKWARD() { return -1; }
+    get DIRECTION_NONE() { return 0; }
+
     init(config) {
         this.config = config;
         this.currentSpeed = 0;
+        this.direction = this.DIRECTION_NONE;
+
         this.positionEstimator = new PositionEstimator();
         this.motionDriver = Mep.DriverManager.getDriver('MotionDriver');
 
@@ -28,6 +34,8 @@ class PositionService extends EventEmitter {
         };
         this.pathObstacleSources = [[], []];
 
+        this._goToNextQueuedPoint = this._goToNextQueuedPoint.bind(this);
+
         // Subscribe on sensors that can provide obstacles on the robot's terrain
         Mep.DriverManager.callMethodByGroup('terrain', 'on', ['pathObstacleDetected', this._onPathObstacleDetected.bind(this)]);
     }
@@ -48,6 +56,10 @@ class PositionService extends EventEmitter {
         return this.positionEstimator.getOrientation();
     }
 
+    getDirection() {
+        return this.motionDriver.getDirection();
+    }
+
     _onPathObstacleDetected(source, relativePOI, detected, front) {
         // Add source or remove source
         // `pathObstacleSources[0]` is array of obstacle source detected on a back of robot
@@ -139,7 +151,6 @@ class PositionService extends EventEmitter {
     }
 
     _goToNextQueuedPoint(resolve, reject) {
-        let positionService = this;
         let point;
         if (this.required.points.length > 0) {
             point = this.required.points[0];
@@ -150,13 +161,14 @@ class PositionService extends EventEmitter {
                 this.required.params.tolerance,
                 this.required.params.speed
             ).then(() => {
-                this._goToNextQueuedPoint.bind(positionService);
                 this._goToNextQueuedPoint(resolve, reject);
+            }).catch((e) => {
+                reject(e);
             });
             return;
-        } else {
-            resolve();
         }
+
+        resolve();
     }
 
     _promiseToReachDestination(point, tolerance) {
@@ -255,6 +267,11 @@ class PositionService extends EventEmitter {
 
     }
 
+    straight(millimeters) {
+        this.motionDriver.goForward(millimeters);
+        return this._promiseToReachDestination(null, -1);
+    }
+
     /**
      * Rotate robot for an angle
      * @param tunedAngle {TunedAngle} - Angle to rotate
@@ -263,7 +280,7 @@ class PositionService extends EventEmitter {
     rotate(tunedAngle, options) {
         this.motionDriver.rotateTo(tunedAngle.getAngle());
 
-        return this._promiseToReachDestination(new Point(0, 0), 0);
+        return this._promiseToReachDestination(null, -1);
     }
 }
 

src/strategy/Scheduler.js

@@ -54,6 +54,8 @@ class Scheduler {
         let nextTask = schedulerService.recommendNextTask(this.tasks);
         if (nextTask !== null) {
             this.runTask(nextTask);
+        } else {
+            Mep.Log.info(TAG, 'No more tasks');
         }
     }
 

src/strategy/Task.js

@@ -58,16 +58,22 @@ class Task {
      * Finish this task and run next one
      */
     finish() {
+        let lastState = this.state;
         this.state = Task.FINISHED;
-        this.scheduler.runNextTask();
+        if (lastState !== Task.FINISHED) {
+            this.scheduler.runNextTask();
+        }
     }
 
     /**
      * Suspend this task and run next one
      */
     suspend() {
+        let lastState = this.state;
         this.state = Task.SUSPENDED;
-        this.scheduler.runNextTask();
+        if (lastState !== Task.SUSPENDED) {
+            this.scheduler.runNextTask();
+        }
     }
 
     /**