johnny-five
Version:
Firmata based Arduino Programming Framework.
315 lines (262 loc) • 7.59 kB
JavaScript
var five = require("../lib/johnny-five.js"),
PVector = require("./pvector.js").PVector,
temporal = require("temporal"),
OpenNI = require("openni");
Object.mixin = function( receiver, supplier ) {
return Object.getOwnPropertyNames( supplier ).reduce(function( receiver, property ) {
return Object.defineProperty(
receiver, property, Object.getOwnPropertyDescriptor( supplier, property )
);
}, receiver );
};
function Initial( defaults, initializer ) {
[ defaults, initializer || {} ].reduce( Object.mixin, this );
}
function Vector( initializer ) {
Initial.call(
this, Vector.DEFAULTS, initializer || {}
);
}
Vector.DEFAULTS = {
x: 0, y: 0, z: 0
};
Vector.degrees = function( angle ) {
return (angle * 180) / Math.PI;
};
Vector.sub = function( vec1, vec2 ) {
return new Vector({
x: vec1.x - vec2.x,
y: vec1.y - vec2.y,
z: vec1.z - vec2.z
});
};
Vector.prototype.mag = function() {
var x, y, z;
x = this.x;
y = this.y;
z = this.z;
return Math.sqrt( x * x + y * y + z * z );
};
Vector.prototype.distance = function( vec ) {
var dx, dy, dz;
dx = this.x - v.x;
dy = this.y - v.y;
dz = this.z - v.z;
return Math.sqrt( dx * dx + dy * dy + dz * dz );
};
Vector.prototype.dot = function( vec ) {
return this.x * vec.x + this.y * vec.y + this.z * vec.z;
};
Vector.prototype.cross = function( vec ) {
var x = this.x,
y = this.y,
z = this.z;
return new Vector({
x: y * vec.z - vec.y * z,
y: z * vec.x - vec.z * x,
z: x * vec.y - vec.x * y
});
};
Vector.prototype.angleTo = function( vec ) {
return Math.atan2(
this.y - vec.y, this.x - vec.x
);
};
Vector.prototype.between = function( vec ) {
return Math.acos(
this.dot(vec) / (this.mag() * this.mag())
);
};
function Skeleton( initializer ) {
Initial.call(
this, Skeleton.DEFAULTS, initializer || {}
);
Skeleton.Joints.forEach(function( joint ) {
this.points[ joint ] = new Vector();
this.kinect.on( joint, function( id, x, y, z ) {
var vec = this.points[ joint ];
if ( vec ) {
vec.x = x;
vec.y = y;
vec.z = z;
}
}.bind(this));
}, this);
Skeletons.push( this );
}
Skeleton.DEFAULTS = {
points: {}
};
Skeleton.Joints = [
"head",
"neck",
// "torso",
// "waist",
"left_shoulder",
"left_elbow",
"left_hand",
"right_shoulder",
"right_elbow",
"right_hand",
"left_hip",
"left_knee",
"left_foot",
"right_hip",
"right_knee",
"right_foot"
];
var Skeletons = [];
five.Board().on("ready", function() {
var defs, servos, kinect;
// http://www.ranchbots.com/robot_arm/images/arm_diagram.jpg
defs = [
// Pivot/Rotator
{ id: "rotator", pin: 6, range: [ 0, 180 ], startAt: 90 },
// Shoulder
{ id: "upper", pin: 9, range: [ 0, 180 ], startAt: 180 },
// Elbow
{ id: "fore", pin: 10, range: [ 90, 180 ], startAt: 90 },
// Wrist
{ id: "wrist", pin: 11, range: [ 10, 170 ], startAt: 60 },
// Grip
{ id: "claw", pin: 12, range: [ 10, 170 ], startAt: 0 }
];
// Reduce the des array into an object of servo instances,
// where the servo id is the property name
servos = defs.reduce(function( accum, def ) {
return (accum[ def.id ] = five.Servo( def )) && accum;
}, {});
// Make servos accessible by name in the REPL
this.repl.inject( servos );
kinect = OpenNI();
[
"newuser",
"lostuser",
"posedetected",
"calibrationstart",
"calibrationsuccess",
"calibrationfail"
].forEach(function( type ) {
kinect.on( type , function( id ) {
console.log( "%s (%d)", type, id );
if ( type === "newuser" ) {
if ( Skeletons.length === 0 ) {
new Skeleton({
kinect: kinect
});
} else {
Skeletons[0].inFrame = true;
}
}
if ( type === "lostuser" ) {
console.log( "OUT OF FRAME" );
if ( Skeletons.length ) {
Skeletons[0].inFrame = false;
}
}
});
});
function scale() {
return Math.round(
five.Fn.scale.apply(null, arguments)
);
};
function angleOf( vec1, vec2, axis ) {
return PVector.degrees(
PVector.between(
PVector.sub( vec2, vec1 ), axis
)
);
}
function Change() {
this.last = 0;
}
Change.prototype.isNoticeable = function(value) {
if ( !Number.isFinite(value) ) {
return false;
}
if ( (value > this.last + 2) || (value < this.last - 2) ) {
this.last = value;
return true;
}
return false;
};
var last = Date.now();
var interval = 1000 / 30;
var rlow = 0;
var rhigh = 0;
var change = {
upper: new Change(),
fore: new Change(),
rotator: new Change()
};
void function main() {
setImmediate( main );
var values, right, orientation, angle, upper, fore, rotator, axis;
values = {
upper: 0,
fore: 0,
rotator: 0
};
angle = {
upper: 0,
fore: 0
};
if ( Skeletons.length && (points = Skeletons[0].points) ) {
if ( points.right_shoulder && points.right_elbow && points.right_hand ) {
if ( Date.now() > (last + interval) ) {
upper = points.right_shoulder;
fore = points.right_elbow;
rotator = points.right_hand;
axis = points.right_hip;
right = {
upper: new PVector( upper.x, upper.y ),
fore: new PVector( fore.x, fore.y ),
rotator: new PVector( rotator.x, rotator.y ),
axis: new PVector( axis.x, axis.y )
};
orientation = {
torso: PVector.sub( right.upper, right.axis ),
arm: PVector.sub( right.fore, right.upper )
};
if ( rlow === 0 || rlow > rotator.z ) {
rlow = Math.round( rotator.z );
}
if ( rhigh === 0 || rhigh < rotator.z ) {
rhigh = Math.round( rotator.z );
}
angle.upper = Math.round(
angleOf( right.fore, right.upper, orientation.torso )
);
angle.fore = Math.round(
angleOf( right.rotator, right.fore, orientation.arm )
);
values.rotator = scale( rotator.z, rlow, rhigh, 0, 180 );
values.upper = scale( angle.upper, 0, 180, 180, 0 );
values.fore = scale( angle.fore, 180, 0, 90, 180 );
if ( change.rotator.isNoticeable( values.rotator ) ) {
// console.log( values.rotator );
servos.rotator.move( values.rotator );
}
if ( change.upper.isNoticeable( values.upper ) ) {
// console.log( values.upper );
servos.upper.move( values.upper );
}
if ( change.fore.isNoticeable( values.fore ) ) {
// console.log( values.fore );
servos.fore.move( values.fore );
}
last = Date.now();
}
}
}
}();
// References
// http://www.ranchbots.com/robot_arm/images/arm_diagram.jpg
// https://github.com/OpenNI/OpenNI/blob/master/Include/XnCppWrapper.h
// http://www.mrtmrcn.com/en/post/2011/11/08/Kinect-Part-5-Kinect-Skeleton-Tracking.aspx
// http://code.google.com/p/bikinect/source/browse/trunk/MappInect/Skeleton.pde
// https://github.com/Sensebloom/OSCeleton-examples/blob/master/processing/Stickmanetic/Stickmanetic.pde
// http://www.pcl-users.org/openni-device-h-47-26-fatal-error-XnCppWrapper-h-No-such-file-or-directory-td3174297.html
// http://kinectcar.ronsper.com/docs/openni/_xn_cpp_wrapper_8h_source.html
});