leap-drone
Version:
Allows users to fly Parrot Bluetooth drones using Leap Motion
330 lines (229 loc) • 7.76 kB
JavaScript
var Cylon = require("cylon");
var Drone = require('leap-drone');
var keypress = require('keypress');
var d = new Drone(process.env.UUID); // Create new Drone object using the devices UUID. Process.env.UUID is created after the inital connection is made to the drone.
var DRONE_STEPS = 60;
// Yaw turn clockwise or counter clockwise;
var TURN_TRESHOLD = 0.3, // Number determins when we should start sending commands to the drone
TURN_SPEED_FACTOR = 50;
// Forward & Backward
var DIRECTION_THRESHOLD = 0.3, // Number determins when we should start sending commands to the drone
DIRECTION_SPEED_FACTOR = 10;
// WIP - Controls the up and down movement using Leap Motion
var UP_CONTROL_THRESHOLD = 50,
UP_SPEED_FACTOR = 0.01;
var handStartPosition = [],
handStartDirection = [],
handWasClosedInLastFrame = false,
flying = false,
stopped = true,
takeoff,
value,
signal,
land,
batteryLevel,
debugHandGestures = false,
showDebugMessages = true;
Cylon.robot({ // We are using Cylon to communicate with the Leap Motion controller. They have built a nice module for us to use without having to build our own.
connections: {
leapmotion: { adaptor: "leapmotion" },
},
devices: {
leapmotion: { driver: "leapmotion" },
},
work: function(my) {
if (!debugHandGestures) {
console.log('Listening for drone...');
d.connect(function(){
console.log('Drone found, Connecting...');
d.setup(function () {
d.flatTrim();
d.startPing();
d.flatTrim();
console.log('Drone ready for flight!');
});
});
}
keypress(process.stdin);
process.stdin.setRawMode(true);
process.stdin.resume();
// Use Keyboard arrows for UP / DOWN... working on palm position vertial space
process.stdin.on('keypress', function (ch, key) {
if (key) {
if (key.name === 'up') {
consoleLog('Drone, move down');
d.up({ steps: 20 });
} else if (key.name === 'down') {
consoleLog('Drone, move down');
d.down({ steps: 20 });
} else if (key.name === 't') {
consoleLog('Drone, take off');
d.flatTrim();
takeoff();
} else if (key.name === 'l') {
consoleLog('Drone, land');
land();
}
}
if (key && key.ctrl && key.name === 'c') {
process.stdin.pause();
process.exit();
}
});
my.leapmotion.on("gesture", function(gesture) {
var type = gesture.type;
switch(type) {
case 'keyTap':
if (flying) { // flying set in the takeoff() function and ensures that drone will land while flying and takeoff while dormant
d.rightFlip({ steps: 2 });
consoleLog('Drone, flip', value = null);
}
}
});
my.leapmotion.on("hand", function(hand) {
var handOpen = !!hand.fingers.filter(function(f) {
return f.extended;
}).length;
if (handOpen && flying) {
var yawRadians = hand.yaw();
// TURNS
value = Math.abs(yawRadians * TURN_SPEED_FACTOR);
if (yawRadians > TURN_TRESHOLD) {
consoleLog('Drone, turn clockwise', value);
d.clockwise({
steps: DRONE_STEPS,
speed: value
});
}
if (yawRadians < -TURN_TRESHOLD) {
consoleLog('Drone, turn counter clockwise', value);
d.counterClockwise({
steps: DRONE_STEPS,
speed: value
});
}
// UP and DOWN using hand motions
// if (vertical > UP_CONTROL_THRESHOLD) {
//
// if ((hand.palmPosition[1] - handStartPosition[1]) >= 0) {
// signal = 1;
// } else {
// signal = -1;
// }
//
// value = Math.round(vertical - UP_CONTROL_THRESHOLD) * UP_SPEED_FACTOR;
//
// if (signal > 0) {
//
// consoleLog('Drone, move up', value);
//
// // my.drone.up(value);
// }
//
// if (signal < 0) {
//
// consoleLog('Drone, move down', value);
//
// // my.drone.down(value);
// }
// }
// DIRECTION FRONT/BACK
if ((Math.abs(hand.palmNormal[2]) > DIRECTION_THRESHOLD)) {
if (hand.palmNormal[2] > 0) {
value = Math.abs(
Math.round(hand.palmNormal[2] * 10 + DIRECTION_THRESHOLD) *
DIRECTION_SPEED_FACTOR
);
d.forward({
steps: DRONE_STEPS,
speed: value
});
consoleLog('Drone, move forward', value);
}
if (hand.palmNormal[2] < 0) {
value = Math.abs(
Math.round(hand.palmNormal[2] * 10 - DIRECTION_THRESHOLD) *
DIRECTION_SPEED_FACTOR
);
d.backward({
steps: DRONE_STEPS,
speed: value
});
consoleLog('Drone, move backward', value);
}
}
// DIRECTION LEFT/RIGHT
if (Math.abs(hand.palmNormal[0]) > DIRECTION_THRESHOLD) {
if (hand.palmNormal[0] > 0) {
value = Math.abs(
Math.round(hand.palmNormal[0] * 10 + DIRECTION_THRESHOLD) *
DIRECTION_SPEED_FACTOR
);
d.tiltLeft({
steps: DRONE_STEPS,
speed: value
});
consoleLog('Drone, move left', value);
}
if (hand.palmNormal[0] < 0) {
value = Math.abs(
Math.round(hand.palmNormal[0] * 10 - DIRECTION_THRESHOLD) *
DIRECTION_SPEED_FACTOR
);
d.tiltRight({
steps: DRONE_STEPS,
speed: value
});
consoleLog('Drone, move right', value);
}
}
// AUTO FREEZE
if (
// within left/right threshold
(Math.abs(hand.palmNormal[0]) < DIRECTION_THRESHOLD) &&
// within forward/back threshold
(Math.abs(hand.palmNormal[2]) < DIRECTION_THRESHOLD) &&
// within up/down threshold
Math.abs(hand.palmPosition[1] - handStartPosition[1]) <
UP_CONTROL_THRESHOLD &&
// within turn threshold
Math.abs(handStartDirection[0] - hand.direction[0]) <
TURN_TRESHOLD) {
d.flatTrim();
}
}
if (!handOpen && !handWasClosedInLastFrame) {
consoleLog('Drone, stop moving. Lost hand signal', value = null);
d.flatTrim();
}
handWasClosedInLastFrame = !handOpen;
});
}
}).start();
var takeoff = function () {
consoleLog('Drone, take off', value = null);
if (!debugHandGestures) {
flying = true; // enables actions to be published
d.takeOff();
d.flatTrim();
}
}
var land = function () {
if (!debugHandGestures) {
d.land();
flying = false; // prevents faye from publishing actions when drone has landed
stopped = true;
consoleLog('Drone, land', value = null);
}
}
var batteryLevel = function() {
d.on('battery', function(data){
console.log('Battery level: ' + data + '%');
});
}
var consoleLog = function(str, value) {
if (showDebugMessages) {
return console.log(str, value);
}
return;
}