diff --git a/examples/mission.js b/examples/mission.js new file mode 100644 index 0000000..ef079ef --- /dev/null +++ b/examples/mission.js @@ -0,0 +1,27 @@ +var df = require('dateformat') + , autonomy = require('../') + , mission = autonomy.createMission() + ; + +mission.log("mission-" + df(new Date(), "yyyy-mm-dd_hh-MM-ss") + ".txt"); + +mission.takeoff() + .hover(1000) + .forward(1) + .right(1) + .backward(1) + .left(1) + .hover(1000) + .land(); + +mission.run(function (err, result) { + if (err) { + console.trace("Oops, something bad happened: %s", err.message); + mission.client().stop(); + mission.client().land(); + } else { + console.log("We are done!"); + process.exit(0); + } +}); + diff --git a/index.js b/index.js index dbf812a..53829eb 100644 --- a/index.js +++ b/index.js @@ -1,12 +1,20 @@ var autonomy = exports; +var ardrone = require('ar-drone'); -exports.StateEstimator = require('./lib/StateEstimator'); exports.EKF = require('./lib/EKF'); exports.Camera = require('./lib/Camera'); exports.Controller = require('./lib/Controller'); +exports.Mission = require('./lib/Mission'); -exports.estimateState = function(client, options) { - var estimator = new autonomy.StateEstimator(client, options); - return estimator; +exports.control = function(client, options) { + return new autonomy.Controller(client, options); +} + +exports.createMission = function(options) { + var client = ardrone.createClient(options); + var control = new autonomy.Controller(client, options); + var mission = new autonomy.Mission(client, control, options); + + return mission; } diff --git a/lib/Controller.js b/lib/Controller.js index c839888..6f5ad78 100644 --- a/lib/Controller.js +++ b/lib/Controller.js @@ -174,6 +174,37 @@ Controller.prototype.ccw = function(angle, callback) { return this.cw(-angle, callback); } +/* + * Climb ups by the given distance (in meters). + */ +Controller.prototype.up = function(distance, callback) { + var state = this.state(); + return this.go({x: state.x, y: state.y, z: state.z + distance, yaw: state.yaw}, callback); +} + +/* + * Lower itself by the given distance (in meters). + */ +Controller.prototype.down = function(distance, callback) { + return this.up(-distance, callback); +} + +/* + * Go to the target altitude + */ +Controller.prototype.altitude = function(altitude, callback) { + var state = this.state(); + return this.go({x: state.x, y: state.y, z: altitude, yaw: state.yaw}, callback); +} + +/* + * Go to the target yaw + */ +Controller.prototype.yaw = function(yaw, callback) { + var state = this.state(); + return this.go({x: state.x, y: state.y, z: state.z, yaw: yaw}, callback); +} + /* * Sets a new goal and enable the controller. When the goal * is reached, the callback is called with the current state. @@ -193,6 +224,11 @@ Controller.prototype.go = function(goal, callback) { goal.yaw = Math.atan2(Math.sin(yaw),Math.cos(yaw)); } + // Make sure we don't attempt to go too low + if (goal.z != undefined) { + goal.z = Math.max(goal.z, 0.5); + } + // Update our goal this._goal = goal; this._goal.reached = false; diff --git a/lib/Mission.js b/lib/Mission.js new file mode 100644 index 0000000..b340ee7 --- /dev/null +++ b/lib/Mission.js @@ -0,0 +1,201 @@ +var async = require('async') + , fs = require('fs') + ; + +module.exports = Mission; +function Mission(client, controller, options) { + + options = options || {}; + + this._options = options; + this._client = client; + this._control = controller; + + this._steps = []; +} + +Mission.prototype.client = function() { + return this._client; +} + +Mission.prototype.run = function(callback) { + async.waterfall(this._steps, callback); +} + +Mission.prototype.log = function(path) { + var dataStream = fs.createWriteStream(path); + this._control.on('controlData', function(d) { + dataStream.write(d.state.x + "," + + d.state.y + "," + + d.state.z + "," + + d.state.yaw + "," + + d.state.vx + "," + + d.state.vy + "," + + d.goal.x + "," + + d.goal.y + "," + + d.goal.z + "," + + d.goal.yaw + "," + + d.error.ex + "," + + d.error.ey + "," + + d.error.ez + "," + + d.error.eyaw + "," + + d.control.ux + "," + + d.control.uy + "," + + d.control.uz + "," + + d.control.uyaw + "," + + d.last_ok + "," + + d.tag + "\n"); + }); +} + +Mission.prototype.takeoff = function() { + var self = this; + this._steps.push(function(cb) { + self._client.takeoff(cb); + }); + + return this; +} + +Mission.prototype.land = function() { + var self = this; + this._steps.push(function(cb) { + self._client.land(cb); + }); + + return this; +} + +Mission.prototype.hover = function(delay) { + var self = this; + this._steps.push(function(cb) { + self._control.hover(); + setTimeout(cb, delay); + }); + + return this; +} + +Mission.prototype.wait = function(delay) { + this._steps.push(function(cb) { + setTimeout(cb, delay); + }); + + return this; +} + +Mission.prototype.task = function(task) { + this._steps.push(function(cb) { + task(cb); + }); +} + +Mission.prototype.taskSync = function(task) { + this._steps.push(function(cb) { + task(); + cb(); + }); +} + +Mission.prototype.go = function(goal) { + var self = this; + this._steps.push(function(cb) { + self._control.go(goal, cb); + }); + + return this; +} + +Mission.prototype.forward = function(distance) { + var self = this; + this._steps.push(function(cb) { + self._control.forward(distance, cb); + }); + + return this; +} + +Mission.prototype.backward = function(distance) { + var self = this; + this._steps.push(function(cb) { + self._control.backward(distance, cb); + }); + + return this; +} + +Mission.prototype.left = function(distance) { + var self = this; + this._steps.push(function(cb) { + self._control.left(distance, cb); + }); + + return this; +} + +Mission.prototype.right = function(distance) { + var self = this; + this._steps.push(function(cb) { + self._control.right(distance, cb); + }); + + return this; +} + +Mission.prototype.up = function(distance) { + var self = this; + this._steps.push(function(cb) { + self._control.up(distance, cb); + }); + + return this; +} + +Mission.prototype.down = function(distance) { + var self = this; + this._steps.push(function(cb) { + self._control.down(distance, cb); + }); + + return this; +} + +Mission.prototype.cw = function(angle) { + var self = this; + this._steps.push(function(cb) { + self._control.cw(angle, cb); + }); + + return this; +} + +Mission.prototype.ccw = function(angle) { + var self = this; + this._steps.push(function(cb) { + self._control.ccw(angle, cb); + }); + + return this; +} + +Mission.prototype.altitude = function(altitude) { + var self = this; + this._steps.push(function(cb) { + self._control.altitude(altitude, cb); + }); + + return this; +} + +Mission.prototype.yaw = function(angle) { + var self = this; + this._steps.push(function(cb) { + self._control.yaw(angle,cb); + }); + + return this; +} + + + + diff --git a/tests/integration/mission.js b/tests/integration/mission.js new file mode 100644 index 0000000..8d4385d --- /dev/null +++ b/tests/integration/mission.js @@ -0,0 +1,32 @@ +var df = require('dateformat') + , common = require('../common') + , autonomy = require(common.root) + , mockClient = require('./mock/client') + ; + +var client = new mockClient(); +var controller = new autonomy.Controller(client); +var mission = new autonomy.Mission(client, controller); + +mission.log("mission-" + df(new Date(), "yyyy-mm-dd_hh-MM-ss") + ".txt"); + +mission.takeoff() + .hover(1000) + .forward(1) + .right(1) + .backward(1) + .left(1) + .hover(1000) + .land(); + +mission.run(function (err, result) { + if (err) { + console.trace("Oops, something bad happened: %s", err.message); + mission.client().stop(); + mission.client().land(); + } else { + console.log("We are done!"); + process.exit(0); + } +}); + diff --git a/tests/integration/mock/client.js b/tests/integration/mock/client.js index 3872f5a..d922c7f 100644 --- a/tests/integration/mock/client.js +++ b/tests/integration/mock/client.js @@ -20,6 +20,14 @@ function Client(options) { }, DT); } +Client.prototype.takeoff = function(callback) { + setTimeout(callback, 1000); +} + +Client.prototype.land = function(callback) { + setTimeout(callback, 1000); +} + Client.prototype.front = function(speed) { this._speed.vx = speed; } diff --git a/tests/integration/simple.js b/tests/integration/simple.js deleted file mode 100644 index 9c6bc92..0000000 --- a/tests/integration/simple.js +++ /dev/null @@ -1,18 +0,0 @@ -// TODO This is broken. Need to write real tests. -console.log("The test is broken. Will have to write proper tests :-)"); - -var autonomy = require('..'); -var client = require('./mock/client'); - -var controller = new autonomy.Controller(new client(), {state: {x: 0, y:0, z:1, yaw: 0}}); - -console.log("State: %j", controller.state()); - -controller.on('controlData', function(data) { - console.log("%j", data); -}); - -controller.go({x: 1, y: 1}, function(state) { - console.log("Reached state %j", state); - controller.disable(); -}); diff --git a/tests/unit/test-Controller.js b/tests/unit/test-Controller.js index b1021ed..8191478 100644 --- a/tests/unit/test-Controller.js +++ b/tests/unit/test-Controller.js @@ -115,5 +115,57 @@ test('Controller', { ctrl.zero(); assert(ctrl._ekf.reset.calledOnce); + }, + + 'up': function() { + var ctrl = new autonomy.Controller(this.mockClient); + var state = {x: 1, y: 2, z: 3, yaw: 0}; + + ctrl._state = state; + ctrl.up(1); + var goal = ctrl._goal; + + assert.equal(goal.x, state.x); + assert.equal(goal.y, state.y); + assert.equal(goal.z, state.z + 1); + assert.equal(goal.yaw, state.yaw); + }, + + 'down is invserse of up': function() { + var ctrl = new autonomy.Controller(this.mockClient); + var cb = function() {}; + ctrl._state = {x: 0, y: 0, z: 1, yaw: 0}; + sinon.spy(ctrl, 'up'); + + ctrl.down(1, cb); + assert(ctrl.up.calledWith(-1, cb)); + }, + + 'cannot go too low': function() { + var ctrl = new autonomy.Controller(this.mockClient); + var state = {x: 0, y: 0, z: 1, yaw: 0}; + + ctrl._state = state; + ctrl.down(1); + var goal = ctrl._goal; + + assert.equal(goal.x, state.x); + assert.equal(goal.y, state.y); + assert.equal(goal.z, 0.5); + assert.equal(goal.yaw, state.yaw); + }, + + 'altitude': function() { + var ctrl = new autonomy.Controller(this.mockClient); + var state = {x: 0, y: 0, z: 1, yaw: 0}; + + ctrl._state = state; + ctrl.altitude(3); + var goal = ctrl._goal; + + assert.equal(goal.x, state.x); + assert.equal(goal.y, state.y); + assert.equal(goal.z, 3); + assert.equal(goal.yaw, state.yaw); } });