johnny-five/wip/quadruped.js

The JavaScript Arduino Programming Framework.

var five = require("../lib/johnny-five.js");
var keypress = require("keypress");
var temporal = require("temporal");
var configs = [
  { pin: 8, isInverted: true, range: [ 35, 145 ], node: "rf", part: "coxa" },
  { pin: 9, isInverted: true, range: [ 35, 145 ], node: "rf", part: "femur" },
  { pin: 10, isInverted: false, range: [ 35, 145 ], node: "rf", part: "tibia" },

  { pin: 2, isInverted: true, range: [ 35, 145 ], node: "rb", part: "coxa" },
  { pin: 3, isInverted: false, range: [ 35, 145 ], node: "rb", part: "femur" },
  { pin: 4, isInverted: true, range: [ 35, 145 ], node: "rb", part: "tibia" },

  { pin: 5, isInverted: true, range: [ 35, 145 ], node: "lb", part: "coxa" },
  { pin: 6, isInverted: true, range: [ 35, 145 ], node: "lb", part: "femur" },
  { pin: 7, isInverted: false, range: [ 35, 145 ], node: "lb", part: "tibia" },

  { pin: 11, isInverted: true, range: [ 35, 145 ], node: "lf", part: "coxa" },
  { pin: 12, isInverted: false, range: [ 35, 145 ], node: "lf", part: "femur" },
  { pin: 13, isInverted: true, range: [ 35, 145 ], node: "lf", part: "tibia" }
];
/*

rf, RF, Right Front = 1;

rb, RB, Right Back  = 2;

lb, LB, Left Back   = 3;

lf, LF, Left Front  = 4;
*/

function scale() {
  return Math.round(
    five.Fn.scale.apply(null, arguments)
  );
}

var siblings = {
  lf: { r: "rf", l: "lb" },
  rf: { r: "rb", l: "lf" },
  lb: { r: "lf", l: "rb" },
  rb: { r: "lb", l: "rf" }
};

// Limb measurements
var coxa = 29;
var femur = 57;
var tibia = 122.40;

// Body measurements
var center = {
  y: 3,
  x: 4
};

// Can't wait for destructuring
var atan = Math.atan;
var acos = Math.acos;
var sqrt = Math.sqrt;
var pow = Math.pow;
var cos = Math.cos;

// coxa
function gamma(x, y, a) {
  var g = atan(x / y);
  return g * rad + 90;
}

// femur
function beta(x, y, a) {
  var g = atan(x / y);
  x /= cos(g);
  var bet = acos(
    (pow(a, 2) + pow(y - coxa, 2) - pow(tibia, 2) - pow(femur, 2)) /
    (-2 * femur * tibia)
  );
  return bet * rad;
}

// tibia
function alpha(x, y, a) {
  var g = atan(x / y);
  x /= cos(g);
  var l = sqrt(pow(a, 2) + pow(y - coxa, 2));
  var alph1 = atan((y - coxa) / a);
  var alph2 = acos(
    (pow(tibia, 2) - pow(femur, 2) - pow(l, 2)) /
    (-2 * femur * l)
  );
  var alp = alph1 + alph2;
  return alp * rad;
}


         // smoothFullMove( 2, -1,  z, 2, 1, z, steps * 2, time );
// function smoothFullMove(x0, y0, z0, x, y, z,        t1, steps) {
//   var dx = (x - x0) / steps;
//   var dy = (y - y0) / steps;
//   var dz = (z - z0) / steps;
//   for (var i = 0; i < steps; i++) {
//     x0 += dx;
//     y0 += dy;
//     z0 += dz;
//     fullMove(x0, y0, z0);
//     delay(t1);
//   }
// }

function fullMove(x, y, z) {

  var rightY = center.y - y;
  var leftY = center.y + y;

  var frontX = x - center.x;
  var backX = x + center.x;

  RFx = x - center.x;
  RFy = center.y - y;
  RFz = z;

  RBx = x + center.x;
  RBy = center.y - y;
  RBz = z;

  LFx = x - center.x;
  LFy = center.y + y;
  LFz = z;

  LBx = x + center.x;
  LBy = center.y + y;
  LBz = z;

  moveServos(
    "rf", gamma(RFx, RFy, RFz), alpha(RFx, RFy, RFz), beta(RFx, RFy, RFz)
  );
  moveServos(
    "rb", gamma(RBx, RBy, RBz), alpha(RBx, RBy, RBz), beta(RBx, RBy, RBz)
  );
  moveServos(
    "lf", gamma(LFx, LFy, LFz), alpha(LFx, LFy, LFz), beta(LFx, LFy, LFz)
  );
  moveServos(
    "lb", gamma(LBx, LBy, LBz), alpha(LBx, LBy, LBz), beta(LBx, LBy, LBz)
  );

}

