johnny-five
Version:
Firmata based Arduino Programming Framework.
598 lines (480 loc) • 12.1 kB
JavaScript
var five = require("../lib/johnny-five.js");
var temporal = require("temporal");
var keypress = require("keypress");
// var XboxController = require("xbox-controller");
// var board = new five.Board({ port: "/dev/cu.usbmodem1421" });
// var util = require("util"),
// Emitter = require("events").EventEmitter;
// function XboxController() {
// Emitter.call(this);
// }
// util.inherits(XboxController, Emitter);
// var controller = new XboxController();
var board = new five.Board();
var flags = process.argv.slice(2).reduce(function(flags, flag) {
flags[flag] = true;
return flags;
}, {});
//
//
//
// TODO:
// - Work through all items in Step 17:
// http://www.lynxmotion.com/images/html/build106.htm
//
// - Gait:
// Step 1 Shift weight to robot's right
// Step 2 Step forward
// Step 3 Shift weight to robot's left
// Step 4 Step forward
//
// rh -25
// lh 25
// rk -20
// lk 20
// rf -8
// lf 8
//
// Then reverse.
function move(positions, speed) {
joints.forEach(function(part, i) {
legs[part].to(positions[i], speed);
});
}
function step(positions, speed) {
joints.forEach(function(part, i) {
legs[part].step(positions[i], speed);
});
}
var all, hips, knees, feet, scanner, task;
var isAvoiding = false;
var isScanning = false;
var isTurning = false;
var joints = [ "rf", "rk", "rh", "lf", "lk", "lh" ];
var cinvert = {
rev: "fwd",
fwd: "rev",
turn: "turn"
};
var legs = {};
var inits = [
{ pin: 12, id: "rh", startAt: 98 },
{ pin: 11, id: "rk", startAt: 92 },
{ pin: 10, id: "rf", startAt: 91 },
{ pin: 9, id: "lh", startAt: 89, isInverted: true },
{ pin: 8, id: "lk", startAt: 89, isInverted: true },
{ pin: 7, id: "lf", startAt: 83, isInverted: true }
];
var gaits = [
{
id: "fwd",
sets: [
{
id: "m",
lapse: 500,
sequence: [
// [ rf, rk, rh, lf, lk, lh ]
// [ 105, 70, 70, 75, 110, 110 ],
// [ 75, 70, 70, 105, 110, 110 ],
// [ 75, 110, 110, 105, 70, 70 ],
// [ 105, 110, 110, 75, 70, 70 ]
// [ 105, 70, 60, 75, 110, 110 ],
// [ 75, 70, 80, 105, 110, 110 ],
// [ 75, 110, 110, 105, 70, 70 ],
// [ 105, 110, 110, 75, 70, 70 ]
[ 105, 70, 70, 75, 100, 100 ],
[ 85, 70, 70, 105, 100, 100 ],
[ 85, 110, 110, 105, 80, 80 ],
[ 105, 110, 110, 75, 80, 80 ]
]
},
{
id: "long",
lapse: 600,
sequence: [
// [ rf, rk, rh, lf, lk, lh ]
// [ 105, 45, 45, 70, 135, 135 ],
// [ 78, 45, 45, 102, 135, 135 ],
// [ 78, 135, 135, 102, 45, 45 ],
// [ 105, 135, 135, 70, 45, 45 ]
[ 105, 45, 45, 70, 125, 125 ],
[ 78, 45, 45, 102, 125, 125 ],
[ 78, 135, 135, 102, 50, 50 ],
[ 105, 135, 135, 70, 50, 50 ]
]
}
]
},
{
id: "rev",
sets: [
{
id: "long",
lapse: 600,
sequence: [
[ 78, 45, 45, 102, 135, 135 ],
[ 105, 45, 45, 70, 135, 135 ],
[ 105, 135, 135, 70, 45, 45 ],
[ 78, 135, 135, 102, 45, 45 ]
]
}
]
},
{
id: "turn",
sets: [
{
id: "left",
lapse: 600,
sequence: [
[ 105, 70, 70, 75, 100, 100 ],
[ 85, 110, 110, 105, 80, 80 ],
[ 105, 110, 110, 75, 80, 80 ]
]
},
{
id: "right",
lapse: 600,
sequence: [
// [ rf, rk, rh, lf, lk, lh ]
[ 75, 100, 100, 105, 70, 70 ],
[ 105, 80, 80, 85, 110, 110 ],
[ 75, 80, 80, 105, 110, 110 ]
]
}
]
},
{
id: "home",
sets: [
{
id: "go",
lapse: 500,
sequence: inits.map(function(init) {
return init.startAt;
})
}
]
}
];
board.on("ready", function() {
var neck = new five.Servo({ pin: 5, range: [20, 160], startAt: 90 });
var eyes = new five.IR.Distance({
device: "2Y0A21",
pin: "A0",
freq: 100
});
// Leg initialization
legs = inits.reduce(function(accum, init) {
accum[init.id] = new five.Servo(init);
return accum;
}, {});
// Groupings
all = new five.Servos();
hips = new five.Servos([ legs.rh, legs.lh ]);
knees = new five.Servos([ legs.rk, legs.lk ]);
feet = new five.Servos([ legs.rf, legs.lf ]);
var controls = gaits.reduce(function(controls, gait) {
controls[gait.id] = gait.sets.reduce(function(seq, set) {
seq[set.id] = function() {
if (task) {
task.stop();
}
var queue = set.sequence.map(function(sequence) {
return {
wait: set.lapse,
task: function() {
move(sequence, set.lapse);
}
}
});
queue.push({
wait: 10,
task: controls[gait.id][set.id]
});
task = temporal.queue(queue);
};
return seq;
}, {});
return controls;
}, {});
function scan() {
var direction = 1;
if (flags.noscan) {
return;
}
isScanning = true;
// neck.center();
scanner = temporal.loop(600, function() {
neck.to(neck.range[(direction ^= 0x01)], 600);
});
}
var last = {
control: "long",
direction: "rev",
heading: 0
};
console.log( "Scanning: %s", !flags.noscan );
eyes.on("data", function() {
var heading, direction, control;
if (flags.noscan) {
return;
}
if (this.inches < 10) {
heading = neck.last.degrees;
if (scanner) {
scanner.stop();
}
neck.center();
if (isAvoiding) {
return;
}
if (isTurning) {
return;
}
if (heading < 90) {
// left
direction = "turn";
control = "right";
} else {
// right
direction = "turn";
control = "left";
}
if (heading < 120 && heading > 70) {
direction = "fwd";
control = "long";
}
console.log(
"DANGER: %d, %d, %s, %s", this.inches, heading, direction, control
);
isTurning = true;
isAvoiding = true;
isScanning = false;
if (control !== "long") {
console.log( "turn towards: ", control );
console.log( "last turn towards: ", last.control );
}
controls[cinvert[direction]][control]();
last.control = control;
last.direction = direction;
last.heading = heading;
temporal.delay(4000, function() {
isTurning = false;
});
} else {
if (isAvoiding && !isTurning) {
console.log(
"RESUME: %d", this.inches
);
isAvoiding = false;
controls.fwd.long();
scan();
} else {
// ...
}
}
});
// {{ CONTROLLER CODE HERE }}
function scale(x, fromLow, fromHigh, toLow, toHigh) {
return (x - fromLow) * (toHigh - toLow) /
(fromHigh - fromLow) + toLow;
}
function degrees(val) {
if (val >= 0) {
return scale(val, 0, -32768, 90, 180);
}
return scale(val, 0, 32768, 89, 0);
}
var commands = null;
var side = -1;
var index = 0;
var Group = {
isGrouped: false,
get side() {
return side;
},
set side(value) {
// -1 = both, 0 = right, 1 = left
var which = value === -1 ?
"both" : (value ? "right" : "left");
console.log(
"Controlling %s", which.toUpperCase()
);
side = value;
},
get index() {
return index;
},
set index(value) {
console.log(
"Controlling %s", this.sets[value].name
);
index = value;
},
sets: [
// Feet
{ name: "Feet",
pins: [ 7, 10 ],
parts: [ "lf", "rf" ],
values: [ null, null ]
},
// Knees
{ name: "Knees",
pins: [ 8, 11 ],
parts: [ "lk", "rk" ],
values: [ null, null ]
},
// Hips
{ name: "Hips",
pins: [ 9, 12 ],
parts: [ "lh", "rh" ],
values: [ null, null ]
}
]
};
// controller.on("y:press", function() {
// var which = side === -1 ?
// "both" : (side ? "left" : "right");
// var part = Group.sets[index].name;
// console.log(
// "Controlling %s %s", which, part
// );
// });
// controller.on("b:press", function() {
// Group.side = -1;
// });
// controller.on("dright:press", function() {
// Group.side = 1;
// });
// controller.on("dleft:press", function() {
// Group.side = 0;
// });
// controller.on("dup:press", function() {
// var next = Group.index + 1;
// if (next === Group.sets.length) {
// next = 0;
// }
// Group.index = next;
// });
// controller.on("ddown:press", function() {
// var next = Group.index - 1;
// if (next === -1) {
// next = Group.sets.length - 1;
// }
// Group.index = next;
// });
// controller.on("left:move", function(position) {
// var pos = degrees(position.y);
// var part = Group.sets[Group.index].parts[0];
// legs[part].to(pos);
// });
// controller.on("right:move", function(position) {
// var pos = degrees(position.y);
// var part = Group.sets[Group.index].parts[1];
// legs[part].to(pos);
// });
// controller.on("left:move", function(position) {
// var part = Group.sets[Group.index].parts[0];
// // var pos = degrees(legs[part].startAt, position.y);
// var pos = degrees(90, position.y);
// legs[part].to(pos);
// });
// controller.on("right:move", function(position) {
// var part = Group.sets[Group.index].parts[1];
// var pos = degrees(legs[part].startAt, position.y);
// legs[part].to(pos);
// });
// controller.on("lb:press", function() {
// tuner(0);
// });
// controller.on("rb:press", function() {
// tuner(1);
// });
function tuner(step) {
// Cannot control both at a time
if (Group.side === -1) {
return;
}
var set = Group.sets[Group.index];
var part = set.parts[Group.side];
var value = set.values[Group.side];
console.log( part, "(part)" );
console.log( legs[part] );
if (value === null) {
value = legs[part].startAt;
}
if (step < 0) {
value--;
} else {
value++;
}
console.log( value, "(value)" );
process.nextTick(function() {
legs[part].to(value);
});
set.values[Group.side] = value;
}
function controller(ch, key) {
if (key) {
if (key.name === "space") {
var next = Group.index - 1;
if (next === -1) {
next = Group.sets.length - 1;
}
Group.index = next;
}
if (key.name === "up") {
tuner(1);
}
if (key.name === "down") {
tuner(-1);
}
if (key.name === "left") {
Group.side = 0;
}
if (key.name === "right") {
Group.side = 1;
}
commands = [].slice.call(arguments);
} else {
if (ch >= 1 && ch <= 9) {
speed = five.Fn.scale(ch, 1, 9, 0, 255);
controller.apply(null, commands);
}
}
}
this.repl.inject({
get task() {
return task;
},
neck: neck,
all: all,
hips: hips,
knees: knees,
feet: feet,
legs: legs,
controls: controls,
// fwd: fwd,
// fwdlong: fwdlong,
scan: scan,
eyes: eyes,
peanuts: function() {
var direction = 0;
task = temporal.loop(500, function() {
if (direction) {
feet.to(150, 500);
} else {
feet.to(90, 500);
}
direction = direction ? 0 : 1;
});
}
});
// controls.turn.r()
// scan();
// controls.fwd.long();
process.stdin.on("keypress", controller);
process.stdin.setRawMode(true);
process.stdin.resume();
});