Milestone. Working movement, multiple movement and path finding

memristor · Nov 29, 2016 · 6ca3d99 · 6ca3d99
1 parent d76d9bb
commit 6ca3d99
Show file tree

Hide file tree

Showing 8 changed files with 110 additions and 16 deletions.
diff --git a/package.json b/package.json
@@ -23,6 +23,7 @@
     "bunyan": "^1.8.1",
     "bunyan-elasticsearch": "^1.0.0",
     "bunyan-format": "^0.2.1",
+    "es6-mixin": "^0.3.0",
     "latodoc": "github:smeijer/latodoc",
     "nan": "^2.3.5",
     "nconf": "^0.8.4",
@@ -31,6 +32,7 @@
     "require-new": "^1.1.0",
     "should": "^11.1.1",
     "sinon": "^1.17.6",
-    "ws": "^1.1.1"
+    "ws": "^1.1.1",
+    "xmultiple": "^1.0.0"
   }
 }
diff --git a/src/config/default.json b/src/config/default.json
@@ -10,9 +10,9 @@
         "MotionDriver": {
             "class": "drivers/motion/MotionDriver",
             "load": true,
-            "startX": 0,
+            "startX": -1300,
             "startY": 0,
-            "startOrientation": 90,
+            "startOrientation": 0,
             "startSpeed": 100,
             "refreshDataPeriod": 100,
             "precision": 2

diff --git a/src/drivers/motion/MotionDriver.js b/src/drivers/motion/MotionDriver.js
@@ -2,14 +2,20 @@ const MotionDriverBinder = require('bindings')('motion').MotionDriverBinder;
 const Point = Mep.require('types/Point');
 const PositionDriver = Mep.require('types/PositionDriver');
 const Constants = require('./Constants');
+const Util = require('util');
+const EventEmitter = require('events');
+//const mixin = require('es6-mixin').mixin;
+
+Util.inherits(MotionDriverBinder, EventEmitter);
+
 
 /**
  * Driver enables communication with Memristor's motion driver.
  *
  * @author Darko Lukic <lukicdarkoo@gmail.com>
  * @fires MotionDriver#positionChanged
  */
-class MotionDriver extends classes(MotionDriverBinder, PositionDriver) {
+class MotionDriver extends MotionDriverBinder  {
     /**
      * Read data from motion driver (as the electronic component)
      * @method refreshData
@@ -51,17 +57,18 @@ class MotionDriver extends classes(MotionDriverBinder, PositionDriver) {
      * @param config {Object} - Configuration presented as an associative array
      */
     constructor(name, config) {
-        super([
+        super(
             true,
             config.startX,
             config.startY,
             config.startOrientation,
             config.startSpeed
-        ], []);
+        );
+
         this.name = name;
         this.config = config;
 
-        this.positon = new Point(0, 0);
+        this.positon = new Point(config.startX, config.startY);
         this.direction = Constants.DIRECTION_FORWARD;
         this.state = Constants.STATE_IDLE;
 
@@ -73,6 +80,7 @@ class MotionDriver extends classes(MotionDriverBinder, PositionDriver) {
         let motionDriver = this;
 
         this.refreshData(() => {
+
             let data = motionDriver.getData();
 
             // If position is changed fire an event
@@ -90,7 +98,7 @@ class MotionDriver extends classes(MotionDriverBinder, PositionDriver) {
                  */
                 motionDriver.emit('positionChanged',
                     motionDriver.name,
-                    motionDriver.getPosition(),
+                    motionDriver.positon,
                     motionDriver.config.precision
                 );
             }
@@ -132,6 +140,10 @@ class MotionDriver extends classes(MotionDriverBinder, PositionDriver) {
     getState() {
         return this.state;
     }
+
+	getGroups() {
+		return ['position'];
+	}
 }
 
 module.exports = MotionDriver;
diff --git a/src/drivers/motion/lib/MotionDriverBinder.cpp b/src/drivers/motion/lib/MotionDriverBinder.cpp
@@ -40,7 +40,9 @@ void MotionDriverBinder::Init(Local<Object> exports) {
 	Nan::SetPrototypeMethod(tmpl, "getPosition", getPosition);
 	Nan::SetPrototypeMethod(tmpl, "getState", getPosition);
 	Nan::SetPrototypeMethod(tmpl, "refreshData", refreshData);
-	Nan::SetPrototypeMethod(tmpl, "getData", refreshData);
+	Nan::SetPrototypeMethod(tmpl, "getData", getData);
+	Nan::SetPrototypeMethod(tmpl, "moveArc", moveArc);
+	Nan::SetPrototypeMethod(tmpl, "rotateTo", rotateTo);
 
     exports->Set(Nan::New("MotionDriverBinder").ToLocalChecked(), tmpl->GetFunction());
 }
@@ -216,6 +218,44 @@ void MotionDriverBinder::getState(const Nan::FunctionCallbackInfo<Value> &args)
     args.GetReturnValue().Set(state);
 }
 
+void MotionDriverBinder::rotateTo(const Nan::FunctionCallbackInfo<Value> &args) {
+	Nan::HandleScope scope;
+
+	if (args.Length() != 1 ||
+		args[0]->IsInt32() == false) {
+
+			args.GetIsolate()->ThrowException(Exception::TypeError(
+		    Nan::New("Please check arguments. Expected arguments: (uint32 speed)").ToLocalChecked()
+		));
+	}
+
+	MotionDriver *motionDriver = ObjectWrap::Unwrap<MotionDriverBinder>(args.Holder())->getMotionDriver();
+	motionDriver->rotateTo(args[0]->Int32Value());
+}
+
+void MotionDriverBinder::moveArc(const Nan::FunctionCallbackInfo<Value> &args) {
+	Nan::HandleScope scope;
+
+	if (args.Length() != 4 ||
+		args[0]->IsInt32() == false ||
+		args[1]->IsInt32() == false ||
+		args[2]->IsInt32() == false ||
+		args[3]->IsInt32() == false) {
+
+		args.GetIsolate()->ThrowException(Exception::TypeError(
+		    Nan::New("Please check arguments").ToLocalChecked()
+		));
+    	}
+
+    	MotionDriver *motionDriver = ObjectWrap::Unwrap<MotionDriverBinder>(args.Holder())->getMotionDriver();
+
+    	Point2D center(args[0]->Int32Value(), args[1]->Int32Value());
+	int angle = args[2]->Int32Value();
+    	MotionDriver::MovingDirection direction = (args[3]->Int32Value() == 1) ? MotionDriver::FORWARD : MotionDriver::BACKWARD;
+
+    	motionDriver->moveArc(center, angle, direction);
+}
+
 void MotionDriverBinder::refreshData(const Nan::FunctionCallbackInfo<Value> &args) {
     Nan::HandleScope scope;
 

diff --git a/src/drivers/motion/lib/MotionDriverBinder.h b/src/drivers/motion/lib/MotionDriverBinder.h
@@ -41,6 +41,8 @@ class MotionDriverBinder : public Nan::ObjectWrap {
 	static void refreshData(const Nan::FunctionCallbackInfo<Value> &args);
 	static void getState(const Nan::FunctionCallbackInfo<Value> &args);
 	static void getData(const Nan::FunctionCallbackInfo<Value> &args);
+	static void rotateTo(const Nan::FunctionCallbackInfo<Value> &args);
+	static void moveArc(const Nan::FunctionCallbackInfo<Value> &args);
 
     MotionDriver *motionDriver;
 };

diff --git a/src/services/position/PositionService.js b/src/services/position/PositionService.js
@@ -68,7 +68,7 @@ class PositionService {
         direction = this.config.moveOptions.direction,
         relative = this.config.moveOptions.relative,
         tolerance = this.config.moveOptions.tolerance,
-        speed = this.config.moveOptions.speed
+        speed = undefined
     } = {}) {
         let positionService = this;
         let destinationPoint = tunedPoint.getPoint();
@@ -82,7 +82,9 @@ class PositionService {
 
         // Apply path finding
         if (pathfinding === true) {
+
             let currentPoint = this.positionEstimator.getPosition();
+		Mep.Log.debug(TAG, 'Start path finding from position', currentPoint);
 
             points = Mep.getPathService().search(currentPoint, destinationPoint);
             Mep.Log.debug(TAG, 'Start path finding', points, 'from point', currentPoint);
@@ -108,7 +110,7 @@ class PositionService {
 
     _basicSet(point, direction, tolerance, speed) {
         // Set speed
-        if (this.currentSpeed !== speed) {
+        if (speed !== undefined && this.currentSpeed !== speed) {
             this.currentSpeed = speed;
             this.motionDriver.setSpeed(speed);
         }
@@ -138,6 +140,22 @@ class PositionService {
         });
     }
 
+	arc(point, angle, direction) {
+		this.motionDriver.moveArc(point.getX(), point.getY(), angle, direction);
+
+		return new Promise((resolve, reject) => {
+		    this.motionDriver.on('stateChanged', (state) => {
+		        if (state === MotionDriverConstants.STATE_IDLE) {
+		            resolve();
+		        }
+		        else if (state === MotionDriverConstants.STATE_ERROR ||
+		            state === MotionDriverConstants.STATE_STUCK) {
+		            reject(state);
+		        }
+		    });
+		});
+	}
+
     rotate(tunedAngle, options) {
         // TODO
     }

diff --git a/src/types/PositionDriver.js b/src/types/PositionDriver.js
@@ -12,4 +12,4 @@ class PositionDriver extends BaseDriver {
     }
 }
 
-module.exports = PositionDriver;
+module.exports = PositionDriver;
diff --git a/strategies/default/InitTask.js b/strategies/default/InitTask.js
@@ -1,5 +1,6 @@
 const Task = Mep.require('types/Task');
 const TunedPoint = Mep.require('types/TunedPoint');
+const Point = Mep.require('types/Point');
 const Delay = Mep.require('utils/Delay');
 const position = Mep.getPositionService();
 
@@ -8,11 +9,30 @@ class InitTask extends Task {
         // Wait WebSocketClient to connect to WebSocketServer
         await Delay(200);
 
+	await position.set(new TunedPoint(1100, 0), { speed: 130 });
+        await position.set(new TunedPoint(-1300, 0));
+
+
+	if (false) {
+		await position.set(new TunedPoint(-1100, 0), { speed: 40 });
+		await position.arc(new Point(-1300, 100), 30, 1);
+		await position.arc(new Point(-1300, 100), 60, 1);
+	}
+
+	if (false) {
+		await position.set(new TunedPoint(-1200, 0), { speed: 40 });
+		await position.set(new TunedPoint(-735, -850), { pathfinding: true });
+		await position.set(new TunedPoint(765, -850), { pathfinding: true });
+		await position.set(new TunedPoint(-1200, 0), { pathfinding: true });
+	}
+
+	//await position.set(new TunedPoint(-1300, 0));
+
         // Let's move around
-        await position.set(new TunedPoint(-735, -850), { pathfinding: true });
-        await position.set(new TunedPoint(765, -850), { pathfinding: true });
-        await position.set(new TunedPoint(-1200, 400), { pathfinding: true });
-        await position.set(new TunedPoint(-170, 820), { pathfinding: true });
+        //await position.set(new TunedPoint(-735, -850), { pathfinding: true });
+        //await position.set(new TunedPoint(765, -850), { pathfinding: true });
+        //await position.set(new TunedPoint(-1200, 400), { pathfinding: true });
+        //await position.set(new TunedPoint(-170, 820), { pathfinding: true });
     }
 
 }
-Original file line number
+Diff line change
@@ Expand Up / @@ -12,4 +12,4 @@ class PositionDriver extends BaseDriver { @@
         }
     }
-    module.exports = PositionDriver;
+    module.exports = PositionDriver;