johnny-five/wip/brat.js

The JavaScript Arduino Programming Framework.

// var lodash = require("lodash");
// var machina = require("machina")(lodash);
var five = require("../lib/johnny-five.js");
var temporal = require("temporal");
var keypress = require("keypress");
var dualShock = require("dualshock-controller");
var argv = require("optimist").default({
  noscan: false,
  calibrate: false
}).argv;

var board = new five.Board();
var controller = dualShock({
  config: "dualShock3",
  analogStickSmoothing: true
});

controller.isConnected = false;

// argv flags:
//
// --calibrate
//    - enables calibrating the servos, defaults to false
//
// --noscan
//    - disables the scanner, defaults to false
//


// n-DOF move
function move(positions, speed) {
  joints.forEach(function(part, i) {
    legs[part].to(positions[i], speed);
  });
}

function scale(x, fromLow, fromHigh, toLow, toHigh) {
  return (x - fromLow) * (toHigh - toLow) /
    (fromHigh - fromLow) + toLow;
}

function degrees(center, val) {
  if (val >= 128) {
    return scale(val, 128, 255, center - 1, 0);
  }
  return scale(val, 127, 0, center, 180);
}

function still() {
  Object.keys(tasks).forEach(function(name) {
    if (tasks[name]) {
      tasks[name].stop();
    }
  });
}

var all, hips, knees, feet;

var tasks = {
  controls: null,
  dance: null,
  turn: null,
  scanner: null,
  walk: null
};

var isAutonomous = false;
var isAvoiding = false;
var isMoving = false;
var isScanning = false;
var isTurning = false;
var currently = null;

var cinvert = {
  rev: "fwd",
  fwd: "rev",
  turn: "turn"
};

var legs = {};

var inits = [
  { pin: 10, id: "rf", startAt: 91 },
  { pin: 11, id: "rk", startAt: 92 },
  { pin: 12, id: "rh", startAt: 98 },

  { pin: 7,  id: "lf", startAt: 83, isInverted: true },
  { pin: 8,  id: "lk", startAt: 89, isInverted: true },
  { pin: 9,  id: "lh", startAt: 89, isInverted: true }
];

var joints = inits.map(function(init) {
  return init.id;
});

var starts = inits.map(function(init) {
  return init.startAt;
});

