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!");
}
})