Add Robot Calibration Script

software/package.json

@@ -24,6 +24,7 @@
      "temp": "~0.5.0",
      "request": "~2.12.0",
      "johnny-five": "git://github.com/rwaldron/johnny-five.git",
-     "xmlhttprequest" : "~1.5.0"
+     "prompt": "~0.2.14",
+     "request": "~2.12.0"
    }
 }

software/src/calibrate.js

@@ -0,0 +1,111 @@
+#! /usr/local/bin/node
+
+var prompt = require("prompt")
+  , fs = require("fs")
+  , eol = require('os').EOL
+  , ArgumentParser = require('argparse').ArgumentParser
+  , robot = require('./lib/server/robot_http_client').client("127.0.0.1","4242");
+
+var args = {},
+  newCalibrationData = {};
+
+function CalibrationManager(argv) {
+  args = argv;
+  prompt.message = '';
+  prompt.delimiter = '';
+  prompt.start();
+}
+exports.CalibrationManager = CalibrationManager;
+
+var getCommandLineArgs = function() {
+  var parser = new ArgumentParser({
+    version: '0.0.1',
+    addHelp:true,
+    description: 'Tapster Calibration Script'
+  });
+
+  parser.addArgument(
+    [ '-o', '--output' ], {
+      defaultValue: "calibration.json"
+      , help: 'file to save calibration data to'
+    }
+  );
+
+  return parser.parseArgs();
+};
+
+CalibrationManager.prototype.calibrate = function() {
+  robot.calibrationData(function (calibrationData) {
+    console.log("Receiving existing calibration data.");
+    newCalibrationData = calibrationData;
+    console.log(newCalibrationData);
+    var schema = {
+      description: 'Please remove the arms from the robot and press any key to continue...',
+      type: 'string'
+    };
+    prompt.get(schema, function () {
+      calibrateServos(function () {
+        console.log("New Calibration Data Generated.");
+        console.log(newCalibrationData);
+        robot.setCalibrationData(newCalibrationData, function () {
+          console.log("Robot is now calibrated!");
+          fs.writeFile(args.output, JSON.stringify(newCalibrationData, null, 2), function(err) {
+            if(err) {
+              console.log('Calibration data could not be saved: ' + err);
+            } else {
+              console.log('Calibration data saved to "' + args.output +'"');
+            }
+          });
+        });
+      });
+    });
+  });
+};
+
+
+var calibrateServos = function(cb) {
+  var calibrateServoMinAndMax = function(armIndex, cb) {
+    return robot.reset(function () {
+      return calibrateServo(armIndex, true, function () {
+        return calibrateServo(armIndex, false, cb);
+      });
+    });
+  };
+  return calibrateServoMinAndMax(0, function() {
+    return calibrateServoMinAndMax(1, function() {
+      return calibrateServoMinAndMax(2, function() {
+        return robot.reset(cb);
+      });
+    });
+  });
+};
+
+var calibrateServo = function(armIndex, isMin, cb) {
+  robot.angles(function (angles) {
+    var description = 'Enter an adjustment for arm #' + (armIndex +1) + ', enter 0 when the arm is ' +
+      (isMin ? 'parallel to the roof.' : 'perpendicular to the roof');
+    var schema = {
+      name: "delta",
+      description: description,
+      type: 'number'
+    };
+
+    return prompt.get(schema, function (err, result) {
+      if (result.delta < 0.05 && result.delta > -0.05) {
+        newCalibrationData["servo" + (armIndex+1)][(isMin ? "min" : "max" ) + "imumAngle"] = angles[armIndex];
+        return cb();
+      } else {
+        console.log("Old Angles: " + angles);
+        angles[armIndex] = angles[armIndex] + result.delta;
+        console.log("New Angles: " + angles);
+        robot.setAngles(angles[0], angles[1], angles[2], function() {
+          return calibrateServo(armIndex, isMin, cb);
+        });
+      }
+    });
+  });
+};
+
+if(require.main === module) {
+  new CalibrationManager(getCommandLineArgs()).calibrate();
+}

software/src/lib/server/calibration.js

@@ -0,0 +1,30 @@
+var fs = require("fs");
+
+// default calibration
+module.exports.defaultData = {
+  restPoint : {
+    x : 0,
+    y : 0,
+    z : -120
+  },
+  servo1 : {
+    minimumAngle : 20,
+    maximumAngle : 90
+  },
+  servo2 : {
+    minimumAngle : 20,
+    maximumAngle : 90
+  },
+  servo3 : {
+    minimumAngle : 20,
+    maximumAngle : 90
+  }
+};
+
+module.exports.getDataFromFilePath = function(filePath) {
+  if (fs.existsSync(filePath)) {
+    return JSON.parse(fs.readFileSync(filePath, "utf8"));
+  } else {
+    return null;
+  }
+};

