wellton/ardrone-autonomy
1
0
Fork 0
Código Issues Pull Requests Ações Pacotes Projetos Versões Wiki Atividade

Iterating on controller

Ver Código-Fonte 
- Fixing some issues in PID code
- Resetting PID at each goal change
- Added two examples (not working) of autonomous fly
Esse commit está contido em:
Laurent Eschenauer
2013-06-20 22:08:51 +02:00
pai 437a777abe
commit 401d7c920a
6 arquivos alterados com 261 adições e 26 exclusões
Baixar Arquivo de Patch Baixar Arquivo de Diff Expandir todos os arquivos Colapsar todos os arquivos
examples/pano.js
+103
Ver Arquivo
@@ -0,0 +1,103 @@
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){
console.log("Going to 90");
ctrl.go({yaw: 90}, cb);
},
function(cb) {
console.log("Going to 180");
ctrl.go({yaw: 180}, cb);
},
function(cb) {
console.log("Going to 270");
ctrl.go({yaw: 270}, cb);
},
function(cb) {
console.log("Going back to 0");
ctrl.go({yaw: 0}, cb);
},
function(cb) {
console.log("Going back home");
ctrl.go({x:0, y:0}, cb);
},
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!");
}
})
examples/repl.js
+8
Ver Arquivo
@@ -39,6 +39,14 @@ fs.mkdir(path.join('/tmp', folder), function() {
dataStream = fs.createWriteStream(path.join('/tmp', folder, 'data.txt'));
});
ctrl.on('goalReached', function(state) {
console.log("Goal reached.");
});
ctrl.on('goalLeft', function(state) {
console.log("Goal left.");
});
ctrl.on('controlData', function(d) {
dataStream.write(d.state.x + "," +
d.state.y + "," +
examples/square.js
+102
Ver Arquivo
@@ -0,0 +1,102 @@
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("Waiting 1 sec");
setTimeout(cb, 1000);
},
function(cb){
console.log("Going to base position");
ctrl.go({x: 0, y: 0}, cb);
},
function(cb){
console.log("Going to 1");
ctrl.go({x: 1, y: 0}, cb);
},
function(cb) {
console.log("Going to 2");
ctrl.go({x: 1, y: 1}, cb);
},
function(cb) {
console.log("Going to 3");
ctrl.go({x: 0, y: 1}, cb);
},
function(cb) {
console.log("Going back to 0");
ctrl.go({x: 0, y: 0}, cb);
},
function(cb) {
console.log("Landing...");
ctrl.disable();
client.land();
}
], function (err, result) {
if (err) {
console.trace("Oops, something bad happened: %s", err.message);
client.stop();
client.land();
} else {
console.log("We are done!");
}
})
lib/Controller.js
+36 -17
Ver Arquivo
@@ -5,9 +5,9 @@ var PID = require('./PID');
var EKF = require('./EKF');
var Camera = require('./Camera');
EPS_LIN = 0.05; // We are ok with 5 cm precision
EPS_ANG = 0.05; // We are ok with 0.05 rad precision (2.5 deg)
STABLE_DELAY = 1000; // Time in ms to wait before declaring the drone on target
EPS_LIN = 0.05; // We are ok with 5 cm precision
EPS_ANG = 0.05; // We are ok with 0.05 rad precision (2.5 deg)
STABLE_DELAY = 200; // Time in ms to wait before declaring the drone on target
module.exports = Controller;
util.inherits(Controller, EventEmitter);
@@ -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.3, 0, 0.1);
this._pid_yaw = new PID(5.0, 0, 1.0);
this._pid_x = new PID(0.5, 0, 0.35);
this._pid_y = new PID(0.5, 0, 0.35);
this._pid_z = new PID(0.6, 0, 0.15);
this._pid_yaw = new PID(1.0, 0, 0.5);
// kalman filter is used for the drone state estimation
this._ekf = new EKF(options);
@@ -37,6 +37,9 @@ function Controller(client, options) {
// Control will only work if enabled
this._enabled = false;
// Ensure that we don't enter the processing loop twice
this._busy = false;
// The curretn target goal and an optional callback to trigger
// when goal is reached
this._goal = null;
@@ -51,8 +54,12 @@ function Controller(client, options) {
// Register the listener on navdata for our control loop
var self = this;
client.on('navdata', function(d) {
self._processNavdata(d);
self._control(d);
if (!this._busy) {
this._busy = true;
self._processNavdata(d);
self._control(d);
this._busy = false;
}
});
}
@@ -61,6 +68,10 @@ function Controller(client, options) {
* the drone (and maintain it) to the goal.
*/
Controller.prototype.enable = function() {
this._pid_x.reset();
this._pid_y.reset();
this._pid_z.reset();
this._pid_yaw.reset();
this._enabled = true;
};
@@ -85,7 +96,7 @@ 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, z: this._state.z});
this.go({x: this._state.x, y: this._state.y});
}
/*
@@ -109,6 +120,7 @@ Controller.prototype.go = function(goal, callback) {
// Update our goal
this._goal = goal;
this._goal.reached = false;
// Keep track of the callback to trigger when we reach the goal
this._callback = callback;
@@ -191,17 +203,19 @@ Controller.prototype._control = function(d) {
var now = Date.now();
if (ux == 0 && uy == 0 && uz == 0 && uyaw == 0) {
// If we have been in this position long enough we consider the target reached
if (this._last_ok != 0) {
if (this._goal.reached) {
// do nothing
} else if (this._last_ok != 0) {
if ((now - this._last_ok) > STABLE_DELAY) {
// Set the drone in hover mode
this._client.stop();
this._goal.reached = true;
// Trigger the callback. We clear the callback before calling
// it, just in case the callback adds a new callback and a new goal.
// We schedule the callback in the near future. This is to make
// sure we finish all our work before the callback is called.
if (this._callback != null) {
var cb = this._callback;
setTimeout(this._callback, 10);
this._callback = null;
cb(this.state());
}
// Emit a state reached
@@ -224,8 +238,13 @@ Controller.prototype._control = function(d) {
this._client.up(cz);
this._client.clockwise(cyaw);
// Reset last ok since we are in motion
this._last_ok = 0;
// If we just left the goal, we notify
if (this._last_ok != 0) {
// Reset last ok since we are in motion
this._last_ok = 0;
this._goal.reached = false;
this.emit('goalLeft', this._state);
}
}
// Emit the control data for others
lib/PID.js
+10 -8
Ver Arquivo
@@ -11,7 +11,7 @@ PID.prototype.configure = function(kp,ki,kd) {
}
PID.prototype.reset = function() {
this._last_time = Date.now();
this._last_time = 0;
this._last_error = Infinity;
this._error_sum = 0;
}
@@ -19,16 +19,18 @@ PID.prototype.reset = function() {
PID.prototype.getCommand = function(e) {
// Compute dt in seconds
var time = Date.now();
var dt = (time - this._last_time) /1000
var dt = (time - this._last_time) / 1000
// Compute de (error derivation)
var de = 0;
if (this._last_error < Infinity) {
de = (e - this._last_error) / dt;
}
if (this._last_time != 0) {
// Compute de (error derivation)
if (this._last_error < Infinity) {
de = (e - this._last_error) / dt;
}
// Integrate error
this._error_sum += e * dt;
// Integrate error
this._error_sum += e * dt;
}
// Update our trackers
this._last_time = time;
package.json
+2 -1
Ver Arquivo
@@ -16,7 +16,8 @@
"pid"
],
"dependencies": {
"sylvester": "0.0.21"
"sylvester": "0.0.21",
"async": "~0.2.9"
},
"author": "Laurent Eschenauer <laurent@eschenauer.be>",
"license": "MIT"