johnny-five/wip/quad.js

The JavaScript Arduino Programming Framework.

var five = require("../lib/johnny-five.js");
var keypress = require("keypress");
var board = new five.Board();

// Limb measurements
var coxa = 1.5;
var femur = 2.25;
var tibia = 4.5;

// Body measurements
var center = {
  // 4 inches from coxa to coxa
  y: 4,
  // 3 inches from coxa to center of body
  x: 3
};

// var props = [ "x", "y", "z" ];

// var limbs = {
//   rf: {},
//   rr: {},
//   lf: {},
//   lr: {}
// };

// Object.keys(limbs).forEach(function(limb) {
//   limbs[limb] = props.reduce(function(accum, prop) {
//     accum[prop] = 0;
//     return accum;
//   }, {});
// });

// console.log( limbs );
// Current positions
var RFx, RFy, RFz;
var RRx, RRy, RRz;
var LFx, LFy, LFz;
var LRx, LRy, LRz;



board.on("ready", function() {
  var speed, commands, motors;

  // // Right Front Servos (1)
  // var RF0 = new five.Servo(8);
  // var RF1 = new five.Servo(9);
  // var RF2 = new five.Servo(10);

  // // Right Rear Servos (2)
  // var RR0 = new five.Servo(2);
  // var RR1 = new five.Servo(3);
  // var RR2 = new five.Servo(4);

  // // Left Front Servos (3)
  // var LF0 = new five.Servo(11);
  // var LF1 = new five.Servo(12);
  // var LF2 = new five.Servo(13);

  // // Left Rear Servos (4)
  // var LR0 = new five.Servo(5);
  // var LR1 = new five.Servo(6);
  // var LR2 = new five.Servo(7);

  // counters
  var j = 0;
  var counter = 0;

  function controller( ch, key ) {

    if ( key ) {
      // moves the robot body around in a circle
      if (key.name === "a") {
        around(1.5, 3);
      }
      // The following three functions are the basic creep gait walk functions. By varying the number of steps in shiftWalk(steps,time) you can control the speed of the grait
      else if (key.name === "q") {
        shiftWalk(50, 10);
      } else if (key.name === "w") {
        shiftWalk(25, 10);
      } else if (key.name === "e") {
        shiftWalk(13, 10);
      }

      // center the robot
      else if (key.name === "c") {
        fullMove(0, 0, 4);
      }

      // move the robot up and down using IK
      else if (key.name === "u") {
        up(3, 5);
      }

      // Rotate the robots body back and forth
      else if (key.name === "l") {
        rotate(25);
        rotate(-50);
        rotate(25);
      } else if (key.name === "r") {
        smoothMove(3, LFx, LFy, 4 - 2, 50, 10);
      } else if (key.name === "t") {
        smoothMove(1, RFx, RFy, RFz - 2, 50, 10);
      } else if (key.name === "f") {
        smoothMove(4, LRx, LRy, 4 - 2, 50, 10);
        smoothMove(2, RRx, RRy, 4 - 2, 50, 10);
      } else if (key.name === "g") {
        smoothMove(2, RRx, RRy, 4 - 2, 50, 10);
      } else if (key.name === "z") {
        turn(25, 10);
      }
    }
  }

  keypress(process.stdin);

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

// // This function rotates the robot body from the current
// // position to "degree" and then back.
// function rotate(degree) {
//   var RFcur = RF0.position;
//   var RRcur = RR0.position;
//   var LFcur = LF0.position;
//   var LRcur = LR0.position;

//   Serial.println(RFcur);

//   if (degree > 0) {
//     for (var count1 = 0; count1 < degree; count1++) {
//       RFcur += 1;
//       RRcur += 1;
//       LFcur += 1;
//       LRcur += 1;
//       RF0.write(RFcur);
//       RR0.write(RRcur);
//       LF0.write(LFcur);
//       LR0.write(LRcur);
//       delay(10);
//     }
//   } else {
//     for (var count1 = 0; count1 > degree; count1--) {
//       RFcur -= 1;
//       RRcur -= 1;
//       LFcur -= 1;
//       LRcur -= 1;
//       RF0.write(RFcur);
//       RR0.write(RRcur);
//       LF0.write(LFcur);
//       LR0.write(LRcur);
//       delay(10);
//     }
//   }

// }



// // Move a leg from current position to new position smoothly by using small angle chages broken up into "steps" every t1 milliseconds

// function smoothMove(servoNumber, x, y, z, t1, steps) {

//   // RF
//   if (servoNumber == 1) {
//     // how big each step is
//     var dx = (x - RFx) / steps;
//     var dy = (y - RFy) / steps;
//     var dz = (z - RFz) / steps;

//     // moves the leg along each axis by the corresponding delta, waits t1 then repeats
//     for (var i = 0; i <= steps; i++) {
//       RFx += dx;
//       RFy += dy;
//       RFz += dz;
//       moveServos2RF(RFx, RFy, RFz);
//       delay(t1);
//     }
//   }

//   // RR
//   else if (servoNumber == 2) {
//     var dx = (x - RRx) / steps;
//     var dy = (y - RRy) / steps;
//     var dz = (z - RRz) / steps;
//     for (var i = 0; i <= steps; i++) {
//       RRx += dx;
//       RRy += dy;
//       RRz += dz;
//       moveServos2RR(RRx, RRy, RRz);
//       delay(t1);
//     }
//   }

//   // LF
//   if (servoNumber == 3) {
//     var dx = (x - LFx) / steps;
//     var dy = (y - LFy) / steps;
//     var dz = (z - LFz) / steps;
//     for (var i = 0; i <= steps; i++) {
//       LFx += dx;
//       LFy += dy;
//       LFz += dz;
//       moveServos2LF(LFx, LFy, LFz);
//       delay(t1);
//     }
//   }

//   // LR
//   else if (servoNumber == 4) {
//     var dx = (x - LRx) / steps;
//     var dy = (y - LRy) / steps;
//     var dz = (z - LRz) / steps;
//     for (var i = 0; i <= steps; i++) {
//       LRx += dx;
//       LRy += dy;
//       LRz += dz;
//       moveServos2LR(LRx, LRy, LRz);
//       delay(t1);
//     }
//   } else if (servoNumber == 0) {

//   }
// }


// // This is the primary creep gait that I've developed. It uses the smoothMove functions broken up into a number of "steps" with a delay "time" between each step
// function shiftWalk(steps, time) {
//   // starting vertical position
//   var z = 3;
//   // How far to raise the leg for each movement
//   var dz = -2;

//   // shift entire body forward and to the left. This causes the robot to move forward 1 inch
//   // and prepares for the left legs to move
//   smoothFullMove(2, -1, z, 2, 1, z, steps * 2, time);
//   print3(LRx, LRy, LRz);

//   // Moves left back leg up then forward then down
//   smoothMove(4, LRx, LRy, z + dz, steps, time); // moveServos2LR(3,5,1);
//   smoothMove(4, 1, 5, z + dz, steps, time); // moveServos2LR(1,5,1);
//   smoothMove(4, 1, 5, z, steps, time); // moveServos2LR(0,5,3);

//   // Moves left front leg up then forward then down
//   smoothMove(3, -1, 5, z + dz, steps, time); // moveServos2LF(-1,5,0);
//   smoothMove(3, -3, 5, z + dz, steps, time); // moveServos2LF(-3,5,1);
//   smoothMove(3, -3, 5, z, steps, time); // moveServos2LF(-3,5,3);

//   // shift entire body forward and to the right. This causes the robot to move forward 1 inch
//   // and prepares for the right legs to move
//   smoothFullMove(2, 1, z, 2, -1, z, steps * 2, time);

//   // Moves right back leg up then forward then down
//   smoothMove(2, 3, 5, z + dz, steps, time); // moveServos2LR(3,5,1);
//   smoothMove(2, 1, 5, z + dz, steps, time); // moveServos2LR(1,5,1);
//   smoothMove(2, 1, 5, z, steps, time); // moveServos2LR(0,5,3);

//   // Moves right front leg up then forward then down
//   smoothMove(1, -1, 5, z + dz, steps, time); // moveServos2LF(-1,5,0);
//   smoothMove(1, -3, 5, z + dz, steps, time); // moveServos2LF(-3,5,1);
//   smoothMove(1, -3, 5, z, steps, time); // moveServos2LF(-3,5,3);

// }


// /*IK MOVE FUNCTIONS*/
// // moves the robot vertically from z1 to z2 and then back to z1
// function up(z1, z2) {
//   var z = z1;
//   while (z <= z2) {
//     fullMove(0, 0, z);
//     delay(25);
//     z += .1;
//   }
//   while (z >= z1) {
//     fullMove(0, 0, z);
//     delay(25);
//     z -= .1;
//   }
// }

// // moves the robot in a circle of radius r and at a height z
// function around(r, z) {
//   var x = 0;
//   while (x <= 2 * 3.14) {
//     fullMove(r * cos(x), r * sin(x), z);
//     delay(50);
//     x += .1;
//   }
// }



// // Moves the entire robot body to position x,y,z. Note this results in very jerky movements
// 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;

//   RRx = x + center.x;
//   RRy = center.y - y;
//   RRz = z;

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

//   LRx = x + center.x;
//   LRy = center.y + y;
//   LRz = z;

//   moveServosRF(gamma(RFx, RFy, RFz), alpha(RFx, RFy, RFz), beta(RFx, RFy, RFz));
//   moveServosRR(gamma(RRx, RRy, RRz), alpha(RRx, RRy, RRz), beta(RRx, RRy, RRz));
//   moveServosLF(gamma(LFx, LFy, LFz), alpha(LFx, LFy, LFz), beta(LFx, LFy, LFz));
//   moveServosLR(gamma(LRx, LRy, LRz), alpha(LRx, LRy, LRz), beta(LRx, LRy, LRz));

// }

// // Moves the entire body from x0,y0,z0 to x,y,z smoothly with "steps" steps separated by t1 milliseconds. This is usefully for shifting the center of gravity
// 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);
//   }
// }

// // used for attaching the legs to the servos at the right position
// function calibrate() {
//   RR0.write(90);
//   RR1.write(0);
//   RR2.write(90);

//   RF0.write(90);
//   RF1.write(0);
//   RF2.write(90);

//   LR0.write(90);
//   LR1.write(0);
//   LR2.write(90);

//   LF0.write(90);
//   LF1.write(0);
//   LF2.write(90);
// }

// function calibrate2() {
//   moveServosRF(90, 90, 90);
//   moveServosRR(90, 90, 90);
//   moveServosLF(90, 90, 90);
//   moveServosLR(90, 90, 90);
// }

// /***INVERSE KINEMATICS***/

// // IK for gamma (coxa angle)
// function gamma(x, y, a) {
//   var g = atan(x / y);
//   return int(g * 57.3) + 90;
// }

// // IK for beta (femur angle)
// function beta(x, y, a) {
//   var g = atan(x / y);
//   x /= cos(g);
//   // var bet =acos((pow(x,2)+pow(y,2)+pow(a,2)-pow(c,2)-pow(b,2))/(-2*c*b));
//   var bet = acos((pow(a, 2) + pow(y - coxa, 2) - pow(tibia, 2) - pow(femur, 2)) / (-2 * femur * tibia));
//   return int(bet * 57.3);

// }

// // IK for alpha (tibia angle)
// function alpha(x, y, a) {
//   var g = atan(x / y);
//   x /= cos(g);
//   var L2 = sqrt(pow(a, 2) + pow(y - coxa, 2));
//   var alp1 = atan((y - coxa) / a);
//   var alp2 = acos((pow(tibia, 2) - pow(femur, 2) - pow(L2, 2)) / (-2 * femur * L2));
//   var alp = alp1 + alp2;
//   return int(alp * 57.3);
// }

// /**Move Functions**/

// // The following "moveServos2XX" move the tip of the corresponding
// // legs to the points x,y,z. This probably could have been combined
// // with the "moveServosXX" functions but were written after so
// // they simply call those functions.
// //
// function moveServos2RR(x, y, z) {
//   //
//   RRx = x;
//   RRy = y;
//   RRz = z;
//   moveServosRR(gamma(RRx, RRy, RRz), alpha(RRx, RRy, RRz), beta(RRx, RRy, RRz));
// }

// function moveServos2RF(x, y, z) {
//   RFx = x;
//   RFy = y;
//   RFz = z;
//   moveServosRF(gamma(RFx, RFy, RFz), alpha(RFx, RFy, RFz), beta(RFx, RFy, RFz));

// }

// function moveServos2LR(x, y, z) {
//   LRx = x;
//   LRy = y;
//   LRz = z;
//   //   Serial.print("alpha");
//   //  Serial.println(alpha(LRx,LRy,LRz));
//   moveServosLR(gamma(LRx, LRy, LRz), alpha(LRx, LRy, LRz), beta(LRx, LRy, LRz));

// }

// function moveServos2LF(x, y, z) {
//   LFx = x;
//   LFy = y;
//   LFz = z;
//   moveServosLF(gamma(LFx, LFy, LFz), alpha(LFx, LFy, LFz), beta(LFx, LFy, LFz));
// }

// // The following "moveServosXX" functions move the servos in one leg to the given angles. gamma (g), alpha (a), and beta (b) as defined in the IK model
// function moveServosRR(g, a, b) {
//   // The following calibrations were determined experimentally to get the angles to line up with the IK equations
//   g = (g);
//   a = (180 - a + 10);
//   b = (b + 10);

//   RR0.write(g);
//   RR1.write(a);
//   RR2.write(b);
//   // if out of range
//   if (g > 180 || g < 0 || a > 180 || a < 0 || b > 180 || b < 0)
//     Serial.println("RR error");
//   //  print3(g,a,b);
// }


// // move the joints in the RF servo to angles g, a, and b
// function moveServosRF(g, a, b) {
//   // The following calibrations were determined experimentally to get the angles to line up with the IK equations
//   g = (g);
//   a = (180 - a + 10);
//   b = (b + 10);

//   RF0.write(g);
//   RF1.write(a);
//   RF2.write(b);
//   // if out of range
//   if (g > 180 || g < 0 || a > 180 || a < 0 || b > 180 || b < 0)
//     Serial.println("RF error");
// }

// // move the joints in the LR servo to angles g, a, and b
// function moveServosLR(g, a, b) {
//   // The following calibrations were determined experimentally to get the angles to line up with the IK equations
//   g = 180 - g - 10;
//   a = 180 - a + 5;
//   b = b + 5;
//   LR0.write(g);
//   LR1.write(a);
//   LR2.write(b);
//   // if out of range
//   if (g > 180 || g < 0 || a > 180 || a < 0 || b > 180 || b < 0) {
//     Serial.println("LR error");
//   }
// }

// // move the joints in the LF servo to angles g, a, and b
// function moveServosLF(g, a, b) {
//   // The following calibrations were determined experimentally to get the angles to line up with the IK equations
//   g = 180 - g + 5;
//   a = 180 - a - 7;
//   b = b + 15;
//   LF0.write(g);
//   LF1.write(a);
//   LF2.write(b);
//   // if out of range
//   if (g > 180 || g < 0 || a > 180 || a < 0 || b > 180 || b < 0)
//     Serial.println("LF error");
// }