software/src/server/robot.js → software/src/lib/server/robot.js

@@ -1,14 +1,11 @@
-var kinematics = require("./../lib/kinematics");
+var kinematics = require("./../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;
 }
 
@@ -68,8 +65,8 @@ method.setPosition = function(x, y, z) {
   this.setAngles(t1,t2,t3);
 };
 
-method.reset = function() {
-  this.setPosition(calibration.restPoint.x, calibration.restPoint, calibration.restPoint.z);
+method.resetPosition = function() {
+  this.setPosition(this._calibration.restPoint.x, this._calibration.restPoint, this._calibration.restPoint.z);
 };
 
 method.getPositionForAngles = function(t1,t2,t3) {
@@ -85,9 +82,12 @@ method.getAnglesForPosition = function(x,y,z) {
 
 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);
+    var minAngle = 10;
+    var maxAngle = 20;
+    var range = maxAngle - minAngle;
+    var t1 = parseInt((Math.random() * range) + minAngle, 10);
+    var t2 = parseInt((Math.random() * range) + minAngle, 10);
+    var t3 = parseInt((Math.random() * range) + minAngle, 10);
     this.setAngles(t1,t2,t3);
   }.bind(this);
 
@@ -103,5 +103,13 @@ method.stopDancing = function() {
   }
 };
 
+method.getCalibrationData = function() {
+  return this._calibration;
+};
+
+method.setCalibrationData = function(newData) {
+  this._calibration = newData;
+};
+
 module.exports = {};
 module.exports.Robot = Robot;

software/src/lib/server/robot_http_client.js

@@ -0,0 +1,84 @@
+var http = require('http');
+
+exports.client = function(address, port) {
+  var get = function(path, cb) {
+    return http.get({ host: address, port: port, path: path }, function(res){
+      res.setEncoding('utf8');
+      return res.on('data', function(chunk) {
+        var result = JSON.parse(chunk);
+        return cb(result.data);
+      });
+    }).on("error", function(err){
+      console.log("Got error: " + err.message);
+      return cb(null, err);
+    });
+  };
+  var post = function(path, bodyData, cb) {
+    var req = http.request({ host: address, port: port, path: path, method: 'POST'}, function(res) {
+      res.setEncoding('utf8');
+      return res.on('data', function (chunk) {
+        var result = JSON.parse(chunk);
+        return cb(result.data);
+      });
+    }).on('error', function(err) {
+      console.log("Got error: " + err.message);
+      return cb(null, err);
+    });
+    req.setHeader('Content-Type', 'application/x-www-form-urlencoded');
+    req.write(bodyData);
+    req.write('\n');
+    req.end();
+  };
+  return {
+    address : address,
+    port : port,
+    url : function(uri) {
+      return 'http://' + address + ":" + port + uri;
+    },
+    angles : function(cb) {
+      return get('/angles', cb);
+    },
+    setAngles : function(theta1, theta2, theta3, cb) {
+      var postData = "theta1=" + theta1 + "&theta2=" +theta2 + "&theta3=" + theta3;
+      return post('/setAngles', postData, cb);
+    },
+    position : function(cb) {
+      return get('/position', cb);
+    },
+    setPosition : function(x, y, z, cb) {
+      var postData = "x=" + x + "&y=" + y + "&z=" + z;
+      return post('/setPosition', postData, cb);
+    },
+    reset : function(cb) {
+      return post('/reset', '', cb);
+    },
+    calibrationData : function(cb) {
+      return get('/calibrationData', cb);
+    },
+    setCalibrationData : function(newData, cb) {
+      var postData = "newData=" + JSON.stringify(newData);
+      return post('/setCalibrationData', postData, cb);
+    }
+    /*
+     positionForCoordinates : function(x,y) {
+     var xmlHttp = new XMLHttpRequest();
+     xmlHttp.open( "GET", this.url('/positionForCoordinates/x/' + x + "/y/" + y), false );
+     xmlHttp.send( null );
+     return eval(xmlHttp.responseText);
+     },
+     coordinatesForPosition : function(x,y) {
+     var xmlHttp = new XMLHttpRequest();
+     xmlHttp.open( "GET", this.url('/coordinatesForPosition/x/' + x + "/y/" + y), false );
+     xmlHttp.send( null );
+     return eval(xmlHttp.responseText);
+     },
+     */
+    /*
+    tap : function(x,y) {
+      var xmlHttp = new XMLHttpRequest();
+      xmlHttp.open( "GET", this.url('/tap/x/' + x + "/y/" + y), false );
+      xmlHttp.send( null );
+      return xmlHttp.responseText;
+    }*/
+  };
+};