biped-preset-controls.js

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

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 legs = inits.reduce(function(accum, init) {
  accum[init.id] = new five.Servo(init);
  return accum;
}, {});

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


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: [
      //  [ rf, rk, rh, lf, lk, lh ]
          { 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: [
      //  [ rf, rk, rh, lf, lk, lh ]
          { 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: [
      //  [ rf, rk, rh, lf, lk, lh ]
          { 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: [
      //  [ rf, rk, rh, lf, lk, lh ]
          { 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 state = {
  task: null, 
  isMoving: false
};

var isMoving = false;

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

    seq[set.id] = function(opts, callback) {
      var noLoop;

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

      opts = opts || {};

      noLoop = opts.noLoop;

      if (set.isInterruption) {
        state.isMoving = false;
      }

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

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

      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() {
            state.isMoving = false;

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

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

      state.task = temporal.queue(queue);
    };

    return seq;
  }, {});

  return controls;
}, {});


// Creates the following APIs:
presets.rev.short();
presets.rev.long();

presets.fwd.short();
presets.fwd.long();

presets.action.lkick();
presets.action.rkick();

presets.turn.left();
presets.turn.right();

presets.correct.left();
presets.correct.right();

presets.home.rest();
presets.home.stand();