Added higher level functions to controller

lib/Controller.js

@@ -96,7 +96,72 @@ Controller.prototype.state = function() {
  * Sets the goal to the current state and attempt to hover on top.
  */
 Controller.prototype.hover = function() {
-    this.go({x: this._state.x, y: this._state.y});
+    this.go({x: this._state.x, y: this._state.y, z: this._state.z, yaw: this._state.yaw});
+}
+
+/*
+ * Move forward (direction faced by the front camera) by the given
+ * distance (in meters).
+ */
+Controller.prototype.forward = function(distance, callback) {
+   // Our starting position
+   var state = this.state();
+
+   // Remap our target position in the world coordinates
+   var gx = state.x + Math.cos(state.yaw) * distance;
+   var gy = state.y + Math.sin(state.yaw) * distance;
+
+   // Assign the new goal
+   this.go({x: gx, y: gy, z: state.z, yaw: state.yaw}, callback);
+}
+
+/*
+ * Move backward by the given distance (in meters).
+ */
+Controller.prototype.backward = function(distance, callback) {
+    return this.forward(-distance, callback);
+}
+
+/*
+ * Move right (front being the direction faced by the front camera) by the given
+ * distance (in meters).
+ */
+Controller.prototype.right = function(distance, callback) {
+   // Our starting position
+   var state = this.state();
+
+   // Remap our target position in the world coordinates
+   var gx = state.x - Math.sin(state.yaw) * distance;
+   var gy = state.y + Math.cos(state.yaw) * distance;
+
+   // Assign the new goal
+   this.go({x: gx, y: gy, z: state.z, yaw: state.yaw}, callback);
+}
+
+/*
+ * Move left by the given distance (in meters).
+ */
+Controller.prototype.left = function(distance, callback) {
+    return this.right(-distance, callback);
+}
+
+/*
+ * Turn clockwise of the given angle. Note that this does not
+ * force a clockwise motion, if the angle is > 180 then the drone
+ * will turn in the other direction, taking the shortest path.
+ */
+Controller.prototype.cw = function(angle, callback) {
+    var state = this.state();
+    var yaw   = state.yaw.toDeg();
+
+    return this.go({x: state.x, y: state.y, z: state.z, yaw: yaw + angle}, callback);
+}
+
+/*
+ * Turn counter clockwise of the given angle
+ */
+Controller.prototype.ccw = function(angle, callback) {
+    return this.cw(-angle, callback);
 }
 
 /*

lib/EKF.js

@@ -129,3 +129,10 @@ if (typeof(Number.prototype.toRad) === "undefined") {
     return this * Math.PI / 180;
   }
 }
+
+/** Converts radians to numeric dregrees */
+if (typeof(Number.prototype.toDeg) === "undefined") {
+  Number.prototype.toDeg = function() {
+    return this * 180 / Math.PI;
+  }
+}

package.json

@@ -15,10 +15,18 @@
     "kalman",
     "pid"
   ],
+  "scripts": {
+    "test": "node tests/run.js"
+  },
   "dependencies": {
     "sylvester": "0.0.21",
     "async": "~0.2.9"
   },
+  "devDependencies": {
+    "utest": "0.0.6",
+    "urun": "0.0.6",
+    "sinon": "1.4.2"
+  },
   "author": "Laurent Eschenauer <laurent@eschenauer.be>",
   "license": "MIT"
 }

tests/common.js

@@ -0,0 +1,5 @@
+var common = exports;
+var path   = require('path');
+
+common.root     = path.join(__dirname, '..');
+common.lib      = path.join(common.root, 'lib');

tests/run.js

@@ -0,0 +1,2 @@
+var urun = require('urun');
+urun(__dirname);

tests/unit/test-Controller.js

