Adding robot server

.gitignore

@@ -1,2 +1,4 @@
 .DS_Store
 node_modules
+.idea
+npm-debug.log

software/package.json

@@ -17,6 +17,13 @@
       "node":"0.x.x"
    },
    "dependencies":{
-      "johnny-five":"0.7.x"
+     "johnny-five":"0.7.x",
+     "hapi": "8.4.0",
+     "argparse": "~0.1.10",
+     "path": "~0.4.9",
+     "temp": "~0.5.0",
+     "request": "~2.12.0",
+     "johnny-five": "git://github.com/rwaldron/johnny-five.git",
+     "xmlhttprequest" : "~1.5.0"
    }
 }

software/src/bot.js

@@ -1,5 +1,5 @@
 five = require("johnny-five");
-ik = require("./ik");
+ik = require("./lib/kinematics");
 board = new five.Board({
   debug: false
 });

software/src/server/calibration.js

@@ -0,0 +1,29 @@
+var fs = require("fs");
+
+module.exports.loadDataFromFilePath = function(filePath) {
+// Default Calibration
+  module.exports.data = {
+    restPoint : {
+      x : 0,
+      y : 0,
+      z : -120
+    },
+    servo1 : {
+      minimumAngle : 20,
+      maximumAngle : 90
+    },
+    servo2 : {
+      minimumAngle : 20,
+      maximumAngle : 90
+    },
+    servo3 : {
+      minimumAngle : 20,
+      maximumAngle : 90
+    }
+  };
+
+  // Load Calibration Data
+  if (fs.existsSync(filePath)) {
+    module.exports.data = eval(fs.readFileSync(filePath, "utf8"));
+  }
+};

software/src/server/parser.js

@@ -0,0 +1,32 @@
+var ArgumentParser = require('argparse').ArgumentParser;
+
+// parse arguments
+var parser =  new ArgumentParser({
+  version: '0.0.1',
+  addHelp:true,
+  description: 'Tapster Server'
+});
+
+parser.addArgument(
+  [ '-c', '--calibration'], {
+    help: 'file to load calibration data from'
+  });
+
+parser.addArgument(
+  ['-p', '--port'] , {
+    defaultValue: 4242
+    , required: false
+    , type: 'int'
+    , example: "4242"
+    , help: 'port to listen on'
+  });
+
+parser.addArgument(
+  ['-a', '--address'], {
+    defaultValue: '127.0.0.1'
+    , required: false
+    , example: "127.0.0.1"
+    , help: 'IP Address to listen on'
+  });
+
+module.exports = parser;

software/src/server/robot.js

@@ -0,0 +1,107 @@
+var kinematics = require("./../lib/kinematics");
+var method = Robot.prototype;
+
+function Robot(servo1, servo2, servo3, calibration) {
+  this._servo1 = servo1;
+  this._servo2 = servo2;
+  this._servo3 = servo3;
+  this._calibration = calibration;
+  this._minAngle = 10;
+  this._maxAngle = 20;
+  this._range = this._maxAngle - this._minAngle;
+  this._dancer_interval = null;
+}
+
+var sin = function(degree) {
+  return Math.sin(Math.PI * (degree/180));
+};
+
+var cos = function(degree) {
+  return Math.cos(Math.PI * (degree/180));
+};
+
+var mapNumber = function (num,  in_min , in_max , out_min , out_max ) {
+  return ( num - in_min ) * ( out_max - out_min ) / ( in_max - in_min ) + out_min;
+};
+
+var rotate = function(x,y) {
+  var theta = -60;
+  var x1 = x * cos(theta) - y * sin(theta);
+  var y1 = y * cos(theta) + x * sin(theta);
+  return [x1,y1]
+};
+
+var reflect = function(x,y) {
+  var theta = 0;
+  var x1 = x;
+  var y1 = x * sin(2*theta) - y * cos(2*theta);
+  return [x1,y1]
+};
+
+
+method.getAngles = function() {
+  return [this._servo1.last.degrees, this._servo2.last.degrees, this._servo3.last.degrees];
+};
+
+method.setAngles = function(t1,t2,t3) {
+  console.log("Setting Angles:" + [t1,t2,t3]);
+  t1 = isNaN(t1) ? this._calibration.servo1.minimumAngle : t1;
+  t2 = isNaN(t2) ? this._calibration.servo1.minimumAngle : t2;
+  t3 = isNaN(t3) ? this._calibration.servo1.minimumAngle : t3;
+  this._servo1.to(t1);
+  this._servo2.to(t2);
+  this._servo3.to(t3);
+};
+
+method.getPosition = function() {
+  var angles = this.getAngles();
+  return kinematics.forward(angles[0], angles[1], angles[2]);
+};
+
+method.setPosition = function(x, y, z) {
+  var reflected = reflect(x,y);
+  var rotated = rotate(reflected[0],reflected[1]);
+  var angles = kinematics.inverse(rotated[0], rotated[1], z);
+  var t1 = mapNumber(angles[1], 0 , 90 , this._calibration.servo1.minimumAngle ,  this._calibration.servo1.maximumAngle);
+  var t2 = mapNumber(angles[2], 0 , 90 , this._calibration.servo2.minimumAngle ,  this._calibration.servo2.maximumAngle);
+  var t3 = mapNumber(angles[3], 0 , 90 , this._calibration.servo3.minimumAngle ,  this._calibration.servo3.maximumAngle);
+  this.setAngles(t1,t2,t3);
+};
+
+method.reset = function() {
+  this.setPosition(calibration.restPoint.x, calibration.restPoint, calibration.restPoint.z);
+};
+
+method.getPositionForAngles = function(t1,t2,t3) {
+  var points = kinematics.forward(t1,t2,t3);
+  return [points[1], points[2], points[3]];
+};
+
+method.getAnglesForPosition = function(x,y,z) {
+  var angles = kinematics.inverse(x,y,z);
+  return [angles[1], angles[2], angles[3]];
+};
+
+
+method.startDancing = function() {
+  var _dance = function() {
+    var t1 = parseInt((Math.random() * this._range) + this._minAngle, 10);
+    var t2 = parseInt((Math.random() * this._range) + this._minAngle, 10);
+    var t3 = parseInt((Math.random() * this._range) + this._minAngle, 10);
+    this.setAngles(t1,t2,t3);
+  }.bind(this);
+
+  if (!this._dancer_interval) {
+    this._dancer_interval = setInterval(_dance, 250);
+  }
+};
+
+method.stopDancing = function() {
+  if (this._dancer_interval) {
+    clearInterval(this._dancer_interval);
+    this._dancer_interval = null;
+  }
+};
+
+module.exports = {};
+module.exports.Robot = Robot;

