Navigator

Run this example from the command line with:

node eg/navigator.js

var five = require("johnny-five"),
  __ = require("fn"),
  board, Navigator, navigator, servos,
  pivotExpansion, directionMap, scale;


directionMap = {
  reverse: {
    right: "left",
    left: "right",
    fwd: "rev",
    rev: "fwd"
  },
  translations: [{
    f: "forward",
    r: "reverse",
    fwd: "forward",
    rev: "reverse"
  }, {
    r: "right",
    l: "left"
  }]
};

scale = function(speed, low, high) {
  return Math.floor(five.Fn.map(speed, 0, 5, low, high));
};


/**
 * Navigator
 * @param {Object} opts Optional properties object
 */

function Navigator(opts) {

  // Boe Navigator continuous are calibrated to stop at 90°
  this.center = opts.center || 90;

  // Initialize the right and left cooperative servos
  this.servos = {
    right: new five.Servo({
      pin: opts.right,
      type: "continuous"
    }),
    left: new five.Servo({
      pin: opts.left,
      type: "continuous"
    })
  };

  // Set the initial servo cooperative direction
  this.direction = opts.direction || {
    right: this.center,
    left: this.center
  };

  this.compass = opts.compass || null;
  this.gripper = opts.gripper || null;

  // Store the cooperative speed
  this.speed = opts.speed === undefined ? 0 : opts.speed;

  // Store a recallable history of movement
  // TODO: Include in savable history
  this.history = [];

  // Initial direction
  this.which = "forward";

  // Track directional state
  this.isTurning = false;

  // Wait 10ms, send fwd pulse on, then off to
  // "wake up" the servos
  setTimeout(function() {
    this.fwd(1).fwd(0);
  }.bind(this), 10);
}


Navigator.DIR_MAP = directionMap;

/**
 * move Move the bot in an arbitrary direction
 * @param  {Number} right Speed/Direction of right servo
 * @param  {Number} left  Speed/Direction of left servo
 * @return {Object} this
 */
Navigator.prototype.move = function(right, left) {

  // Quietly ignore duplicate instructions
  if (this.direction.right === right &&
    this.direction.left === left) {
    return this;
  }

  // Cooperative servo motion.
  // Servos are mounted opposite of each other,
  // the values for left and right will be in
  // opposing directions.
  this.servos.right.to(right);
  this.servos.left.to(left);

  // Push a record object into the history
  this.history.push({
    timestamp: Date.now(),
    right: right,
    left: left
  });

  // Update the stored direction state
  this.direction.right = right;
  this.direction.left = left;

  return this;
};


[
  /**
   * forward Move the bot forward
   * fwd Move the bot forward
   *
   * @param  {Number} 0-5, 0 is stopped, 5 is fastest
   * @return {Object} this
   */
  {
    name: "forward",
    abbr: "fwd",
    args: function(center, val) {
      return [center - (val - center), val];
    }
  },

  /**
   * reverse Move the bot in reverse
   * rev Move the bot in reverse
   *
   * @param  {Number}0-5, 0 is stopped, 5 is fastest
   * @return {Object} this
   */
  {
    name: "reverse",
    abbr: "rev",
    args: function(center, val) {
      return [val, center - (val - center)];
    }
  }

].forEach(function(dir) {

  var method = function(speed) {
    // Set default direction method
    speed = speed === undefined ? 1 : speed;

    this.speed = speed;
    this.which = dir.name;

    return this.move.apply(this,
      dir.args(this.center, scale(speed, this.center, 110))
    );
  };

  Navigator.prototype[dir.name] = Navigator.prototype[dir.abbr] = method;
});

/**
 * stop Stops the bot, regardless of current direction
 * @return {Object} this
 */
Navigator.prototype.stop = function() {
  this.speed = this.center;
  this.which = "stop";

  return this.to(this.center, this.center);
};


[
  /**
   * right Turn the bot right
   * @return {Object} this
   */
  "right",

  /**
   * left Turn the bot left
   * @return {Object} this
   */
  "left"

].forEach(function(dir) {
  Navigator.prototype[dir] = function(time) {

    // Use direction value and reverse direction map to
    // derive the direction values for moving the
    // cooperative servos
    var actual = this.direction[directionMap.reverse[dir]];

    time = time || 500;

    if (!this.isTurning) {
      // Set turning lock
      this.isTurning = true;

      // Send turning command
      this.to(actual, actual);

      // Cap turning time
      setTimeout(function() {

        // Restore direction after turn
        this[this.which](this.speed || 2);

        // Release turning lock
        this.isTurning = false;

      }.bind(this), time);
    }

    return this;
  };
});

pivotExpansion = function(which) {
  var parts;

  if (which.length === 2) {
    parts = [which[0], which[1]];
  }

  if (/\-/.test(which)) {
    parts = which.split("-");
  }

  return parts.map(function(val, i) {
    console.log(val);
    return directionMap.translations[i][val];
  }).join("-");
};


/**
 * pivot Pivot the bot with combo directions:
 * rev Move the bot in reverse
 *
 * @param  {String} which Combination directions:
 *                        "forward-right", "forward-left",
 *                        "reverse-right", "reverse-left"
 *                        (aliased as: "f-l", "f-r", "r-r", "r-l")
 *
 * @return {Object} this
 */
Navigator.prototype.pivot = function(which, time) {
  var actual, directions, scaled;

  scaled = scale(this.speed, this.center, 110);

  directions = {
    "forward-right": function() {
      this.to(this.center, scaled);
    },
    "forward-left": function() {
      this.to(this.center - (scaled - this.center), this.center);
    },
    "reverse-right": function() {
      this.to(scaled, this.center);
    },
    "reverse-left": function() {
      this.to(this.center, this.center - (scaled - this.center));
    }
  };

  which = directions[which] || directions[pivotExpansion(which)];

  which.call(this, this.speed);

  setTimeout(function() {

    this[this.which](this.speed);

  }.bind(this), time || 1000);

  return this;
};