// TODO: These can/should be loaded in from a JSON file
var gaits = [
  {
    id: "rev",
    sets: [
      {
        id: "short",
        isLoop: true,
        isInterruption: false,
        lapse: 500,
        sequence: [
      //  [ rf, rk, rh, lf, lk, lh ]
          { moves: [ -7,-20,-20,  7, 20, 20 ] },
          { moves: [  7,-20,-20, -7, 20, 20 ] },
          { moves: [  7, 20, 20, -7,-20,-20 ] },
          { moves: [ -7, 20, 20,  7,-20,-20 ] }
        ]
      },
      {
        id: "long",
        isLoop: true,
        isInterruption: false,
        lapse: 600,
        sequence: [
          { moves: [-12,-35,-35, 12, 35, 35 ] },
          { moves: [ 12,-35,-35,-12, 35, 35 ] },
          { moves: [ 12, 35, 35,-12,-35,-35 ] },
          { moves: [-12, 35, 35, 12,-35,-35 ] }
        ]
      }
    ]
  },
  {
    id: "fwd",
    sets: [
      {
        id: "short",
        isLoop: true,
        isInterruption: false,
        lapse: 500,
        sequence: [
      //  [ rf, rk, rh, lf, lk, lh ]
          { moves: [  7,-20,-20, -7, 20, 20 ] },
          { moves: [ -7,-20,-20,  7, 20, 20 ] },
          { moves: [ -7, 20, 20,  7,-20,-20 ] },
          { moves: [  7, 20, 20, -7,-20,-20 ] }
        ]
      },
      {
        id: "long",
        isLoop: true,
        isInterruption: false,
        lapse: 600,
        sequence: [
      //  [ rf, rk, rh, lf, lk, lh ]
          { moves: [ 12,-35,-35,-12, 35, 35 ] },
          { moves: [-12,-35,-35, 12, 35, 35 ] },
          { moves: [-12, 35, 35, 12,-35,-35 ] },
          { moves: [ 12, 35, 35,-12,-35,-35 ] }
        ]
      }
    ]
  },
  {
    id: "action",
    sets: [
      {
        id: "rkick",
        isLoop: false,
        isInterruption: true,
        lapse: 500,
        sequence: [
      //  [ rf, rk, rh, lf, lk, lh ]
          { moves: [  0,  0,  0,  0,  0,  0 ] },
          { moves: [ 42,  0,  0,-14,  0,  0 ] },
          { moves: [  0,-32, 41,-23,  0,  0 ] },
          { moves: [  0, 24,-20,-23,  0,  0 ], lapse: 250 },
          { moves: [  0,  0,  0,-18,  0,  0 ] },
          { moves: [  0,  0,  0,  0,  0,  0 ] }
        ]
      },
      {
        id: "lkick",
        isLoop: false,
        isInterruption: true,
        lapse: 500,
        sequence: [
      //  [ rf, rk, rh, lf, lk, lh ]
          { moves: [  0,  0,  0,  0,  0,  0 ] },
          { moves: [-14,  0,  0, 42,  0,  0 ] },
          { moves: [-23,  0,  0,  0,-32, 41 ] },
          { moves: [-23,  0,  0,  0, 24,-20 ], lapse: 250 },
          { moves: [-18,  0,  0,  0,  0,  0 ] },
          { moves: [  0,  0,  0,  0,  0,  0 ] }
        ]
      }
    ]
  },
  {
    id: "turn",
    sets: [
      {
        id: "left",
        isLoop: false,
        isInterruption: true,
        lapse: 300,
        sequence: [
          { moves: [  0,-35,-40,  0, 35, 37 ] },
          { moves: [  0, 35, 37,  0,-35,-40 ] },
          { moves: [-14, 35, 37, 20,-35,-40 ] },
          { moves: [-14, 35, 37, 20,  0,  0 ] },
          { moves: [ 20,  0,  0,-14,  0,  0 ] }
        ]
      },
      {
        id: "right",
        isLoop: false,
        isInterruption: true,
        lapse: 300,
        sequence: [
          { moves: [  0, 35, 37,  0,-35,-40 ] },
          { moves: [  0,-35,-40,  0, 35, 37 ] },
          { moves: [ 20,-35,-40,-14, 35, 37 ] },
          { moves: [ 20,  0,  0,-14, 35, 37 ] },
          { moves: [-14,  0,  0, 20,  0,  0 ] },
        ]
      }
    ]
  },
  {
    id: "correct",
    sets: [
      {
        id: "left",
        isLoop: false,
        isInterruption: true,
        lapse: 250,
        sequence: [
          // { moves: [  0,  0,  0,  0,  0,  0 ] },
          { moves: [ 35,  0,  0,-18,  0,  0 ] },
          { moves: [], lapse: 100 },
          { moves: [  2,-20,-20,-18, 20, 20 ] },
          { moves: [  0,-20,-20,  0, 20, 20 ] },
          { moves: [  0,  0,  0,  0,  0,  0 ] },
        ]
      },
      {
        id: "right",
        isLoop: false,
        isInterruption: true,
        lapse: 250,
        sequence: [
          // { moves: [  0,  0,  0,  0,  0,  0 ] },
          { moves: [-18,  0,  0, 35,  0,  0 ] },
          { moves: [], lapse: 100 },
          { moves: [-18, 20, 20,  2,-20,-20 ] },
          { moves: [  0, 20, 20,  0,-20,-20 ] },
          { moves: [  0,  0,  0,  0,  0,  0 ] },
        ]
      }
    ]
  },
  {
    id: "home",
    sets: [
      {
        id: "stand",
        isLoop: false,
        isInterruption: true,
        lapse: 500,
        sequence: [
          { moves: [ 0, 0, 0, 0, 0, 0 ] }
        ]
      },
      {
        id: "rest",
        isLoop: false,
        isInterruption: true,
        lapse: 500,
        sequence: [
          { moves: [ 0, 35, 35, 0, 35, 35 ] }
        ]
      }
    ]
  }
];

var paths = [ "left", "right" ];

var looks = {
  left: 50,
  center: 90,
  right: 130,
  which: "center"
};

var redirects = {
  left: "right",
  center: null,
  right: "left"
};

var Scanner = {
  lookingAt: "center",
  isSafe: true,
  isObserving: false,
  isThinking: false,
  get isScanning() {
    return this.data.isScanning;
  },
  set isScanning(value) {
    if (value) {
      this.reset();
    }
    this.data.isScanning = value;
  },
  data: {
    isScanning: false,
    distance: 10,
    samples: []
  },
  reset: function() {
    this.data.samples.length = 0;
  },
  process: function(value) {
    if (value < this.data.distance) {
      this.data.samples.push(value);

      if (this.data.samples.length >= 5) {
        this.data.samples.length = 0;
        this.isSafe = false;
        return;
      }
    } else {
      this.data.samples.length = 0;
    }
    this.isSafe = true;
  }
};


var isRightfirst = false;
var isWalking = false;

