johnny-five/wip/brat-pre-refactor.js

The JavaScript Arduino Programming Framework.

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

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

  if (tasks.walk) {
    tasks.walk.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() {
      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;

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

console.log( queue );
  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: 50
  });

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

  // Groupings
  all = new five.Servos();
  hips = new five.Servos([ legs.rh, legs.lh ]);
  knees = new five.Servos([ legs.rk, legs.lk ]);
  feet = new five.Servos([ legs.rf, legs.lf ]);


  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) {

        opts = opts || {};

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

        // console.log( hash, isMoving );

        // 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;

        // If any other task is active, stop it.
        if (tasks.walk) {
          tasks.walk.stop();
        }

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

        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;
              controls[gait.id][set.id]();
            }
          });
        } else {
          queue.push({
            wait: 10,
            task: function() {
              isMoving = false;
            }
          });
        }

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

      return seq;
    }, {});

    return controls;
  }, {});


  function scan() {
    var direction = 1;

    if (argv.noscan) {
      return;
    }

    isScanning = true;

    neck.center();

    tasks.scanner = temporal.loop(600, function() {
      neck.to(neck.range[(direction ^= 0x01)], 600);
    });
  }

  var last = {
    control: "long",
    direction: "rev",
    heading: 0
  };

  console.log( "Scanning: %s", !argv.noscan );

  var reads = 0;
  var count = 0;
  var scannerAt = null;

  eyes.on("data", function() {
    var direction, control;

    var heading = neck.last.degrees;

    if (argv.noscan) {
      return;
    }

    reads++;

    if (this.inches < 10) {
      if (!count && isScanning) {
        console.log( "STOPPING SCANNER" );
        isScanning = false;
        tasks.scanner.stop();

        reads = 1;
        scannerAt = heading;

        temporal.wait(2000, function() {
          if (!isScanning) {
            reads = 0;
            count = 0;
            scan();
          }
        });
      }
      if (!isScanning) {
        if (Math.abs(scannerAt - heading) < 5) {
          count++;
          console.log( "IN VIEW", reads, count );
        }
      }
    }

    //  else {
    //   if (reads > 0 && (count > 0 || count < 3)) {
    //     scannerAt = null;
    //     count = 0;
    //     reads = 0;
    //   }
    // }

    if (reads !== count && !isScanning) {
      scan();
    }

    if (reads === 3 && count === 3) {
      scannerAt = null;
      reads = 0;
      count = 0;

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

      neck.center();

      if (isAvoiding) {
        return;
      }

      if (isTurning) {
        return;
      }

      if (heading < 90) {
        // left
        direction = "turn";
        control = "right";
      } else {
        // right
        direction = "turn";
        control = "left";
      }

      if (heading < 110 && heading > 70) {
        direction = "fwd";
        control = "long";
      }

      console.log(
        "DANGER: %d inches @ %d degrees (%s)",
          this.inches, heading | 0, control.toUpperCase()
      );

      isTurning = true;
      isAvoiding = true;
      isScanning = false;

      if (control !== "long") {
        console.log( "TURNING: ", control );
      }

      controls[cinvert[direction]][control]();

      last.control = control;
      last.direction = direction;
      last.heading = heading;

      temporal.delay(2000, function() {
        isTurning = false;
      });
    } else {

      if (isAvoiding && !isTurning) {
        console.log(
          "RESUME: %d", this.inches
        );

        isAvoiding = false;
        controls.fwd.long();

        scan();
      } else {
        // ...
      }
    }
  });


  // Calibration mode utilities

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

  var labels = [ "Foot", "Knee", "Hip" ];

  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: [
      // Feet
      { name: "Feet",
        pins: [ 7, 10 ],
        parts: [ "lf", "rf" ],
        values: [ null, null ]
      },
      // Knees
      { name: "Knees",
        pins: [ 8, 11 ],
        parts: [ "lk", "rk" ],
        values: [ null, null ]
      },
      // Hips
      { name: "Hips",
        pins: [ 9, 12 ],
        parts: [ "lh", "rh" ],
        values: [ null, null ]
      }
    ]
  };

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


  // Calibration Mode
  //
  if (argv.calibrate) {

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

      console.log(
        "Controlling %s %s", which, part
      );
    });

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


    controller.on("left:move", function(position) {
      var part = Group.sets[Group.index].parts[0];
      var pos = degrees(legs[part].startAt, position.y);

      legs[part].to(pos);
    });

    controller.on("right:move", function(position) {
      var part = Group.sets[Group.index].parts[1];
      var pos = degrees(legs[part].startAt, position.y);

      legs[part].to(pos);
    });

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

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

  } else {

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

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

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

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


      neck.to(scale(position.x, 0, 255, 20, 160) | 0);
    });

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

    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("circle:press", function() {
      controls.home.rest();
    });

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

    controller.on("square:press", function() {
      if (!isScanning) {
        scan();
      }
    });

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

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

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

    controller.on("r2:press", function(position) {
      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({
    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(scan, 1000);

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