// Begin program when the board, serial and
// firmata are connected and ready

(board = new five.Board()).on("ready", function() {

  // TODO: Refactor into modular program code

  var center, collideAt, degrees, step, facing,
    range, laser, look, isScanning, scanner, gripper, isGripping, sonar, gripAt, ping, mag, bearing;

  // Collision distance (inches)
  collideAt = 6;

  gripAt = 2;

  // Servo scanning steps (degrees)
  step = 2;

  // Current facing direction
  facing = "";

  // Scanning range (degrees)
  range = [10, 170];

  // Servo center point (degrees)
  center = ((range[1] - range[0]) / 2) + range[0];

  // Starting scanner scanning position (degrees)
  degrees = center;

  // Direction to look after releasing scanner lock (degrees)
  // look = {
  //   forward: center,
  //   left: 130,
  //   right: 40
  // };

  // Scanning state
  isScanning = true;

  // Gripping state
  isGripping = false;

  // compass/magnetometer
  mag = new five.Magnetometer();

  // Servo gripper
  gripper = new five.Gripper({
    servo: {
      pin: 13,
      range: [20, 160]
    },
    scale: [0, 10]
  });

  // New base navigator
  // right servo = pin 10, left servo = pin 11
  navigator = new Navigator({
    right: 10,
    left: 11,
    compass: mag,
    gripper: gripper
  });

  // The laser is just a special case Led
  laser = new five.Led(9);

  // Digital PWM (range)
  ping = new five.Ping(7);

  // Analog Voltage (range)
  // sonar = new five.Sonar("A0");


  // Servo scanner instance (panning)
  scanner = new five.Servo({
    pin: 12,
    range: range
  });


  // Inject navigator object into REPL
  this.repl.inject({
    b: navigator,
    g: gripper
  });


  // Initialize the scanner at it's center point
  // Will be exactly half way between the range's
  // lower and upper bound
  scanner.center();

  // Wait 1000ms, then initialize forward movement
  this.wait(1000, function() {
    // navigator.fwd(3);
  });


  // Scanner/Panning loop
  this.loop(50, function() {
    var bounds;

    bounds = {
      left: center + 15, //center + 10,
      right: center - 15 //center - 10
    };

    // During course change, scanning is paused to avoid
    // overeager redirect instructions[1]
    if (isScanning) {
      // Calculate the next step position
      if (degrees >= scanner.range[1] || degrees <= scanner.range[0]) {
        step *= -1;
      }

      // Update the position in degrees
      degrees += step;

      // The following three conditions will help determine
      // which way the navigator should turn if a potential collideAt
      // may occur in the ping "change" event handler[2]
      if (degrees > bounds.left) {
        facing = "left";
      }

      if (degrees < bounds.right) {
        facing = "right";
      }

      // if ( degrees > bounds.right && degrees < bounds.left ) {
      if (__.range(bounds.right, bounds.left).indexOf(degrees) > -1) {
        facing = "fwd";
      }


      scanner.to(degrees);
    }
  });

  // sonar.on("change", function() {
  // ping.on("change", function() {
  //   var distance = Math.abs(this.inches);

  //   // TODO: Wrap this behaviour in an abstraction
  //   if ( distance <= collideAt && !isGripping ) {
  //     gripper.max();

  //     // simulate drop instruction
  //     setTimeout(function() {
  //       isGripping = false;
  //       gripper.min();
  //     }, 5000);
  //   }
  // });

  // Compass heading monitor
  // mag.on("headingchange", function() {

  //   if ( !/[\-by]/.test(this.bearing.name) && this.bearing.name !== bearing ) {
  //     bearing = this.bearing.name;

  //     console.log( this.bearing );
  //   }
  // });

  // [2] ping "change" events are emitted when the value of a
  // distance reading has changed since the previous reading
  //
  // TODO: Avoid false positives?
  ping.on("data", function() {
    var release = 750,
      distance = Math.abs(this.inches),
      isReverse = false,
      turnTo;

    if (navigator.isTurning) {
      return;
    }

    // If distance value is null or NaN
    if (distance === null || isNaN(distance)) {
      return;
    }



    // Detect collideAt
    // && isScanning
    if (distance <= collideAt && isScanning) {

      laser.strobe();

      // Scanning lock will prevent multiple collideAt
      // detections piling up for the same obstacle
      isScanning = false;

      // Determine direction to turn
      turnTo = Navigator.DIR_MAP.reverse[facing];

      // Set reversal flag.
      isReverse = turnTo === "rev";

      // Log collideAt detection to REPL
      console.log(
        [Date.now(),
          "\tCollision detected " + this.inches + " inches away.",
          "\tTurning " + turnTo.toUpperCase() + " to avoid"
        ].join("\n")
      );

      // Turn the navigator
      navigator[turnTo](navigator.speed);


      if (isReverse) {
        release = 1500;
      }

      // [1] Allow Nms to pass and release the scanning lock
      // by setting isScanning state to true.
      board.wait(release, function() {
        console.log("Release Scanner Lock");

        degrees = 89;

        scanner.center();

        if (isReverse) {
          // navigator.fwd( navigator.speed );
          navigator.pivot("reverse-right");
          navigator.which = "fwd";
        }

        laser.brightness(0);
        isScanning = true;
      });
    }
  });
});


// References
//
// http://www.maxbotix.com/documents/MB1010_Datasheet.pdf

Navigator

License