var normalize = {
  rb: function(g, a, b) {
    g = g;
    a = 180 - a + 10;
    b = b + 10;
    return {
      g: g, a: a, b: b
    };
  },

  rf: function(g, a, b) {
    g = g;
    a = 180 - a + 10;
    b = b + 10;
    return {
      g: g, a: a, b: b
    };
  },

  lb: function(g, a, b) {
    g = 180 - g - 10;
    a = 180 - a + 5;
    b = b + 5;
    return {
      g: g, a: a, b: b
    };
  },

  lf: function(g, a, b) {
    // g = 180 - g + 5;
    // a = 180 - a - 7;

    g = 180 - g;
    a = 180 - a;

    // b = b + 15;
    return {
      g: g, a: a, b: b
    };
  }
};


function moveServos(which, gamma, alpha, beta) {
  var normal = normalize[which](gamma, alpha, beta);
  // console.log( servos[which] );

  console.log( normal );

  servos[which].coxa.to(normal.g, 1000);
  servos[which].tibia.to(normal.a, 1000);
  servos[which].femur.to(normal.b, 1000);
}

(new five.Board()).on("ready", function() {

  servos = configs.reduce(function(accum, config) {
    if (!accum[config.node]) {
      accum[config.node] = {};
    }

    // accum[config.node][config.part] = new five.Servo(config.pin)[config.init]();
    accum[config.node][config.part] = new five.Servo({
      pin: config.pin,
      isInverted: config.isInverted
    }).center();

    return accum;
  }, {});

  var all = new five.Servos();
  var rf = servos.rf;
  var lf = servos.lf;
  var rb = servos.rb;
  var lb = servos.lb;

  var leg = 0;

  function kozachok() {
    temporal.queue([
      {
        delay: 0,
        task: function() {
          while (++leg % 3 !== 0);

          if (leg >= configs.length) {
            leg = 0;
          }

          servos[configs[leg].node].femur.to(90, 100);
          servos[configs[leg].node].tibia.to(90, 100);
        }
      },
      {
        delay: 500,
        task: squat
      },
      {
        delay: 0,
        task: squat
      },
      {
        delay: 1000,
        task: function() {
          while (++leg % 3 !== 0);

          if (leg >= configs.length) {
            leg = 0;
          }

          servos[configs[leg].node].femur.to(90, 100);
          servos[configs[leg].node].tibia.to(90, 100);
        }
      },
      {
        delay: 250,
        task: stand
      },
      {
        delay: 600,
        task: kozachok
      }
    ]);
  }


  function squat() {
    rf.tibia.to(140, 500);
    rb.tibia.to(140, 500);
    lb.tibia.to(140, 500);
    lf.tibia.to(140, 500);

    rf.femur.to(45, 500);
    rb.femur.to(45, 500);
    lb.femur.to(45, 500);
    lf.femur.to(45, 500);
  }

  function stand() {
      // this.straight();

    rf.femur.to(120, 500);
    rf.tibia.to(60, 500);

    rb.femur.to(120, 500);
    rb.tibia.to(60, 500);

    lb.femur.to(120, 500);
    lb.tibia.to(60, 500);

    lf.femur.to(120, 500);
    lf.tibia.to(60, 500);
  }

  function pushups() {
    temporal.queue([
      {
        delay: 0,
        task: squat
      },
      {
        delay: 600,
        task: stand
      },
      {
        delay: 600,
        task: pushups
      }
    ]);
  }

  function rotate(degrees) {
    rf.coxa.to(rf.coxa.position + degrees, 500);
    lf.coxa.to(lf.coxa.position + degrees, 500);
    rb.coxa.to(rb.coxa.position + degrees, 500);
    lb.coxa.to(lb.coxa.position + degrees, 500);
  }

  function straight() {
    all.to(90, 1000);
    // rf.coxa.to(90, 500);
    // lf.coxa.to(90, 500);
    // rb.coxa.to(90, 500);
    // lb.coxa.to(90, 500);

    // rf.femur.to(90, 500);
    // lf.femur.to(90, 500);
    // rb.femur.to(90, 500);
    // lb.femur.to(90, 500);

    // rf.tibia.to(90, 500);
    // lf.tibia.to(90, 500);
    // rb.tibia.to(90, 500);
    // lb.tibia.to(90, 500);
  }

  var limb = 0;
  var limbs = configs.reduce(function(accum, config) {
    if (accum.indexOf(config.node) === -1) {
      accum.push(config.node);
    }
    return accum;
  }, []);

  function flirt(which) {
    var isLoop = false;
    var right, left;

    if (!which) {
      isLoop = true;
      which = limbs[limb];

      if (++limb === 4) {
        limb = 0;
      }
    }

    this.squat();

    temporal.queue([
      {
        delay: 100,
        task: function() {

          var l = 130;
          var r = 70;

          right = servos[siblings[which].r];
          left = servos[siblings[which].l];

          right.coxa.to(r, 500);
          left.coxa.to(l, 500);
        }
      },
      {
        delay: 500,
        task: function() {
          var femur = 0;
          var range = [ 60, 90 ];

          servos[which].femur.to(femur, 500);
          servos[which].tibia.sweep(range);
        }
      },
      {
        delay: 2000,
        task: function() {
          servos[which].tibia.stop();
          stand()
        }.bind(this)
      },
      {
        delay: 500,
        task: function() {
          if (isLoop) {
            flirt();
          } else {
            straight()
          }
        }
      }
    ]);
  }

  function wave() {
    var which, right, left;

    which = limbs[limb];

    if (++limb === 4) {
      limb = 0;
    }

    if (!wave.inProgress) {
      this.squat();
      wave.inProgress = true;
    }

    var queue = temporal.queue([
      {
        delay: 100,
        task: function() {
          var l = 110;
          var r = 70;

          right = servos[siblings[which].r];
          left = servos[siblings[which].l];

          right.coxa.to(r, 500);
          left.coxa.to(l, 500);
        }
      },
      {
        delay: 500,
        task: function() {
          var femur = 0;
          var range = [ 60, 90 ];

          servos[which].femur.to(45, 250);
          servos[which].tibia.to(45, 250);
        }
      },
      {
        delay: 500,
        task: function() {
          wave();
        }
      }
    ]);
  }


  function walk(dir) {

    if (!dir) {
      dir = "fwd";
    }


    temporal.queue([
      {
        wait: 250,
        task: function() {
          lf.coxa.to(82, 250);
          lf.femur.to(111, 250);
          lf.tibia.to(103, 250);
          lb.coxa.to(98, 250);
          lb.femur.to(111, 250);
          lb.tibia.to(103, 250);
          rf.coxa.to(77, 250);
          rf.femur.to(70, 250);
          rf.tibia.to(77, 250);
          rb.coxa.to(103, 250);
          rb.femur.to(70, 250);
          rb.tibia.to(77, 250);

          console.log( "a" );
        }
      },
      {
        wait: 750,
        task: function() {
          if (dir) {
            lf.femur.to(0, 250);
            lf.coxa.to(130, 250);
          }

          console.log( "b" );
        }
      },
      {
        wait: 250,
        task: function() {
          lf.coxa.to(103, 250);
          lf.femur.to(70, 250);
          lf.tibia.to(77, 250);
          lb.coxa.to(77, 250);
          lb.femur.to(70, 250);
          lb.tibia.to(77, 250);
          rf.coxa.to(98, 250);
          rf.femur.to(111, 250);
          rf.tibia.to(103, 250);
          rb.coxa.to(82, 250);
          rb.femur.to(111, 250);
          rb.tibia.to(103, 250);

          console.log( "c" );
        }
      },
      {
        wait: 750,
        task: function() {
          if (dir) {
            rf.femur.to(0, 250);
            rf.coxa.to(50, 250);
          }


          console.log( "d" );
          walk();
        }
      }
    ]);
  }




  function lean(direction) {


    // var rf = rf;
    // var lf = lf;
    // var rb = rb;
    // var lb = lb;

    // rf.coxa.to(rf.coxa.position + degrees, 500);
    // lf.coxa.to(lf.coxa.position + degrees, 500);
    // rb.coxa.to(rb.coxa.position + degrees, 500);
    // lb.coxa.to(lb.coxa.position + degrees, 500);
  }


  // Inject a Servo Arbay into the REPL as "s"
  this.repl.inject({
    rf: rf,
    lf: lf,
    rb: rb,
    lb: lb,
    servos: servos,
    all: new five.Servos(),

    fm: fullMove,

    straight: function() {
      all.to(90, 500);
      // rf.coxa.to(90, 500);
      // lf.coxa.to(90, 500);
      // rb.coxa.to(90, 500);
      // lb.coxa.to(90, 500);

      // rf.femur.to(90, 500);
      // lf.femur.to(90, 500);
      // rb.femur.to(90, 500);
      // lb.femur.to(90, 500);

      // rf.tibia.to(90, 500);
      // lf.tibia.to(90, 500);
      // rb.tibia.to(90, 500);
      // lb.tibia.to(90, 500);
    },

    rotate: rotate,




    lean: {
      // right: function() {

      //   rf.coxa.to(70, 500);
      //   lf.coxa.to(70, 500);

      //   rb.coxa.to(90, 500);
      //   lb.coxa.to(70, 500);


      //   rf.femur.to(70, 500);
      //   lf.femur.to(180, 500);

      //   rb.femur.to(90, 500);
      //   lb.femur.to(70, 500);


      //   rf.tibia.to(70, 500);
      //   lf.tibia.to(70, 500);

      //   rb.tibia.to(90, 500);
      //   lb.tibia.to(70, 500);
      // },

      left: function() {

        rf.coxa.to(70, 500);
        lf.coxa.to(70, 500);

        rb.coxa.to(90, 500);
        lb.coxa.to(70, 500);


        rf.femur.to(70, 500);
        lf.femur.to(180, 500);

        rb.femur.to(90, 500);
        lb.femur.to(70, 500);




        rf.tibia.to(70, 500);
        lf.tibia.to(70, 500);

        rb.tibia.to(90, 500);
        lb.tibia.to(70, 500);

      }
    },

    flirt: flirt,

    kozachok: kozachok,

    pushups: pushups,

    squat: squat,

    stand: stand,

    wave: wave,

    walk: walk
  });

  var mode = null;
  var index = 0;
  var servo;



  // function controller( ch, key ) {
  //   var value = 0;
  //   var previous = mode;
  //   var operand = 1;
  //   var isCycling = false;


  //   if ( key ) {
  //     if (key.name === "r") {
  //       mode = "rotate";
  //     }

  //     if (key.name === "l") {
  //       mode = "lean";
  //     }

  //     if (key.name === "u") {
  //       // up(3, 5);
  //     }

  //     if (key.name === "s") {
  //       mode = "servo";
  //     }

  //     if (key.name === "z") {
  //       mode = null;
  //     }

  //     if (mode !== previous) {
  //       console.log( "Mode Changed: ", mode );
  //     }

  //     if (mode) {
  //       if (mode === "rotate") {
  //         value = key.name === "right" ? 1 : -1;

  //         if (key.shift) {
  //           value *= 10;
  //         }

  //         if (key.ctrl) {
  //           value *= 2;
  //         }

  //         rotate(value);
  //       }

  //       if (mode === "lean") {
  //         if (key.name === "right") {

  //         }
  //         value = key.name === "right" ? 1 : -1;

  //         if (key.shift) {
  //           value *= 10;
  //         }
  //       }

  //       if (mode === "servo") {

  //         if (key.name === "right") {

  //           if (key.shift) {
  //             while (++index % 3 !== 0);
  //           } else if (key.cntrl) {
  //             index += 3;
  //           } else {
  //             index++;
  //           }

  //           if (index >= configs.length) {
  //             index = 0;
  //           }

  //           isCycling = true;
  //         }

  //         if (key.name === "left") {
  //           if (key.shift) {
  //             while (--index % 3 !== 0);
  //           } else if (key.cntrl) {
  //             index -= 3;
  //           } else {
  //             index--;
  //           }

  //           if (index < 0) {
  //             index = configs.length - 1;

  //             if (key.shift) {
  //               index = 9;
  //             }
  //           }

  //           isCycling = true;
  //         }

  //         servo = servos[configs[index].node][configs[index].part];
  //         value = servo.position;

  //         if (key.shift) {
  //           operand = 10;
  //         }

  //         if (key.name === "up") {
//             value += operand;
  //         }

  //         if (key.name === "down") {
//             value -= operand;
  //         }

  //         if (isCycling) {
  //           console.log(
  //             "Switched to: %s %s", configs[index].node, configs[index].part
  //           );
  //         } else {
  //           console.log(
  //             "Moving: %s %s to %d",
  //             configs[index].node, configs[index].part, value
  //           );

  //           console.log( value );
  //           servo.to(value);
  //         }
  //       }


  //       // console.log( "action: ", mode, value, key );

  //       // modes[mode](value);
  //     }
  //   } else {
  //     // A number...


  //   }
  // }

  // keypress(process.stdin);

  // process.stdin.on("keypress", controller);
  // process.stdin.setRawMode(true);
  // process.stdin.resume();
});