software/src/server/server.js

@@ -0,0 +1,151 @@
+#! /usr/local/bin/node
+var application_root = __dirname
+  , parser = require("./parser")
+  , Hapi = require("hapi")
+  , path = require("path")
+  , five = require("johnny-five")
+  , calibration = require("./calibration")
+  , Robot = require("./robot").Robot
+
+args = parser.parseArgs();
+calibration.loadDataFromFilePath(args.calibration);
+
+var board = new five.Board({ debug: false});
+board.on("ready", function() {
+  var servo1 = five.Servo({
+    pin: 9,
+    range: [0,90]
+  });
+
+  var servo2 = five.Servo({
+    pin: 10,
+    range: [0,90]
+  });
+
+  var servo3 = five.Servo({
+    pin: 11,
+    range: [0,90]
+  });
+
+  servo1.on("error", function() {
+    console.log(arguments);
+  });
+  servo2.on("error", function() {
+    console.log(arguments);
+  });
+  servo3.on("error", function() {
+    console.log(arguments);
+  });
+
+  // Initialize Objects
+  var robot = new Robot(servo1,servo2,servo3,calibration.data);
+
+  // Move to starting point
+  robot.setPosition(calibration.data.restPoint.x, calibration.data.restPoint.y, calibration.data.restPoint.z);
+
+  // create a server with a host and port
+  var server = new Hapi.Server();
+  server.connection({
+    host: args.address,
+    port: args.port
+  });
+
+  server.route({
+    method: 'GET',
+    path:'/status',
+    handler: function (request, reply) {
+      console.log("GET " + request.path + ": ");
+      reply('\"OK\"');
+    }
+  });
+
+  server.route({
+    method: 'POST',
+    path:'/reset',
+    handler: function (request, reply) {
+      console.log("POST " + request.path + ": ");
+      robot.reset();
+      reply(robot.getAngles());
+    }
+  });
+
+  server.route({
+    method: 'POST',
+    path:'/dance',
+    handler: function (request, reply) {
+      console.log("POST " + request.path + ": ");
+      robot.startDancing();
+      reply('\"Dancing!\"');
+    }
+  });
+
+  server.route({
+    method: 'POST',
+    path:'/stopDancing',
+    handler: function (request, reply) {
+      console.log("POST " + request.path + ": ");
+      robot.stopDancing();
+      reply('\"No more dancing.\"');
+    }
+  });
+
+  server.route({
+    method: 'POST',
+    path:'/setAngles',
+    handler: function (request, reply) {
+      console.log("POST " + request.path + ": ");
+      var theta1 = parseFloat(request.payload.theta1);
+      var theta2 = parseFloat(request.payload.theta2);
+      var theta3 = parseFloat(request.payload.theta3);
+      robot.setAngles(theta1, theta2, theta3);
+      return reply("\"OK\"");
+    }
+  });
+
+  server.route({
+    method: 'POST',
+    path:'/setPosition',
+    handler: function (request, reply) {
+      console.log("POST " + request.path + ": ");
+      var x = parseFloat(request.payload.x);
+      var y = parseFloat(request.payload.y);
+      var z = parseFloat(request.payload.z);
+      robot.setPosition(x, y, z);
+      return reply("\"OK\"");
+    }
+  });
+
+  server.route({
+    method: 'GET',
+    path:'/angles',
+    handler: function (request, reply) {
+      console.log("GET " + request.path + ": ");
+      return reply(robot.getAngles());
+    }
+  });
+
+  server.route({
+    method: 'GET',
+    path:'/position',
+    handler: function (request, reply) {
+      console.log("POST " + request.path + ": ");
+      return reply(robot.getPosition());
+    }
+  });
+
+  server.route({
+    method: 'GET',
+    path:'/anglesForPosition/x/{x}/y/{y}/z/{z}',
+    handler: function (request, reply) {
+      console.log("GET " + request.path + ": ");
+      var x = parseFloat(request.params.x);
+      var y = parseFloat(request.params.y);
+      var z = parseFloat(request.params.z);
+      return reply(robot.getAnglesForPosition(x,y,z));
+    }
+  });
+
+  server.start();
+  console.log("Robot listening on port " + args.port);
+
+});