Skip to content
Permalink
Branch: master
Find file Copy path
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
577 lines (425 sloc) 11.5 KB

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

 

License

Copyright (c) 2012-2014 Rick Waldron waldron.rick@gmail.com Licensed under the MIT license. Copyright (c) 2015-2019 The Johnny-Five Contributors Licensed under the MIT license.

You can’t perform that action at this time.