p71924506
/
cylon
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Clean up and update Leap ARDrone example

This commit is contained in:
Andrew Stewart 2014-10-20 13:27:23 -07:00
parent 1f36d5a030
commit c3a3f76f24
1 changed files with 117 additions and 133 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

250
examples/leap_ardrone/leap_ardrone.js
View File

@ -1,28 +1,30 @@
/*
* leap_ardrone.js
*
*
* Written by Giuliano Sposito and Fábio Uechi
* Copyright (c) 2013-2014 CI&T Software
* Licensed under the Apache 2.0 license.
*/
Cylon = require('cylon');
var Cylon = require('cylon');
// constants
var lastS = 0,
handStartPosition = [],
handStartDirection = [],
UP_CONTROL_THRESHOLD = 50,
UP_SPEED_FACTOR = 0.01,
CIRCLE_THRESHOLD = 1.5,
DIRECTION_THRESHOLD = 0.25,
DIRECTION_SPEED_FACTOR = 0.05,
TURN_SPEED = 0.5,
var TURN_SPEED = 0.5,
TURN_TRESHOLD = 0.2,
TURN_SPEED_FACTOR = 2.0;
Cylon.robot({
var DIRECTION_THRESHOLD = 0.25,
DIRECTION_SPEED_FACTOR = 0.05;
var UP_CONTROL_THRESHOLD = 50,
UP_SPEED_FACTOR = 0.01,
CIRCLE_THRESHOLD = 1.5;
var handStartPosition = [],
handStartDirection = [];
var handWasClosedInLastFrame = false;
Cylon.robot({
connections: [
{ name: 'leapmotion', adaptor: 'leapmotion', port: '127.0.0.1:6437' },
{ name: 'ardrone', adaptor: 'ardrone', port: '192.168.1.1' },
@ -30,140 +32,122 @@ Cylon.robot({
],
devices: [
{ name: 'drone', driver: 'ardrone', connection:'ardrone' },
{ name: 'leapmotion', driver: 'leapmotion', connection:'leapmotion' },
{ name: 'keyboard', driver: 'keyboard', connection:'keyboard'}
{ name: 'drone', driver: 'ardrone', connection: 'ardrone' },
{ name: 'leapmotion', driver: 'leapmotion', connection: 'leapmotion' },
{ name: 'keyboard', driver: 'keyboard', connection: 'keyboard'}
],
work: function(my) {
my.keyboard.on('right', my.drone.rightFlip);
my.keyboard.on('left', my.drone.leftFlip);
my.keyboard.on('up', my.drone.frontFlip);
my.keyboard.on('down', my.drone.backFlip);
// HAND
my.leapmotion.on('hand', function(hand) {
my.keyboard.on('w', my.drone.wave);
my.keyboard.on('s', my.drone.stop);
my.keyboard.on('l', my.drone.land);
// detects open hand ==> reset
if (hand.s>1.5 && lastS<=1.5) {
handStartPosition = hand.palmPosition;
handStartDirection = hand.direction;
};
// TURNS
if(hand.s>1.5 && Math.abs(handStartDirection[0]-hand.direction[0]) > TURN_TRESHOLD ) {
var signal = handStartDirection[0]-hand.direction[0];
var value = (Math.abs(handStartDirection[0]-hand.direction[0])-TURN_TRESHOLD)*TURN_SPEED_FACTOR;
if (signal>0){
my.drone.counterClockwise(value);
}
if (signal<0){
my.drone.clockwise(value);
}
}
// UP and DOWN
if (hand.s>1.5 && Math.abs(hand.palmPosition[1]-handStartPosition[1]) > UP_CONTROL_THRESHOLD) {
var signal = (hand.palmPosition[1]-handStartPosition[1]) >= 0 ? 1 : -1;
var value = Math.round(Math.abs((hand.palmPosition[1]-handStartPosition[1]))-UP_CONTROL_THRESHOLD) * UP_SPEED_FACTOR;
if (signal>0) {
my.drone.up(value);
};
if (signal<0) {
my.drone.down(value);
}
}
// DIRECTION FRONT/BACK
if (hand.s>1.5 && (Math.abs(hand.palmNormal[2])>DIRECTION_THRESHOLD)) {
if (hand.palmNormal[2]>0) {
var value = Math.abs(Math.round( hand.palmNormal[2]*10 + DIRECTION_THRESHOLD )*DIRECTION_SPEED_FACTOR);
my.drone.forward( value );
};
if (hand.palmNormal[2]<0) {
var value = Math.abs(Math.round( hand.palmNormal[2]*10 - DIRECTION_THRESHOLD )*DIRECTION_SPEED_FACTOR);
my.drone.back( value );
};
}
// DIRECTION LEFT/RIGHT
if (hand.s>1.5 && (Math.abs(hand.palmNormal[0])>DIRECTION_THRESHOLD)) {
if (hand.palmNormal[0]>0) {
var value = Math.abs(Math.round( hand.palmNormal[0]*10 + DIRECTION_THRESHOLD )*DIRECTION_SPEED_FACTOR);
my.drone.left( value );
};
if (hand.palmNormal[0]<0) {
var value = Math.abs(Math.round( hand.palmNormal[0]*10 - DIRECTION_THRESHOLD )*DIRECTION_SPEED_FACTOR);
my.drone.right( value );
};
}
// AUTO FREEZE
if ( hand.s>1.5 && // open hand
(Math.abs(hand.palmNormal[0])<DIRECTION_THRESHOLD) && // within left/right threshold
(Math.abs(hand.palmNormal[2])<DIRECTION_THRESHOLD) && // within forward/back threshold
Math.abs(hand.palmPosition[1]-handStartPosition[1]) < UP_CONTROL_THRESHOLD && // within up/down threshold
Math.abs(handStartDirection[0]-hand.direction[0]) < TURN_TRESHOLD) // within turn threshold
{
my.drone.stop();
}
// COMMAND FREEZE
if (hand.s<=1.5 && lastS > 1.5) { // closed hand
my.drone.stop();
}
lastS = hand.s;
});// end hand
// Gestures
my.leapmotion.on('gesture', function(gesture) {
if (gesture.type=='circle' && gesture.state=='stop' && gesture.progress > CIRCLE_THRESHOLD ){
var type = gesture.type,
state = gesture.state,
progress = gesture.progress;
if (type === 'circle' && state === 'stop' && progress > CIRCLE_THRESHOLD) {
if (gesture.normal[2] < 0) {
my.drone.takeoff();
};
}
if (gesture.normal[2] > 0) {
my.drone.land();
}
}
// EMERGENCE STOP
if (gesture.type=='keyTap' || gesture.type=='screenTap') {
my.drone.stop();
};
}); // end gesture
//KEYBOARD
my.keyboard.on('right', function(key) {
my.drone.rightFlip();
});
my.keyboard.on('left', function(key) {
my.drone.leftFlip();
});
my.keyboard.on('up', function(key) {
my.drone.frontFlip();
});
my.keyboard.on('down', function(key) {
my.drone.backFlip();
});
my.keyboard.on('w', function(key) {
my.drone.wave();
});
my.keyboard.on('s', function(key) {
// emergency stop
if (type === 'keyTap' || type === 'screenTap') {
my.drone.stop();
});
my.keyboard.on('l', function(key) {
my.drone.land();
}
});
} // end work
}).start();
my.leapmotion.on('hand', function(hand) {
var handOpen = !!hand.fingers.filter(function(f) {
return f.extended;
}).length;
if (handOpen) {
if (handWasClosedInLastFrame) {
handStartPosition = hand.palmPosition;
handStartDirection = hand.direction;
}
// TURNS
if (Math.abs(handStartDirection[0] - hand.direction[0]) > TURN_TRESHOLD) {
var signal = handStartDirection[0] - hand.direction[0],
value = (Math.abs(handStartDirection[0] - hand.direction[0])- TURN_TRESHOLD) * TURN_SPEED_FACTOR;
if (signal > 0){
my.drone.counterClockwise(value);
}
if (signal < 0){
my.drone.clockwise(value);
}
}
// UP and DOWN
if (Math.abs(hand.palmPosition[1] - handStartPosition[1]) > UP_CONTROL_THRESHOLD) {
var signal = (hand.palmPosition[1] - handStartPosition[1]) >= 0 ? 1 : - 1;
var value = Math.round(Math.abs((hand.palmPosition[1] - handStartPosition[1])) - UP_CONTROL_THRESHOLD) * UP_SPEED_FACTOR;
if (signal > 0) {
my.drone.up(value);
}
if (signal < 0) {
my.drone.down(value);
}
}
// DIRECTION FRONT/BACK
if ((Math.abs(hand.palmNormal[2]) > DIRECTION_THRESHOLD)) {
if (hand.palmNormal[2] > 0) {
var value = Math.abs(Math.round(hand.palmNormal[2] * 10 + DIRECTION_THRESHOLD) * DIRECTION_SPEED_FACTOR);
my.drone.forward(value);
}
if (hand.palmNormal[2] < 0) {
var value = Math.abs(Math.round(hand.palmNormal[2] * 10 - DIRECTION_THRESHOLD) * DIRECTION_SPEED_FACTOR);
my.drone.back(value);
}
}
// DIRECTION LEFT/RIGHT
if ((Math.abs(hand.palmNormal[0])>DIRECTION_THRESHOLD)) {
if (hand.palmNormal[0] > 0) {
var value = Math.abs(Math.round(hand.palmNormal[0] * 10 + DIRECTION_THRESHOLD ) * DIRECTION_SPEED_FACTOR);
my.drone.left(value);
}
if (hand.palmNormal[0] < 0) {
var value = Math.abs(Math.round(hand.palmNormal[0] * 10 - DIRECTION_THRESHOLD) * DIRECTION_SPEED_FACTOR);
my.drone.right(value);
}
}
// AUTO FREEZE
if ((Math.abs(hand.palmNormal[0]) < DIRECTION_THRESHOLD) && // within left/right threshold
(Math.abs(hand.palmNormal[2]) < DIRECTION_THRESHOLD) && // within forward/back threshold
Math.abs(hand.palmPosition[1] - handStartPosition[1]) < UP_CONTROL_THRESHOLD && // within up/down threshold
Math.abs(handStartDirection[0] - hand.direction[0]) < TURN_TRESHOLD) // within turn threshold
{
my.drone.stop();
}
}
if (!handOpen && !handWasClosedInLastFrame) {
my.drone.stop();
}
handWasClosedInLastFrame = !handOpen;
});
}
}).start();