diff --git a/software/package.json b/software/package.json index 276b0d5..46523af 100755 --- a/software/package.json +++ b/software/package.json @@ -25,6 +25,8 @@ "request": "~2.12.0", "johnny-five": "git://github.com/rwaldron/johnny-five.git", "prompt": "~0.2.14", - "request": "~2.12.0" + "request": "~2.12.0", + "sylvester":"0.0.21", + "wd": "0.3.11" } } diff --git a/software/src/calibrate.js b/software/src/calibrate.js index fad4ca0..ad20c9d 100755 --- a/software/src/calibrate.js +++ b/software/src/calibrate.js @@ -4,7 +4,8 @@ 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"); + , robot = require('./lib/server/robot_http_client').client("127.0.0.1","4242") + , wd = require('wd'); var args = {}, newCalibrationData = {}; @@ -38,70 +39,221 @@ 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 +'"'); - } - }); + console.log(JSON.stringify(newCalibrationData)); + return askToCalibrateRobot(function() { + return askToCalibrateDevice(function() { + saveCalibrationData(function() { + console.log("Calibration Complete"); }); - }); + }) }); }); }; +var askToCalibrateRobot = function(cb) { + var schema = { + name:"answer", + description: 'Would you like to calibrate the robot arms?', + type: 'string' + }; + prompt.get(schema, function (err, result) { + if (result.answer.toLowerCase().substr(0,1) == "y") { + return calibrateRobot(cb); + } else { + return cb(); + } + }); +}; -var calibrateServos = function(cb) { - var calibrateServoMinAndMax = function(armIndex, cb) { - return robot.reset(function () { - return calibrateServo(armIndex, true, function () { - return calibrateServo(armIndex, false, cb); - }); +var askToCalibrateDevice = function(cb) { + var schema = { + name:"answer", + description: 'Would you like to calibrate the a device?', + type: 'string' + }; + prompt.get(schema, function (err, result) { + if (result.answer.toLowerCase().substr(0,1) == "y") { + return calibrateDevice(cb); + } else { + return cb(); + } + }); +}; + +var saveCalibrationData = function(cb) { + + console.log("New Calibration Data Generated."); + console.log(JSON.stringify(newCalibrationData)); + return robot.setCalibrationData(newCalibrationData, function () { + console.log("Robot is now calibrated!"); + return fs.writeFile(args.output, JSON.stringify(newCalibrationData), function (err) { + if (err) { + console.log('Calibration data could not be saved: ' + err); + } else { + console.log('Calibration data saved to "' + args.output + '"'); + } + return cb(); }); + }); + + +}; + +var calibrateRobot = function(cb) { + var schema = { + description: 'Please remove the arms from the robot and press any key to continue...', + type: 'string' }; - return calibrateServoMinAndMax(0, function() { - return calibrateServoMinAndMax(1, function() { - return calibrateServoMinAndMax(2, function() { - return robot.reset(cb); + return prompt.get(schema, function () { + var calibrateServos = function(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); + }); + } + }); + }); + }; + + var calibrateServoMinAndMax = function(armIndex, cb) { + return robot.reset(function () { + return calibrateServo(armIndex, true, function () { + return calibrateServo(armIndex, false, cb); + }); + }); + }; + + // calibrate the servos + return calibrateServoMinAndMax(0, function() { + return calibrateServoMinAndMax(1, function() { + return calibrateServoMinAndMax(2, function() { + return robot.reset(cb); + }); + }); }); + }; + + return calibrateServos(function () { + return 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' - }; +var calibrateDevice = function(cb) { + newCalibrationData.device = { + contactPoint: { position:{},screenCoordinates:{} }, + point1: { position:{},screenCoordinates:{} }, + point2: { position:{},screenCoordinates:{} } + }; + var driver = wd.remote({port:4723}); + // optional extra logging + driver.on('status', function(info) { + console.log(info.cyan); + }); + driver.on('command', function(eventType, command, response) { + console.log(' > ' + eventType.cyan, command, (response || '').grey); + }); + driver.on('http', function(meth, path, data) { + console.log(' > ' + meth.magenta, path, (data || '').grey); + }); - 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); + var lowerAndCheckForContact = function(x,y,currentZ, cb) { + return robot.setPosition(x,y,currentZ,function() { + setTimeout(function() { + + var coordRegex = /label[^\(,]+\((\d+\.*\d*),\s+(\d+\.*\d*)\)/ ; + return driver.source(function(err, pageSource) { + if (coordRegex.test(pageSource)) { + var match = coordRegex.exec(pageSource); + var screenX = parseFloat(match[1]); + var screenY = parseFloat(match[2]); + return cb(screenX,screenY, currentZ); + } else { + if (currentZ < -150) { + return robot.reset(function() { + return cb(Error("Could not touch the screen.")); + }); + } else { + return lowerAndCheckForContact(x, y, currentZ - 2, cb); + } + } }); - } + //*/ + + /* + return driver.elementByClassName("UIAStaticText", function (err, element) { + if (element) { + return robot.reset(function() { + return element.getLocation(function (err, location) { + if (err) { + return cb(Error("Could get the element's location.")); + } else { + return element.getSize(function (err, size) { + if (err) { + return cb(Error("Could get the element's size.")); + } else { + var screenX = location.x + (size.width / 2.0); + var screenY = location.y + (size.height / 2.0); + return cb(screenX, screenY, currentZ); + } + }); + } + }); + }); + } else { + if (currentZ < -150) { + return robot.reset(function() { + return cb(Error("Could not touch the screen.")); + }); + } else { + return lowerAndCheckForContact(x, y, currentZ - 2, cb); + } + } + }); + //*/ + + }, 2000); + }); + }; + + return driver.init( { + app:"Appium.RobotCalibration", + platform:"iOS", + platformVersion:"8.2", + udid: "481309bbf8a3c341687e617bb7104be41f3abb07" + }, function() { + driver.setImplicitWaitTimeout(1000, function () { + return lowerAndCheckForContact(0, 0, -130, function (screenX, screenY, robotZ) { + newCalibrationData.device.contactPoint.position = {x: 0, y: 0, z:robotZ}; + newCalibrationData.device.contactPoint.screenCoordinates = {x: screenX, y: screenY}; + return lowerAndCheckForContact(20, 20, -130, function (screenX, screenY, robotZ) { + newCalibrationData.device.point1.position = {x: 20, y: 20, z:robotZ}; + newCalibrationData.device.point1.screenCoordinates = {x: screenX, y: screenY}; + return lowerAndCheckForContact(-10, -15, -130, function (screenX, size, robotZ) { + newCalibrationData.device.point2.position = {x: -10, y: -15, z:robotZ}; + newCalibrationData.device.point2.screenCoordinates = {x: screenX, y: screenY}; + return cb(); + }); + }); + }); }); }); }; diff --git a/software/src/calibration.json b/software/src/calibration.json new file mode 100644 index 0000000..5a0175f --- /dev/null +++ b/software/src/calibration.json @@ -0,0 +1 @@ +{"restPoint":{"x":0,"y":0,"z":-130},"servo1":{"minimumAngle":20.447287034628303,"maximumAngle":20.447287034628303},"servo2":{"minimumAngle":16.447287034628303,"maximumAngle":16.447287034628303},"servo3":{"minimumAngle":16.447287034628303,"maximumAngle":16.447287034628303}} \ No newline at end of file diff --git a/software/src/lib/server/robot.js b/software/src/lib/server/robot.js index f752da3..b394008 100644 --- a/software/src/lib/server/robot.js +++ b/software/src/lib/server/robot.js @@ -1,4 +1,7 @@ var kinematics = require("./../kinematics"); + +require("sylvester"); + var method = Robot.prototype; function Robot(servo1, servo2, servo3, calibration) { @@ -9,6 +12,32 @@ function Robot(servo1, servo2, servo3, calibration) { this._dancer_interval = null; } +var getTranslationMatrix = function(calibration) { + var b0x = calibration.device.point1.position.x, + b0y = calibration.device.point1.position.y, + b1x = calibration.device.point2.position.x, + b1y = calibration.device.point2.position.y; + var d0x = calibration.device.point1.screenCoordinates.x, + d0y = calibration.device.point1.screenCoordinates.y, + d1x = calibration.device.point2.screenCoordinates.x, + d1y = calibration.device.point2.screenCoordinates.y; + + var M = $M([ + [d0x, d0y, 1, 0], + [-d0y, d0x, 0, 1], + [d1x, d1y, 1, 0], + [-d1y, d1x, 0, 1] + ]); + var u = $M([ + [b0x], + [b0y], + [b1x], + [b1y] + ]); + var MI = M.inverse(); + return MI.multiply(u); +}; + var sin = function(degree) { return Math.sin(Math.PI * (degree/180)); }; @@ -79,6 +108,28 @@ method.getAnglesForPosition = function(x,y,z) { return [angles[1], angles[2], angles[3]]; }; +method.getPositionForScreenCoordinates = function(x,y) { + var matrix = getTranslationMatrix(); + var a = matrix.elements[0][0], + b = matrix.elements[1][0], + c = matrix.elements[2][0], + d = matrix.elements[3][0]; + var yprime = a * x + b * y + c; + var xprime = b * x - a * y + d; + return {x:xprime, y:yprime}; +}; + +method.tap = function(screenX, screenY) { + var position = this.getPositionForScreenCoordinates(screenX,screenY); + var touchZ = this._calibration.device.contactPoint.position.z * 0.90; + this.setPosition(position[1], position[2], touchZ*0.90); + setTimeout(function() { + this.setPosition(position[1], position[2], touchZ); + setTimeout(function() { + this.resetPosition(); + }.bind(this), 1000); + }.bind(this), 1500); +}; method.startDancing = function() { var _dance = function() { diff --git a/software/src/lib/server/robot_http_client.js b/software/src/lib/server/robot_http_client.js index 0956b50..a7b5982 100644 --- a/software/src/lib/server/robot_http_client.js +++ b/software/src/lib/server/robot_http_client.js @@ -49,6 +49,10 @@ exports.client = function(address, port) { var postData = "x=" + x + "&y=" + y + "&z=" + z; return post('/setPosition', postData, cb); }, + tap : function(x, y, cb) { + var postData = "x=" + x + "&y=" + y; + return post('/tap', postData, cb); + }, reset : function(cb) { return post('/reset', '', cb); }, @@ -59,26 +63,5 @@ exports.client = function(address, port) { 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; - }*/ }; }; \ No newline at end of file diff --git a/software/src/server.js b/software/src/server.js index 0d7fe0d..d7347c0 100755 --- a/software/src/server.js +++ b/software/src/server.js @@ -155,6 +155,29 @@ board.on("ready", function() { } }); + server.route({ + method: 'GET', + path:'/positionForScreenCoordinates/x/{x}/y/{y}', + handler: function (request, reply) { + console.log("GET " + request.path + ": "); + var x = parseFloat(request.params.x); + var y = parseFloat(request.params.y); + return reply(getCommonReponseObject(null,robot.getPositionForScreenCoordinates(x,y))); + } + }); + + server.route({ + method: 'POST', + path:'/tap', + handler: function (request, reply) { + console.log("POST " + request.path + ": "); + var x = parseFloat(request.payload.x); + var y = parseFloat(request.payload.y); + robot.tap(x,y); + return reply(getCommonReponseObject(null, '"OK"')); + } + }); + server.route({ method: 'GET', path:'/calibrationData',