function walk(direction, speed) {
  var sequence;
  var set = gaits[direction].sets[speed < 600 ? 1 : 0];
  var lapse = set.lapse;

  if (isWalking) {
    return;
  }

  isWalking = true;

  sequence = isRightfirst ?
    set.sequence.slice(0, 2) :
    set.sequence.slice(2);

  // Flip the side bit for the next run.
  isRightfirst = !isRightfirst;

  still();

  var queue = sequence.map(function(relative) {
    // var lapse = relative.lapse | 0;
    var absolute;

    // Convert the relative movement values
    // to absolute servo positions.
    absolute = relative.moves.map(function(value, i) {
      return value + starts[i];
    });

    return {
      wait: lapse,
      task: function() {
        if (absolute.length) {
          move(absolute, lapse);
        }
      }
    };
  });

  queue.push({
    wait: 10,
    task: function() {
      isWalking = false;
    }
  });

  tasks.walk = temporal.queue(queue);
}

function turn(direction) {
  var sequence;
  var set = gaits[3].sets[direction];
  var lapse = set.lapse;

  if (isTurning) {
    return;
  }

  isTurning = true;

  sequence = set.sequence;

  Scanner.lookingAt = direction ? "right" : "left";

  still();
  // if (tasks.walk) {
  //   tasks.walk.stop();
  // }

  // if (tasks.turn) {
  //   tasks.turn.stop();
  // }


  var queue = sequence.map(function(relative) {
    // var lapse = relative.lapse | 0;
    var absolute;

    // Convert the relative movement values
    // to absolute servo positions.
    absolute = relative.moves.map(function(value, i) {
      return value + starts[i];
    });

    return {
      wait: lapse,
      task: function() {
        if (absolute.length) {
          move(absolute, lapse);
        }
      }
    };
  });

  queue.push({
    wait: 10,
    task: function() {
      isTurning = false;
    }
  });

  tasks.turn = temporal.queue(queue);
}

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: 10
  });

  // Leg initialization
  legs = inits.reduce(function(accum, init) {
    accum[init.id] = new five.Servo(init);
    return accum;
  }, {});

  function retreat() {
    Scanner.isScanning = true;
    Scanner.isPanning = true;

    controls.rev.long(function() {
      if (eyes.inches < 10) {
        retreat();
      } else {
        Scanner.isPanning = false;
        Scanner.isScanning = true;
        Scanner.reset();

        controls.fwd.long();
      }
    });
  }

  function safeCourse(callback) {

    function look() {
      neck.to(range[dir], 250);
    }

    var dir = 0;
    var readings = [ /* left, right */ ];
    var data = {};
    var range = [ 20, 160 ];
    var onComplete = function(data) {
      if (data !== null) {
        Scanner.isThinking = false;
      }
      if (typeof callback === "function") {
        callback(data);
      }
    };

    if (Scanner.isThinking || Scanner.isPanning) {
      onComplete(null);
      return;
    }

    Scanner.isThinking = true;

    neck.on("move:complete", function moveComplete() {
      var sorted, safest;

      readings.push(eyes.inches);

      if (readings.length < 2) {
        dir ^= 1;
        process.nextTick(look);
      } else {
        neck.removeListener("move:complete", moveComplete);

        data[readings[0]] = "left";
        data[readings[1]] = "right";

        sorted = readings.sort(function(a, b) {
          return a > b;
        });

        safest = data[sorted[1]];

        onComplete(data, safest);
      }
    });

    process.nextTick(look);
  }


  eyes.on("data", function() {

    if (Scanner.isThinking || Scanner.isObserving) {
      return;
    }

    Scanner.process(this.inches);

    if (!Scanner.isSafe) {
      console.log( "COLLISION IMMINENT" );

      still();

      Scanner.isObserving = true;

      controls.home.rest(function() {

        safeCourse(function(data, safest) {
          // var readings = Object.keys(data).map(Number).sort(function(a, b) {
          //   return a > b;
          // });
          // var safest = data[readings[1]];

          Scanner.lookingAt = safest;

          controls.correct[safest](function() {
            neck.center();
            Scanner.isObserving = false;
            Scanner.isScanning = true;
          });
        });
      });
    } else {

      if (Scanner.isScanning) {
        // console.log( "TURN TOWARDS: ", Scanner.lookingAt );

        Scanner.isObserving = true;
        Scanner.isScanning = false;

        var correction = Scanner.lookingAt;

        controls.correct[correction](function() {

          console.log( "CORRECTED: ", correction );

          neck.to(90, 50).once("move:complete", function() {
            if (eyes.inches > 10) {
              console.log( "COLLISION: AVOIDED" );

              Scanner.lookingAt = "center";
              Scanner.isObserving = false;
              Scanner.isScanning = false;
              Scanner.reset();

              controls.fwd.long();
            } else {
              retreat();
            }
          });
        });
      }
    }
  });

  var controls = gaits.reduce(function(controls, gait) {
    controls[gait.id] = gait.sets.reduce(function(seq, set) {

      // Create an API from gait.set
      //
      seq[set.id] = function(opts, callback) {
        var noLoop;

        if (typeof opts === "function") {
          callback = opts;
          opts = {};
        }

        opts = opts || {};

        noLoop = opts.noLoop;

        var hash = [gait.id, set.id].join(":");

        // console.log( "set.isInterruption", set.isInterruption );
        if (set.isInterruption) {
          isRightfirst = false;
          isWalking = false;
          isMoving = false;
          currently = null;
        }

        // Allow interruptions
        if (currently !== hash) {
          currently = hash;
          isMoving = false;
        }

        // If `isMoving` is currently `true`, bail out
        // to avoid pile up.
        if (isMoving) {
          return;
        }

        // If `isMoving` is currently `false`,
        // set it to `true` to avoid pile up
        isMoving = true;

        still();

        var queue = set.sequence.map(function(relative) {
          var lapse = set.lapse;
          var absolute;

          if (relative.lapse) {
            lapse = relative.lapse;
          }

          // Convert the relative movement values
          // to absolute servo positions.
          absolute = relative.moves.map(function(value, i) {
            return value + starts[i];
          });

          return {
            wait: lapse,
            task: function() {
              if (absolute.length) {
                move(absolute, lapse);
              }
            }
          }
        });

        if (set.isLoop || opts.isLoop) {
          queue.push({
            wait: 10,
            task: function() {
              isMoving = false;

              if (callback) {
                callback(set.id);
              } else {
                controls[gait.id][set.id]();
              }
            }
          });
        } else {
          queue.push({
            wait: 10,
            task: function() {
              isMoving = false;

              if (callback) {
                callback(set.id);
              }
            }
          });
        }

        tasks.controls = temporal.queue(queue);
      };

      return seq;
    }, {});

    return controls;
  }, {});

  var deadzone = {
    low: 512 - 50,
    high: 512 + 50
  };

  var left = {
    isActivated: false,
    x: 0,
    y: 0
  };

  var right = {
    isActivated: false,
    x: 0,
    y: 0
  };

  temporal.loop(100, function() {

    if (right.isActivated) {
      if (right.y > deadzone.high || right.y < deadzone.low) {
        walk(
          right.y > 512 ? 1 : 0, 750 - (Math.abs(right.y - 512) / 512 * 250)
        );
      } else {
        if (tasks.walk) {
          tasks.walk.stop();
          isWalking = false;
        }
      }
    }

    if (left.isActivated) {
      if (left.x > deadzone.high || left.x < deadzone.low) {
        turn(left.x > 512 ? 1 : 0);
      } else {
        if (tasks.turn) {
          tasks.turn.stop();
          isTurning = false;
        }
      }
    }
  });

  controller.on("connected", function() {
    controller.isConnected = true;
  });

  controller.on("leftAnalogBump:press", function() {
    left.isActivated = !left.isActivated;
  });

  controller.on("rightAnalogBump:press", function() {
    right.isActivated = !right.isActivated;
  });

  controller.on("left:move", function(position) {
    left.x = scale(position.x, 0, 255, 0, 1024) | 0;
    left.y = scale(position.y, 255, 0, 0, 1024) | 0;

    if (tasks.scanner) {
      tasks.scanner.stop();
    }

    var degrees = scale(position.x, 0, 255, 20, 160) | 0;

    Scanner.lookingAt = degrees > 90 ? "right" : "left";
    Scanner.isObserving = false;
    neck.to(degrees);
  });

  controller.on("right:move", function(position) {
    right.x = scale(position.x, 0, 255, 0, 1024) | 0;
    right.y = scale(position.y, 255, 0, 0, 1024) | 0;
  });

  controller.on("circle:press", function() {
    still();
    controls.home.rest();
  });

  controller.on("triangle:press", function() {
    still();
    controls.home.stand();
  });

  controller.on("l1:press", function() {
    controls.correct.left();
  });

  controller.on("r1:press", function() {
    controls.correct.right();
  });

  controller.on("l2:press", function() {
    controls.action.lkick();
  });

  controller.on("r2:press", function() {
    controls.action.rkick();
  });

  controller.on("dpadLeft:press", function() {
    controls.correct.left();
  });

  controller.on("dpadRight:press", function() {
    controls.correct.right();
  });

  controller.on("dpadUp:press", function() {
    controls.fwd.long();
  });

  controller.on("dpadDown:press", function() {
    controls.rev.long();
  });

  controller.connect();

  this.repl.inject({

    Scanner: Scanner,
    safeCourse: safeCourse,

    get tasks() {
      return tasks;
    },
    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;

      tasks.dance = temporal.loop(500, function() {

        if (direction) {
          feet.to(150, 500);
        } else {
          feet.to(90, 500);
        }

        direction = direction ? 0 : 1;
      });
    }
  });

  controls.home.rest();

  // setTimeout(function() {
  //   safeCourse(function(data) {
  //     console.log( data );
  //   });
  // }, 500);

  // controls.fwd.long();
});