@@ -0,0 +1,111 @@
+var common   = require('../common');
+var assert   = require('assert');
+var test     = require('utest');
+var sinon    = require('sinon');
+var autonomy = require(common.root);
+
+test('Controller', {
+    before: function() {
+        this.mockClient      = {};
+        this.mockClient.on   = sinon.stub();
+        this.mockClient.stop = sinon.stub();
+    },
+
+    'controller binds on navdata': function() {
+        var ctrl = new autonomy.Controller(this.mockClient);
+        assert.equal(this.mockClient.on.callCount, 1);
+    },
+
+    'disabling the controller stops the drone': function() {
+        var ctrl = new autonomy.Controller(this.mockClient);
+        ctrl.disable();
+        assert.equal(this.mockClient.stop.callCount, 1);
+        assert.equal(ctrl._enabled, false);
+    },
+
+    'hover assigns current state as goal': function() {
+        var ctrl  = new autonomy.Controller(this.mockClient);
+        var state = {x: 1, y: 2, z: 3, yaw: 0};
+
+        ctrl._state = state;
+        ctrl.hover();
+        var goal = ctrl._goal;
+
+        assert.equal(goal.x, state.x);
+        assert.equal(goal.y, state.y);
+        assert.equal(goal.z, state.z);
+        assert.equal(goal.yaw, state.yaw);
+    },
+
+    'forward mapping works with different yaw': function() {
+        var ctrl  = new autonomy.Controller(this.mockClient);
+        ctrl._state = {x: 0, y: 0, z: 1, yaw: 0};
+
+        // Test forward with yaw 0
+        ctrl.forward(1);
+        assert.equal(ctrl._goal.x, 1);
+        assert.equal(ctrl._goal.y, 0);
+
+        // Test forward with yaw 90
+        var yaw = 90;
+        ctrl._state.yaw = yaw.toRad();
+        ctrl.forward(1);
+        assert.equal(Math.round(ctrl._goal.x * 1000) / 1000, 0);
+        assert.equal(ctrl._goal.y, 1);
+
+        // Test forward with yaw 45
+        var yaw = 45;
+        ctrl._state.yaw = yaw.toRad();
+        ctrl.forward(1);
+        assert.equal(Math.round(ctrl._goal.x * 1000) / 1000, Math.round(ctrl._goal.y * 1000) /1000);
+
+        // Test forward with yaw -45
+        var yaw = -45;
+        ctrl._state.yaw = yaw.toRad();
+        ctrl.forward(1);
+        assert.equal(Math.round(ctrl._goal.x * 1000) / 1000, -Math.round(ctrl._goal.y * 1000) /1000);
+    },
+
+    'right mapping works with different yaw': function() {
+        var ctrl  = new autonomy.Controller(this.mockClient);
+        ctrl._state = {x: 0, y: 0, z: 1, yaw: 0};
+
+        // Test right with yaw 0
+        ctrl.right(1);
+        assert.equal(ctrl._goal.x, 0);
+        assert.equal(ctrl._goal.y, 1);
+
+        // Test right with yaw 90
+        var yaw = 90;
+        ctrl._state.yaw = yaw.toRad();
+        ctrl.right(1);
+        assert.equal(Math.round(ctrl._goal.x * 1000) / 1000, -1);
+        assert.equal(Math.round(ctrl._goal.y * 1000) / 1000, 0);
+
+        // Test right with yaw 45
+        var yaw = 45;
+        ctrl._state.yaw = yaw.toRad();
+        ctrl.right(1);
+        assert.equal(Math.round(ctrl._goal.x * 1000) / 1000, -Math.round(ctrl._goal.y * 1000) /1000);
+    },
+
+    'backward is the inverse of forward': function() {
+        var ctrl    = new autonomy.Controller(this.mockClient);
+        var cb      = function() {};
+        ctrl._state = {x: 0, y: 0, z: 1, yaw: 0};
+        sinon.spy(ctrl, 'forward');
+
+        ctrl.backward(1, cb);
+        assert(ctrl.forward.calledWith(-1, cb));
+    },
+
+    'left is the inverse of right': function() {
+        var ctrl    = new autonomy.Controller(this.mockClient);
+        var cb      = function() {};
+        ctrl._state = {x: 0, y: 0, z: 1, yaw: 0};
+        sinon.spy(ctrl, 'right');
+
+        ctrl.left(1, cb);
+        assert(ctrl.right.calledWith(-1, cb));
+    }
+});