johnny-five/wip/scout.js

The JavaScript Arduino Programming Framework.

var five = require("../lib/johnny-five.js");
var exec = require("child_process").exec;
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;

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

// { pin:  2, id: "lf", startAt: 90 },
// { pin:  3, id: "la", startAt: 90 },
// { pin:  4, id: "lk", startAt: 90 },
// { pin:  5, id: "lt", startAt: 90 },
// { pin:  6, id: "lr", startAt: 90 },
// { pin:  7, id: "ly", startAt: 90 },

// { pin:  8, id: "rf", startAt: 90 },
// { pin:  9, id: "ra", startAt: 90 },
// { pin: 10, id: "rk", startAt: 90 },
// { pin: 11, id: "rt", startAt: 90 },
// { pin: 12, id: "rr", startAt: 90 },
// { pin: 13, id: "ry", startAt: 90 },

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

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

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

var legs = {};

var inits = [
  { pin: 16, id: "rf", startAt: 90, isInverted: true },
  { pin: 17, id: "ra", startAt: 90 },
  { pin: 18, id: "rk", startAt: 90, isInverted: true },
  { pin: 19, id: "rt", startAt: 90, isInverted: true },
  { pin: 20, id: "rr", startAt: 90, isInverted: true },
  { pin: 21, id: "ry", startAt: 90, isInverted: true },

  { pin: 32, id: "lf", startAt: 90 },
  { pin: 33, id: "la", startAt: 90 },
  { pin: 34, id: "lk", startAt: 90 },
  { pin: 35, id: "lt", startAt: 90 },
  { pin: 36, id: "lr", startAt: 90 },
  { pin: 37, id: "ly", startAt: 100 }
];

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

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: [
          { 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: "lean",
    sets: [
      {
        id: "both",
        isLoop: false,
        isInterruption: true,
        lapse: 250,
        sequence: [
          { moves: [ -15, 0, 0, 0, 0, 26, 0, 0, 0, 0 ] },
          // { moves: [ 0, 0, 0, 0, 0, -20, 0, 0, 0, 0 ] },
          // { moves: [-20, 0, 0, 0, 0, -16, 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, 0, 0, 0, 0, 0, 0 ] }
        ]
      },
      {
        id: "rest",
        isLoop: false,
        isInterruption: true,
        lapse: 500,
        sequence: [
          { moves: [
              0, 50, 55, -24, 0, 0,
              0, 50, 55, -24, 0, 0
            ]
          }
        ]
      }
    ]
  }
];


