johnny-five/eg/distance-array.js

The JavaScript Arduino Programming Framework.

var five = require("../lib/johnny-five.js");
var StateMachine = require("javascript-state-machine");
var tick = global.setImmediate || process.nextTick;
var board = new five.Board();

board.on("ready", function() {

  var directions = [{
    position: "right",
    model: "0A21",
    range: [6, 7]
  }, {
    position: "center",
    model: "0A02",
    range: [24, 36]
  }, {
    position: "left",
    model: "0A21",
    range: [6, 9]
  }, ];

  var invert = {
    right: "left",
    left: "right",
    advance: "reverse",
    reverse: "advance"
  };

  var infras = Array.from(directions, function(direction, index) {
    return new five.IR.Distance({
      device: direction.model,
      pin: "A" + index,
      id: direction.position,
    });
  });

  var sensor = infras.reduce(function(sensor, infra) {
    sensor[infra.id] = {
      sample: new Sample(10),
      distance: 0,
      isSampled: false,
      isCorrecting: false
    };
    return sensor;
  }, {});

  infras.forEach(function(infra) {
    infra.on("data", function() {
      sensor[this.id].sample.push(this.inches);

      var average = sensor[this.id].sample.average;

      if (average) {
        sensor[this.id].isSampled = true;
        sensor[this.id].distance = average | 0;
      }
    });
  });

  var expandos = ["direction", "movement", "speed"];

  var machine = StateMachine.create({
    initial: "moving",

    /**
     * event => state
     *
     *    move  => moving
     *    avoid => avoiding
     *    stop  => stopped
     */

    events: [{
      name: "move",
      from: ["moving", "avoiding", "stopped"],
      to: "moving"
    }, {
      name: "avoid",
      from: "moving",
      to: "avoiding"
    }, {
      name: "stop",
      from: ["moving", "avoiding"],
      to: "stopped"
    }, ],

    callbacks: {

      // Move/Moving
      // onbeforemove:  function() {
      //   // One time defaults setup
      //   if (!this.direction) {
      //     Object.assign(this, {
      //       isCorrecting: false,
      //       direction: "center",
      //       movement: "advance",
      //       speed: 0.5
      //     });
      //   }
      // },
      onentermoving: function(event, from, to, data) {
        // console.log("onentermoving");

        data = data || {};

        var isChanging = false;

        // One time defaults setup
        if (!this.direction) {
          Object.assign(this, {
            isCorrecting: false,
            alternative: null,
            direction: "center",
            movement: "advance",
            speed: 0.5
          });

          isChanging = true;
        } else {
          isChanging = expandos.some(function(property) {
            return data[property] !== this[property];
          }, this);

          if (isChanging) {
            this.direction = data.direction || this.direction;
            this.movement = data.movement || this.movement;
            this.speed = data.speed || this.speed;
          }
        }

        if (isChanging) {
          console.log("Move %s %s at %d speed", this.direction, this.movement, this.speed);
        }
      },
      onleavemoving: function() {
        console.log("onleavemoving");
        console.log("update steering direction, delay");
        //
        //
        setTimeout(function() {
          machine.transition();
        }, 500);

        return StateMachine.ASYNC;
      },

      // Avoid/Avoiding
      onenteravoiding: function(event, from, to, data) {
        console.log("onenteravoiding");
        console.log("turn to: ", data.direction);
        /**
         *
         * direction?
         *
         *    forward
         *    reverse
         *    left
         *    right
         */
      },
      onleaveavoiding: function() {
        console.log("onleaveavoiding");
        console.log("update steering direction, delay");

        setTimeout(function() {
          machine.transition();
        }, 500);

        return StateMachine.ASYNC;
      },

      // Stop/Stopped
      onenterstopped: function() {
        console.log("enter: stopped");
        console.log("call drive.stop()");
      },
    }
  });

  var time = Date.now();

  tick(function loop() {
    tick(loop);

    var now = Date.now();
    var correction = 4;

    // Reduce to 10hz for development
    if (now - 100 === time) {
      time = now;

      if (machine.isCorrecting) {
        return;
      }

      directions.find(function(direction) {
        var position = direction.position;
        var distance = sensor[position].distance;
        var safe = direction.range[0];
        var unsafe = direction.range[1];
        var alternative;

        if (sensor[position].isSampled) {
          if (distance !== 0 && distance <= unsafe) {
            if (machine.can("avoid")) {
              alternative = invert[position];

              // Obstacle is dead center, pick a random direction to go.
              if (!alternative) {
                alternative = machine.alternative ?
                  machine.alternative :
                  Math.random(10) * 10 > 5 ? "right" : "left";


                // TODO:
                //
                // Once the coast is clear, the `machine.alternative` should be
                // reset to null.
              }

              console.log(
                "Collision %d inches from the %s, move %s", distance, position, alternative
              );

              machine.avoid({
                direction: alternative
              });

              return;
            }
          }

          if (sensor[position].distance >= safe) {
            if (machine.can("move")) {
              machine.move({
                direction: "center"
              });
            }
          }
        }
      });
    }
  });
});


function Sample(samples) {
  Array.call(this, samples);
  Object.defineProperties(this, {
    samples: {
      value: samples
    },
    average: {
      get: function() {
        var average = 0;
        var value = 0;

        if (this.length === samples) {
          for (var i = 0; i < samples; i++) {
            value += this[i];
          }

          average = value / samples;
          this.length = 0;
          return average;
        }
        return null;
      }
    }
  });
}

Sample.prototype = Object.create(Array.prototype, {
  constructor: {
    value: Sample
  }
});