diff --git a/examples/hover.js b/examples/hover.js new file mode 100644 index 0000000..b29e969 --- /dev/null +++ b/examples/hover.js @@ -0,0 +1,86 @@ +var fs = require('fs') + , async = require('async') + , path = require('path') + , df = require('dateformat') + , arDrone = require('ar-drone') + , arDroneConstants = require('ar-drone/lib/constants') + , autonomy = require('..'); + +var client = arDrone.createClient(); +var ctrl = new autonomy.Controller(client, {debug: false}); + +// Configure the client for tag detection +function navdata_option_mask(c) { + return 1 << c; +} + +var navdata_options = ( + navdata_option_mask(arDroneConstants.options.DEMO) + | navdata_option_mask(arDroneConstants.options.VISION_DETECT) + | navdata_option_mask(arDroneConstants.options.MAGNETO) + | navdata_option_mask(arDroneConstants.options.WIFI) +); + +// Connect and configure the drone +client.config('general:navdata_demo', true); +client.config('general:navdata_options', navdata_options); +client.config('video:video_channel', 1); +client.config('detect:detect_type', 12); + +// Log control data for debugging +var folder = df(new Date(), "yyyy-mm-dd_hh-MM-ss"); +fs.mkdir(path.join('/tmp', folder), function() { + dataStream = fs.createWriteStream(path.join('/tmp', folder, 'data.txt')); +}); + +ctrl.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"); +}); + +// Let's move ! +async.waterfall([ + function(cb){ + console.log("Waiting for takeoff"); + client.takeoff(cb); + }, + function(cb){ + console.log("Going to base position"); + ctrl._ekf.reset(); + ctrl.go({x: 0, y: 0, z: 1}, cb); + }, + function(cb){ + setTimeout(cb, 5000); + }, + function(cb) { + console.log("Landing..."); + ctrl.disable(); + client.land(); + } +], function (err, result) { + if (err) { + console.log("Oops, something bad happened: %s", err); + client.stop(); + client.land(); + } else { + console.log("We are done!"); + } +}) diff --git a/lib/Controller.js b/lib/Controller.js index 876d83b..439aed0 100644 --- a/lib/Controller.js +++ b/lib/Controller.js @@ -23,10 +23,10 @@ function Controller(client, options) { this._tag = options.tag || {x: 0, y: 0, yaw: 0}; // Configure the four PID required to control the drone - this._pid_x = new PID(0.3, 0, 0.1); - this._pid_y = new PID(0.3, 0, 0.1); - this._pid_z = new PID(0.5, 0, 0.1); - this._pid_yaw = new PID(1.0, 0, 0.3); + this._pid_x = new PID(0.5, 0, 0.15); + this._pid_y = new PID(0.5, 0, 0.15); + this._pid_z = new PID(0.5, 0, 0.15); + this._pid_yaw = new PID(1.0, 0, 0.30); // kalman filter is used for the drone state estimation this._ekf = new EKF(options);