Skip to content

Instantly share code, notes, and snippets.

@BrianGenisio
Created November 2, 2014 02:00
Show Gist options
  • Save BrianGenisio/fdc8288da038036a9bb5 to your computer and use it in GitHub Desktop.
Save BrianGenisio/fdc8288da038036a9bb5 to your computer and use it in GitHub Desktop.
Using a PID controller to keep a bot close to the wall
var five = require("johnny-five"),
board = new five.Board(),
PID = require('pid-controller');
board.on("ready", function() {
console.log('ready');
var sampleRate = 10;
/////////////////////////////////////////
// CONFIGURE SERVOS AND PING SENSORS
var head = new five.Servo(6);
var wheels = {
left: new five.Servo({ pin: 10, type: 'continuous' }),
right: new five.Servo({ pin: 9, type: 'continuous' })
};
var walls = {
left: new five.Ping(13),
front: new five.Ping(11),
right: new five.Ping(12)
};
var ctrl = new PID(0, 0, 1, 10, 0, PID.DIRECT);
/////////////////////////////////////////
// ACTIONS
var actions = {
forward: function(center) {
var speed = {};
if(center < 0) {
console.log('<<<<');
speed = { left: 0.035, right: 0.045 };
} else if (center > 0) {
console.log('>>>>');
speed = { left: 0.045, right: 0.035 };
} else {
console.log('||||');
speed = { left: 0.04, right: 0.04 }
}
wheels.left.cw(speed.left);
wheels.right.ccw(speed.right);
},
stop: function() {
wheels.left.center();
wheels.right.center();
head.to(60);
}
}
///////////////////////
// READINGS
function getSetpoint() {
return 4; // inches
}
function readInput() {
return walls.left.inches;
}
//////////////////////
// GO!
function init() {
console.log('init')
ctrl.setMode(PID.AUTOMATIC);
ctrl.setSampleTime(sampleRate);
actions.stop();
}
function loop() {
ctrl.setPoint(getSetpoint());
ctrl.setInput(readInput());
ctrl.compute();
actions.forward(ctrl.myOutput - 1);
}
board.repl.inject({
head: head,
wheels: wheels,
walls: walls,
ctrl: ctrl,
actions: actions
});
init();
setInterval(loop, sampleRate);
});
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment