johnny-five/wip/quadruped-2.js

Firmata based Arduino Programming Framework.

var temporal = require("temporal");

var PI = Math.PI;
var acos = Math.acos;
var atan2 = Math.atan2;
var sqrt = Math.sqrt;

var X = [ 114, 114 ];
var Z = [ 35, 60 ]; // 90


var limbs = {
  femur: {
    pos: 0,
    mm: 57
  },
  tibia: {
    pos: 0,
    mm: 118.67
  }
};

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

function sq(n) {
  return n * n;
}

function alpha(hypot, upper, lower) {
  return (acos((sq(upper) + sq(hypot) - sq(lower)) / (2 * upper * hypot))) * 180 / PI;
}

function beta(a, g) {
  return 90 - a + g;
}

function gamma(x, z) {
  return atan2(z, x) * 180 / PI;
}

function delta(hypot, a, b) {
  console.log( acos((sq(a) + sq(b) - sq(hypot)) / (2 * a * b)) );

  return (acos((sq(a) + sq(b) - sq(hypot)) / (2 * a * b))) * 180 / PI;
}

function calculate(upper, lower, x, z) {
  var hypot, a, b, g, d, e;

  hypot = sqrt(sq(x) + sq(z));
  a = alpha(hypot, upper, lower);
  g = gamma(x, z);
  b = beta(a, g);
  // console.log( "calling delta with ", hypot, a, b );
  d = delta(hypot, a, b);
  // console.log( "delta: ", d );
  e = 180 - d;

  return {
    hypot: hypot,
    alpha: a,
    beta: b,
    gamma: g,
    delta: d,
    epsilon: e,
    upper: scale(Math.round(b * 11 + 560), 600, 2400, 0, 180),
    lower: scale(Math.round(e * 11 + 560), 600, 2400, 0, 180),
  };
}

// temporal.loop(1600, function() {
//   temporal.repeat(2, 800, function(context) {
//     var index = context.called - 1;

//     var position = calculate(
//       limbs.femur.mm, limbs.tibia.mm, X[index], Z[index]
//     );


//     // moveservo (0, limbs.femur.pos, 800);
//     // moveservo (15, limbs.femur.pos, 800);
//     // moveservo (16, limbs.femur.pos, 800);
//     // moveservo (31, limbs.femur.pos, 800);

//     // moveservo (3, limbs.tibia.pos, 800);
//     // moveservo (12, limbs.tibia.pos, 800);
//     // moveservo (19, limbs.tibia.pos, 800);
//     // moveservo (28, limbs.tibia.pos, 800);
//   });
// });


for (var i = 0; i < Z.length; i++) {
  console.log(
    X[i], Z[i],
    calculate(limbs.femur.mm, limbs.tibia.mm, X[i], Z[i])
  );
}


console.log(
  calculate(limbs.femur.mm, limbs.tibia.mm, 80, 100)
);



function moveservo(servo, position, time) {
  console.log( "Set %s to %d, servo", position );
}