board.on("ready", function() {
  legs = inits.reduce(function(accum, init) {
    accum[init.id] = new five.Servo(init);
    return accum;
  }, {});


  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 labels = [ "Foot", "Ankle", "Knee", "Thigh", "Hip Roll", "Hip Yaw" ];

  var Group = {
    stash: {
      side: -1,
      index: 0
    },
    isGrouped: false,
    get side() {
      return Group.stash.side;
    },
    set side(value) {
      // -1 = both, 0 = right, 1 = left
      var which = value === -1 ?
        "both" : (value ? "right" : "left");

      console.log(
        "------- %s %s -------",
        which.toUpperCase(),
        labels[Group.stash.index].toUpperCase()
      );

      Group.stash.side = value;
    },
    get index() {
      return Group.stash.index;
    },
    set index(value) {

      console.log(
        "Controlling %s", this.sets[value].name
      );

      Group.stash.index = value;
    },
    sets: [
      { name: "Feet",
        // pins: [ 2, 8 ],
        parts: [ "lf", "rf" ],
        values: [ inits[6].startAt, inits[0].startAt ]
      },
      { name: "Ankles",
        // pins: [ 3, 9 ],
        parts: [ "la", "ra" ],
        values: [ inits[7].startAt, inits[1].startAt ]
      },
      { name: "Knees",
        // pins: [ 4, 10 ],
        parts: [ "lk", "rk" ],
        values: [ inits[8].startAt, inits[2].startAt ]
      },
      { name: "Thighs",
        // pins: [ 5, 11 ],
        parts: [ "lt", "rt" ],
        values: [ inits[9].startAt, inits[3].startAt ]
      },
      { name: "Hip Roll",
        // pins: [ 7, 13 ],
        parts: [ "lr", "rr" ],
        values: [ inits[10].startAt, inits[4].startAt ]
      },
      { name: "Hip Yaw",
        // pins: [ 7, 13 ],
        parts: [ "ly", "ry" ],
        values: [ inits[11].startAt, inits[5].startAt ]
      }
    ],


  // { pin: 40, id: "rf", startAt: 89, isInverted: true },
  // { pin: 41, id: "ra", startAt: 92, isInverted: true },
  // { pin: 42, id: "rk", startAt: 86, isInverted: true },
  // { pin: 43, id: "rt", startAt: 96, isInverted: true },
  // { pin: 44, id: "rr", startAt: 90, isInverted: true },
  // { pin: 45, id: "ry", startAt: 90, isInverted: true },

  // { pin: 24, id: "lf", startAt: 95 },
  // { pin: 25, id: "la", startAt: 88 },
  // { pin: 26, id: "lk", startAt: 89 },
  // { pin: 27, id: "lt", startAt: 90 },
  // { pin: 28, id: "lr", startAt: 90 },
  // { pin: 29, id: "ly", startAt: 100 }

  // { pin:  7, id: "rf", startAt: 89, isInverted: true },
  // { pin:  8, id: "ra", startAt: 92, isInverted: true },
  // { pin:  9, id: "rk", startAt: 86, isInverted: true },
  // { pin: 10, id: "rt", startAt: 96, isInverted: true },
  // { pin: 11, id: "rh", startAt: 90, isInverted: true },

  // { pin:  2, id: "lf", startAt: 95 },
  // { pin:  3, id: "la", startAt: 88 },
  // { pin:  4, id: "lk", startAt: 89 },
  // { pin:  5, id: "lt", startAt: 90 },
  // { pin:  6, id: "lh", startAt: 100 }

    toData: function() {
      var right = [];
      var left = [];

      this.sets.forEach(function(set) {
        left.push(set.values[0]);
        right.push(set.values[1]);
      });

      return right.concat(left);
    }
  };

// console.log( "" );
// console.log( "starts: ", starts );
// console.log( "toData: ", Group.toData() );




  function tuner(step) {
    // Cannot control both at a time
    if (Group.side === -1) {
      return;
    }

    var set = Group.sets[Group.index];
    var part = set.parts[Group.side];
    var value = set.values[Group.side];

    if (value === null) {
      value = legs[part].startAt;
    }

    if (step < 0) {
      value--;
    } else {
      value++;
    }

    process.nextTick(function() {
      legs[part].to(value);
    });

    set.values[Group.side] = value;
  }

  function aMove(y, which) {
    var set = Group.sets[Group.index];
    var part, pos;

    if (Group.isGrouped) {
      set.parts.forEach(function(part, i) {
        var pos = degrees(legs[part].startAt, y) | 0;

        legs[part].to(pos);
        set.values[i] = pos;
      });
    } else {
      part = set.parts[which];
      pos = degrees(legs[part].startAt, y) | 0;

      legs[part].to(pos);
      set.values[1] = pos;
    }
  }

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

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

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


  console.log( "argv.calibrate?", argv.calibrate );
  // Calibration Mode
  //
  if (argv.calibrate) {

    controller.on("start:press", function() {
      var absolute = Group.toData();
      var relative = absolute.map(function(value, i) {
        return value - starts[i];
      });

      console.log("---------------------------");
      console.log("Absolute: ", Group.toData());
      console.log("Relative: ", relative);
    });

    controller.on("square:press", function() {
      var which = Group.side === -1 ?
        "both" : (Group.side ? "right" : "left");
      var part = Group.sets[Group.index].name;
      var say = ["Controlling ", which, part].join(" ");

      console.log(say);

      exec("say " + say);
    });

    controller.on("triangle:press", function() {
      labels[Group.stash.index]

      var values = Group.sets[Group.index].values;
      var which = labels[Group.stash.index].toLowerCase();
      var say = "";

      if (Group.side === -1) {
        // do both
        values.forEach(function(value, i) {
          var part = Group.sets[Group.index].parts[i];
          var side = part[0] === "r" ? "right" : "left";

          if (value === null) {
            value = "starting";
          }

          say += [side, which, "is at", value, "degrees"].join(" ") + ", ";
        });
      } else {
        // do side
        var part = Group.sets[Group.index].parts[Group.side]
        var value = Group.sets[Group.index].values[Group.side];
        var side = part[0] === "r" ? "right" : "left";

        if (value === null) {
          value = "starting";
        }

        say = [side, which, "is at", value, "degrees"].join(" ");
      }

      exec("say " + say);
    });

    controller.on("x:press", function() {
      Group.isGrouped = !Group.isGrouped;
    });

    controller.on("circle:press", function() {
      Group.side = -1;
    });

    controller.on("dpadLeft:press", function() {
      Group.side = 0;
    });

    controller.on("dpadRight:press", function() {
      Group.side = 1;
    });

    controller.on("dpadUp:press", function() {
      var next = Group.index + 1;

      if (next === Group.sets.length) {
        next = 0;
      }

      Group.index = next;
    });

    controller.on("dpadDown:press", function() {
      var next = Group.index - 1;

      if (next === -1) {
        next = Group.sets.length - 1;
      }

      Group.index = next;
    });

    ["left:move", "right:move"].forEach(function(type) {
      controller.on(type, function(position) {
        if (controller.isConnected) {
          aMove(position.y, type === "left:move" ? 0 : 1);
        }
      });
    });

    // controller.on("left:move", function(position) {
    //   if (controller.isConnected) {
    //     aMove(position.y);
    //   }
    // });

    // controller.on("right:move", function(position) {
    //   if (controller.isConnected) {
    //     aMove(position.y);
    //   }
    // });

    controller.on("l1:press", function() {
      tuner(-1);
    });

    controller.on("r1:press", function() {
      tuner(1);
    });



  }

  this.repl.inject({
    rest: controls.home.rest,
    controls: controls,
    Group: Group,
    legs: legs,
    thighs: new five.Servos([legs.rt, legs.lt]),
    knees: new five.Servos([legs.rk, legs.lk]),
    ankles: new five.Servos([legs.ra, legs.la]),
    feet: new five.Servos([legs.rf, legs.lf])
  });

  process.nextTick(function() {
    controller.isConnected = true;
    controller.connect();
  });

});