From 592f6680c5f714f765b4b50f4e03981bc7e93010 Mon Sep 17 00:00:00 2001 From: Dan Cuellar Date: Fri, 20 Mar 2015 15:16:20 +0000 Subject: [PATCH 1/5] Adding robot server --- .gitignore | 2 + software/package.json | 9 +- software/src/bot.js | 2 +- software/src/{ik.js => lib/kinematics.js} | 0 software/src/server/calibration.js | 29 +++++ software/src/server/parser.js | 32 +++++ software/src/server/robot.js | 107 +++++++++++++++ software/src/server/server.js | 151 ++++++++++++++++++++++ 8 files changed, 330 insertions(+), 2 deletions(-) rename software/src/{ik.js => lib/kinematics.js} (100%) create mode 100644 software/src/server/calibration.js create mode 100644 software/src/server/parser.js create mode 100644 software/src/server/robot.js create mode 100755 software/src/server/server.js diff --git a/.gitignore b/.gitignore index 9daa824..116ba54 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,4 @@ .DS_Store node_modules +.idea +npm-debug.log diff --git a/software/package.json b/software/package.json index 0463414..994450f 100755 --- a/software/package.json +++ b/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" } } diff --git a/software/src/bot.js b/software/src/bot.js index f313d6a..1161676 100755 --- a/software/src/bot.js +++ b/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 }); diff --git a/software/src/ik.js b/software/src/lib/kinematics.js similarity index 100% rename from software/src/ik.js rename to software/src/lib/kinematics.js diff --git a/software/src/server/calibration.js b/software/src/server/calibration.js new file mode 100644 index 0000000..649883b --- /dev/null +++ b/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")); + } +}; \ No newline at end of file diff --git a/software/src/server/parser.js b/software/src/server/parser.js new file mode 100644 index 0000000..68a21aa --- /dev/null +++ b/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; \ No newline at end of file diff --git a/software/src/server/robot.js b/software/src/server/robot.js new file mode 100644 index 0000000..ea1374f --- /dev/null +++ b/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; \ No newline at end of file diff --git a/software/src/server/server.js b/software/src/server/server.js new file mode 100755 index 0000000..b4113b3 --- /dev/null +++ b/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); + +}); From 306f79b52eee07aa248efb7ea47477ed1c41867d Mon Sep 17 00:00:00 2001 From: Dan Cuellar Date: Sun, 22 Mar 2015 13:29:09 +0000 Subject: [PATCH 2/5] Add Robot Calibration Script --- software/package.json | 3 +- software/src/calibrate.js | 111 +++++++++++++++++++ software/src/lib/server/calibration.js | 30 +++++ software/src/{ => lib}/server/parser.js | 0 software/src/{ => lib}/server/robot.js | 26 +++-- software/src/lib/server/robot_http_client.js | 84 ++++++++++++++ software/src/{server => }/server.js | 78 +++++++++---- software/src/server/calibration.js | 29 ----- 8 files changed, 298 insertions(+), 63 deletions(-) create mode 100755 software/src/calibrate.js create mode 100644 software/src/lib/server/calibration.js rename software/src/{ => lib}/server/parser.js (100%) rename software/src/{ => lib}/server/robot.js (81%) create mode 100644 software/src/lib/server/robot_http_client.js rename software/src/{server => }/server.js (58%) delete mode 100644 software/src/server/calibration.js diff --git a/software/package.json b/software/package.json index 994450f..276b0d5 100755 --- a/software/package.json +++ b/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" } } diff --git a/software/src/calibrate.js b/software/src/calibrate.js new file mode 100755 index 0000000..fad4ca0 --- /dev/null +++ b/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(); +} diff --git a/software/src/lib/server/calibration.js b/software/src/lib/server/calibration.js new file mode 100644 index 0000000..f90b35b --- /dev/null +++ b/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; + } +}; \ No newline at end of file diff --git a/software/src/server/parser.js b/software/src/lib/server/parser.js similarity index 100% rename from software/src/server/parser.js rename to software/src/lib/server/parser.js diff --git a/software/src/server/robot.js b/software/src/lib/server/robot.js similarity index 81% rename from software/src/server/robot.js rename to software/src/lib/server/robot.js index ea1374f..f752da3 100644 --- a/software/src/server/robot.js +++ b/software/src/lib/server/robot.js @@ -1,4 +1,4 @@ -var kinematics = require("./../lib/kinematics"); +var kinematics = require("./../kinematics"); var method = Robot.prototype; function Robot(servo1, servo2, servo3, calibration) { @@ -6,9 +6,6 @@ function Robot(servo1, servo2, servo3, calibration) { 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.y, 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; \ No newline at end of file diff --git a/software/src/lib/server/robot_http_client.js b/software/src/lib/server/robot_http_client.js new file mode 100644 index 0000000..0956b50 --- /dev/null +++ b/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; + }*/ + }; +}; \ No newline at end of file diff --git a/software/src/server/server.js b/software/src/server.js similarity index 58% rename from software/src/server/server.js rename to software/src/server.js index b4113b3..0d7fe0d 100755 --- a/software/src/server/server.js +++ b/software/src/server.js @@ -1,30 +1,30 @@ #! /usr/local/bin/node -var application_root = __dirname - , parser = require("./parser") + +var parser = require("./lib/server/parser") , Hapi = require("hapi") , path = require("path") , five = require("johnny-five") - , calibration = require("./calibration") - , Robot = require("./robot").Robot + , calibration = require("./lib/server/calibration") + , Robot = require("./lib/server/robot").Robot; -args = parser.parseArgs(); -calibration.loadDataFromFilePath(args.calibration); +var args = parser.parseArgs(); +var robot, servo1, servo2, servo3; var board = new five.Board({ debug: false}); board.on("ready", function() { - var servo1 = five.Servo({ + servo1 = five.Servo({ pin: 9, - range: [0,90] + range: [0,120] }); - var servo2 = five.Servo({ + servo2 = five.Servo({ pin: 10, - range: [0,90] + range: [0,120] }); - var servo3 = five.Servo({ + servo3 = five.Servo({ pin: 11, - range: [0,90] + range: [0,120] }); servo1.on("error", function() { @@ -38,10 +38,12 @@ board.on("ready", function() { }); // Initialize Objects - var robot = new Robot(servo1,servo2,servo3,calibration.data); + var calibrationData = calibration.getDataFromFilePath(args.calibration); + calibrationData = calibrationData == null ? calibration.defaultData : calibrationData; + robot = new Robot(servo1,servo2,servo3,calibrationData); // Move to starting point - robot.setPosition(calibration.data.restPoint.x, calibration.data.restPoint.y, calibration.data.restPoint.z); + robot.resetPosition(); // create a server with a host and port var server = new Hapi.Server(); @@ -50,12 +52,20 @@ board.on("ready", function() { port: args.port }); + var getCommonReponseObject = function(err, data) { + if (err) { + return { status:err.code, data: err }; + } else { + return { status: 0, data: data }; + } + }; + server.route({ method: 'GET', path:'/status', handler: function (request, reply) { console.log("GET " + request.path + ": "); - reply('\"OK\"'); + reply(getCommonReponseObject(null, '"OK"')); } }); @@ -64,8 +74,8 @@ board.on("ready", function() { path:'/reset', handler: function (request, reply) { console.log("POST " + request.path + ": "); - robot.reset(); - reply(robot.getAngles()); + robot.resetPosition(); + reply(getCommonReponseObject(null, robot.getAngles())); } }); @@ -75,7 +85,7 @@ board.on("ready", function() { handler: function (request, reply) { console.log("POST " + request.path + ": "); robot.startDancing(); - reply('\"Dancing!\"'); + reply(getCommonReponseObject(null, '"Dancing!"')); } }); @@ -85,7 +95,7 @@ board.on("ready", function() { handler: function (request, reply) { console.log("POST " + request.path + ": "); robot.stopDancing(); - reply('\"No more dancing.\"'); + reply(getCommonReponseObject(null, '"No more dancing."')); } }); @@ -98,7 +108,7 @@ board.on("ready", function() { var theta2 = parseFloat(request.payload.theta2); var theta3 = parseFloat(request.payload.theta3); robot.setAngles(theta1, theta2, theta3); - return reply("\"OK\""); + return reply(getCommonReponseObject(null, robot.getAngles())); } }); @@ -111,7 +121,7 @@ board.on("ready", function() { var y = parseFloat(request.payload.y); var z = parseFloat(request.payload.z); robot.setPosition(x, y, z); - return reply("\"OK\""); + return reply(getCommonReponseObject(null, '"OK"')); } }); @@ -120,7 +130,7 @@ board.on("ready", function() { path:'/angles', handler: function (request, reply) { console.log("GET " + request.path + ": "); - return reply(robot.getAngles()); + return reply(getCommonReponseObject(null, robot.getAngles())); } }); @@ -129,7 +139,7 @@ board.on("ready", function() { path:'/position', handler: function (request, reply) { console.log("POST " + request.path + ": "); - return reply(robot.getPosition()); + return reply(getCommonReponseObject(null, robot.getPosition())); } }); @@ -141,7 +151,27 @@ board.on("ready", function() { 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)); + return reply(getCommonReponseObject(null,robot.getAnglesForPosition(x,y,z))); + } + }); + + server.route({ + method: 'GET', + path:'/calibrationData', + handler: function (request, reply) { + console.log("GET " + request.path + ": "); + return reply(getCommonReponseObject(null, robot.getCalibrationData())); + } + }); + + server.route({ + method: 'POST', + path:'/setCalibrationData', + handler: function (request, reply) { + console.log("POST " + request.path + ": "); + var newData = JSON.parse(request.payload.newData); + robot.setCalibrationData(newData); + return reply(getCommonReponseObject(null, robot.getCalibrationData())); } }); diff --git a/software/src/server/calibration.js b/software/src/server/calibration.js deleted file mode 100644 index 649883b..0000000 --- a/software/src/server/calibration.js +++ /dev/null @@ -1,29 +0,0 @@ -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")); - } -}; \ No newline at end of file From 9c4215cbb5fa028c0d6090859537c548b5d88202 Mon Sep 17 00:00:00 2001 From: Dan Cuellar Date: Sun, 22 Mar 2015 19:00:53 +0000 Subject: [PATCH 3/5] Adding method to tap screen coordinates and device calibration --- software/package.json | 4 +- software/src/calibrate.js | 252 +++++++++++++++---- software/src/calibration.json | 1 + software/src/lib/server/calibration.js | 2 +- software/src/lib/server/robot.js | 54 ++++ software/src/lib/server/robot_http_client.js | 25 +- software/src/server.js | 23 ++ 7 files changed, 288 insertions(+), 73 deletions(-) create mode 100644 software/src/calibration.json 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..052f803 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, -145, function (screenX, screenY, robotZ) { + newCalibrationData.device.contactPoint.position = {x: 0, y: 0, z:robotZ}; + newCalibrationData.device.contactPoint.screenCoordinates = {x: screenX, y: screenY}; + return lowerAndCheckForContact(50, 50, robotZ *.95, function (screenX, screenY, robotZ) { + newCalibrationData.device.point1.position = {x: 20, y: 20, z:robotZ}; + newCalibrationData.device.point1.screenCoordinates = {x: screenX, y: screenY}; + return lowerAndCheckForContact(-50, -50, robotZ *.95, 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/calibration.js b/software/src/lib/server/calibration.js index f90b35b..64922cd 100644 --- a/software/src/lib/server/calibration.js +++ b/software/src/lib/server/calibration.js @@ -5,7 +5,7 @@ module.exports.defaultData = { restPoint : { x : 0, y : 0, - z : -120 + z : -130 }, servo1 : { minimumAngle : 20, diff --git a/software/src/lib/server/robot.js b/software/src/lib/server/robot.js index f752da3..de1a6bd 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 generateTranslationMatrix = 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,31 @@ method.getAnglesForPosition = function(x,y,z) { return [angles[1], angles[2], angles[3]]; }; +method.getPositionForScreenCoordinates = function(x,y) { + var matrix = generateTranslationMatrix(this._calibration); + 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 = 1.01 * Math.min( + this._calibration.device.contactPoint.position.z, + this._calibration.device.point1.position.z, + this._calibration.device.point2.position.z); + this.setPosition(position.x, position.y, touchZ * 0.9); + setTimeout(function() { + this.setPosition(position.x, position.y, 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', From c56f2328bd5d9e60d0dff42044060e160ded4082 Mon Sep 17 00:00:00 2001 From: Dan Cuellar Date: Thu, 16 Apr 2015 16:01:04 +0100 Subject: [PATCH 4/5] Fixing Calibration --- software/src/calibrate.js | 19 ++++++++------- software/src/calibration.json | 2 +- software/src/lib/server/robot.js | 40 +++++++++++++++++++++++++++++--- 3 files changed, 49 insertions(+), 12 deletions(-) diff --git a/software/src/calibrate.js b/software/src/calibrate.js index 052f803..7517654 100755 --- a/software/src/calibrate.js +++ b/software/src/calibrate.js @@ -165,12 +165,14 @@ var calibrateDevice = function(cb) { }; 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); }); @@ -185,7 +187,8 @@ var calibrateDevice = function(cb) { var match = coordRegex.exec(pageSource); var screenX = parseFloat(match[1]); var screenY = parseFloat(match[2]); - return cb(screenX,screenY, currentZ); + console.log("Found Point: (" + x + "," + y + ") => (" + screenX + "," + screenY + ")"); + return cb(x,y,screenX,screenY,currentZ); } else { if (currentZ < -150) { return robot.reset(function() { @@ -241,16 +244,16 @@ var calibrateDevice = function(cb) { udid: "481309bbf8a3c341687e617bb7104be41f3abb07" }, function() { driver.setImplicitWaitTimeout(1000, function () { - return lowerAndCheckForContact(0, 0, -145, function (screenX, screenY, robotZ) { - newCalibrationData.device.contactPoint.position = {x: 0, y: 0, z:robotZ}; + return lowerAndCheckForContact(0, 0, -145, function (x, y, screenX, screenY, robotZ) { + newCalibrationData.device.contactPoint.position = {x: x, y: y, z:robotZ}; newCalibrationData.device.contactPoint.screenCoordinates = {x: screenX, y: screenY}; - return lowerAndCheckForContact(50, 50, robotZ *.95, function (screenX, screenY, robotZ) { - newCalibrationData.device.point1.position = {x: 20, y: 20, z:robotZ}; + return lowerAndCheckForContact(0, 20, -145, function (x, y, screenX, screenY, robotZ) { + newCalibrationData.device.point1.position = {x: x, y: y, z:robotZ}; newCalibrationData.device.point1.screenCoordinates = {x: screenX, y: screenY}; - return lowerAndCheckForContact(-50, -50, robotZ *.95, function (screenX, size, robotZ) { - newCalibrationData.device.point2.position = {x: -10, y: -15, z:robotZ}; + return lowerAndCheckForContact(20, 0, -145, function (x, y, screenX, screenY, robotZ) { + newCalibrationData.device.point2.position = {x: x, y: y, z:robotZ}; newCalibrationData.device.point2.screenCoordinates = {x: screenX, y: screenY}; - return cb(); + return robot.reset(cb); }); }); }); diff --git a/software/src/calibration.json b/software/src/calibration.json index 5a0175f..323f608 100644 --- a/software/src/calibration.json +++ b/software/src/calibration.json @@ -1 +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 +{"restPoint":{"x":0,"y":0,"z":-130},"servo1":{"minimumAngle":20,"maximumAngle":90},"servo2":{"minimumAngle":20,"maximumAngle":90},"servo3":{"minimumAngle":20,"maximumAngle":90},"device":{"contactPoint":{"position":{"x":0,"y":0,"z":-149},"screenCoordinates":{"x":175,"y":294}},"point1":{"position":{"x":0,"y":20,"z":-149},"screenCoordinates":{"x":179,"y":198}},"point2":{"position":{"x":20,"y":0,"z":-149},"screenCoordinates":{"x":270,"y":290}}}} diff --git a/software/src/lib/server/robot.js b/software/src/lib/server/robot.js index de1a6bd..198b00a 100644 --- a/software/src/lib/server/robot.js +++ b/software/src/lib/server/robot.js @@ -13,6 +13,28 @@ function Robot(servo1, servo2, servo3, calibration) { } var generateTranslationMatrix = function(calibration) { + + var r1x = calibration.device.contactPoint.position.x; + var r1y = calibration.device.contactPoint.position.y; + var r2x = calibration.device.point1.position.x; + var r2y = calibration.device.point1.position.y; + var r3x = calibration.device.point2.position.x; + var r3y = calibration.device.point2.position.y; + + var d1x = calibration.device.contactPoint.screenCoordinates.x; + var d1y = calibration.device.contactPoint.screenCoordinates.y; + var d2x = calibration.device.point1.screenCoordinates.x; + var d2y = calibration.device.point1.screenCoordinates.y; + var d3x = calibration.device.point2.screenCoordinates.x; + var d3y = calibration.device.point2.screenCoordinates.y; + + var deviceXVector = $M([[(d3x-d1x) / (r3x-r1x)], [(d3y-d1y) / (r3x-r1x) ]]); + var deviceYVector = $M([[(d2x-d1x) / (r2y-r1y)], [(d2y-d1y) / (r2y-r1y) ]]); + var offset = $M([d1x-r1x, d1y-r1y]); + var r2dMatrix = $M([[deviceXVector.elements[0], deviceYVector.elements[0]], [deviceXVector.elements[1], deviceYVector.elements[1]]]); + return {offset: offset, matrix: r2dMatrix}; + /* + var b0x = calibration.device.point1.position.x, b0y = calibration.device.point1.position.y, b1x = calibration.device.point2.position.x, @@ -35,7 +57,7 @@ var generateTranslationMatrix = function(calibration) { [b1y] ]); var MI = M.inverse(); - return MI.multiply(u); + return MI.multiply(u);*/ }; var sin = function(degree) { @@ -85,6 +107,7 @@ method.getPosition = function() { }; method.setPosition = function(x, y, z) { + console.log("Setting Position:" + [x,y,z]); var reflected = reflect(x,y); var rotated = rotate(reflected[0],reflected[1]); var angles = kinematics.inverse(rotated[0], rotated[1], z); @@ -109,6 +132,15 @@ method.getAnglesForPosition = function(x,y,z) { }; method.getPositionForScreenCoordinates = function(x,y) { + var calData = generateTranslationMatrix(this._calibration); + var matrix = calData.matrix; + var offset = calData.offset; + var vector = $M([ [x-offset.elements[0]],[y-offset.elements[1]] ]); + var converted = matrix.inverse().multiply(vector); + var newX = converted.elements[0]; + var newY = converted.elements[1]; + return {x:newX, y:newY}; + /* var matrix = generateTranslationMatrix(this._calibration); var a = matrix.elements[0][0], b = matrix.elements[1][0], @@ -116,12 +148,14 @@ method.getPositionForScreenCoordinates = function(x,y) { d = matrix.elements[3][0]; var yprime = a * x + b * y + c; var xprime = b * x - a * y + d; + console.log("(" + x +"," + y +") => (" + xprime + "," + yprime +")"); return {x:xprime, y:yprime}; + */ }; method.tap = function(screenX, screenY) { - var position = this.getPositionForScreenCoordinates(screenX, screenY); - var touchZ = 1.01 * Math.min( + var position = this.getPositionForScreenCoordinates(screenX, screenY); + var touchZ = 1.01 * Math.min( this._calibration.device.contactPoint.position.z, this._calibration.device.point1.position.z, this._calibration.device.point2.position.z); From 46b33a14126ac5f2a7a7f188cd71acb0f593a056 Mon Sep 17 00:00:00 2001 From: Dan Cuellar Date: Sun, 19 Apr 2015 15:02:39 +0100 Subject: [PATCH 5/5] Adding SendKeys And Swipe Implementations --- software/src/calibration.json | 2 +- software/src/lib/server/keyboards.js | 142 +++++++++++++++++++++++++++ software/src/lib/server/robot.js | 66 ++++++++++--- software/src/server.js | 32 +++++- 4 files changed, 227 insertions(+), 15 deletions(-) create mode 100644 software/src/lib/server/keyboards.js diff --git a/software/src/calibration.json b/software/src/calibration.json index 323f608..d042a88 100644 --- a/software/src/calibration.json +++ b/software/src/calibration.json @@ -1 +1 @@ -{"restPoint":{"x":0,"y":0,"z":-130},"servo1":{"minimumAngle":20,"maximumAngle":90},"servo2":{"minimumAngle":20,"maximumAngle":90},"servo3":{"minimumAngle":20,"maximumAngle":90},"device":{"contactPoint":{"position":{"x":0,"y":0,"z":-149},"screenCoordinates":{"x":175,"y":294}},"point1":{"position":{"x":0,"y":20,"z":-149},"screenCoordinates":{"x":179,"y":198}},"point2":{"position":{"x":20,"y":0,"z":-149},"screenCoordinates":{"x":270,"y":290}}}} +{"restPoint":{"x":0,"y":0,"z":-130},"servo1":{"minimumAngle":20,"maximumAngle":90},"servo2":{"minimumAngle":20,"maximumAngle":90},"servo3":{"minimumAngle":20,"maximumAngle":90},"device":{"contactPoint":{"position":{"x":0,"y":0,"z":-149},"screenCoordinates":{"x":184,"y":292}},"point1":{"position":{"x":0,"y":20,"z":-149},"screenCoordinates":{"x":188,"y":195}},"point2":{"position":{"x":20,"y":0,"z":-149},"screenCoordinates":{"x":280,"y":285}}}} \ No newline at end of file diff --git a/software/src/lib/server/keyboards.js b/software/src/lib/server/keyboards.js new file mode 100644 index 0000000..5e79b85 --- /dev/null +++ b/software/src/lib/server/keyboards.js @@ -0,0 +1,142 @@ +var keymaps = {}; + +keymaps['iPhone 6'] = { + q: { position: {x: 0, y: 363}, size: {width: 32, height:42 }, modifiers:[]}, + w: { position: {x: 32, y: 363}, size: {width: 32, height:42 }, modifiers:[]}, + e: { position: {x: 64, y: 363}, size: {width: 32, height:42 }, modifiers:[]}, + r: { position: {x: 96, y: 363}, size: {width: 32, height:42 }, modifiers:[]}, + t: { position: {x: 128, y: 363}, size: {width: 32, height:42 }, modifiers:[]}, + y: { position: {x: 160, y: 363}, size: {width: 32, height:42 }, modifiers:[]}, + u: { position: {x: 192, y: 363}, size: {width: 32, height:42 }, modifiers:[]}, + i: { position: {x: 224, y: 363}, size: {width: 32, height:42 }, modifiers:[]}, + o: { position: {x: 256, y: 363}, size: {width: 32, height:42 }, modifiers:[]}, + p: { position: {x: 288, y: 363}, size: {width: 32, height:42 }, modifiers:[]}, + a: { position: {x: 16, y: 417}, size: {width: 32, height:42 }, modifiers:[]}, + s: { position: {x: 48, y: 417}, size: {width: 32, height:42 }, modifiers:[]}, + d: { position: {x: 80, y: 417}, size: {width: 32, height:42 }, modifiers:[]}, + f: { position: {x: 112, y: 417}, size: {width: 32, height:42 }, modifiers:[]}, + g: { position: {x: 144, y: 417}, size: {width: 32, height:42 }, modifiers:[]}, + h: { position: {x: 176, y: 417}, size: {width: 32, height:42 }, modifiers:[]}, + j: { position: {x: 208, y: 417}, size: {width: 32, height:42 }, modifiers:[]}, + k: { position: {x: 240, y: 417}, size: {width: 32, height:42 }, modifiers:[]}, + l: { position: {x: 272, y: 417}, size: {width: 32, height:42 }, modifiers:[]}, + z: { position: {x: 48, y: 471}, size: {width: 32, height:42 }, modifiers:[]}, + x: { position: {x: 80, y: 471}, size: {width: 32, height:42 }, modifiers:[]}, + c: { position: {x: 112, y: 471}, size: {width: 32, height:42 }, modifiers:[]}, + v: { position: {x: 144, y: 471}, size: {width: 32, height:42 }, modifiers:[]}, + b: { position: {x: 176, y: 471}, size: {width: 32, height:42 }, modifiers:[]}, + n: { position: {x: 208, y: 471}, size: {width: 32, height:42 }, modifiers:[]}, + m: { position: {x: 240, y: 471}, size: {width: 32, height:42 }, modifiers:[]}, + Q: { position: {x: 0, y: 363}, size: {width: 32, height:42 }, modifiers:["shift"]}, + W: { position: {x: 32, y: 363}, size: {width: 32, height:42 }, modifiers:["shift"]}, + E: { position: {x: 64, y: 363}, size: {width: 32, height:42 }, modifiers:["shift"]}, + R: { position: {x: 96, y: 363}, size: {width: 32, height:42 }, modifiers:["shift"]}, + T: { position: {x: 128, y: 363}, size: {width: 32, height:42 }, modifiers:["shift"]}, + Y: { position: {x: 160, y: 363}, size: {width: 32, height:42 }, modifiers:["shift"]}, + U: { position: {x: 192, y: 363}, size: {width: 32, height:42 }, modifiers:["shift"]}, + I: { position: {x: 224, y: 363}, size: {width: 32, height:42 }, modifiers:["shift"]}, + O: { position: {x: 256, y: 363}, size: {width: 32, height:42 }, modifiers:["shift"]}, + P: { position: {x: 288, y: 363}, size: {width: 32, height:42 }, modifiers:["shift"]}, + A: { position: {x: 16, y: 417}, size: {width: 32, height:42 }, modifiers:["shift"]}, + S: { position: {x: 48, y: 417}, size: {width: 32, height:42 }, modifiers:["shift"]}, + D: { position: {x: 80, y: 417}, size: {width: 32, height:42 }, modifiers:["shift"]}, + F: { position: {x: 112, y: 417}, size: {width: 32, height:42 }, modifiers:["shift"]}, + G: { position: {x: 144, y: 417}, size: {width: 32, height:42 }, modifiers:["shift"]}, + H: { position: {x: 176, y: 417}, size: {width: 32, height:42 }, modifiers:["shift"]}, + J: { position: {x: 208, y: 417}, size: {width: 32, height:42 }, modifiers:["shift"]}, + K: { position: {x: 240, y: 417}, size: {width: 32, height:42 }, modifiers:["shift"]}, + L: { position: {x: 272, y: 417}, size: {width: 32, height:42 }, modifiers:["shift"]}, + Z: { position: {x: 48, y: 471}, size: {width: 32, height:42 }, modifiers:["shift"]}, + X: { position: {x: 80, y: 471}, size: {width: 32, height:42 }, modifiers:["shift"]}, + C: { position: {x: 112, y: 471}, size: {width: 32, height:42 }, modifiers:["shift"]}, + V: { position: {x: 144, y: 471}, size: {width: 32, height:42 }, modifiers:["shift"]}, + B: { position: {x: 176, y: 471}, size: {width: 32, height:42 }, modifiers:["shift"]}, + N: { position: {x: 208, y: 471}, size: {width: 32, height:42 }, modifiers:["shift"]}, + M: { position: {x: 240, y: 471}, size: {width: 32, height:42 }, modifiers:["shift"]}, + 1: { position: {x: 0, y: 363}, size: {width: 32, height:42 }, modifiers:["numberToggle"]}, + 2: { position: {x: 32, y: 363}, size: {width: 32, height:42 }, modifiers:["numberToggle"]}, + 3: { position: {x: 64, y: 363}, size: {width: 32, height:42 }, modifiers:["numberToggle"]}, + 4: { position: {x: 96, y: 363}, size: {width: 32, height:42 }, modifiers:["numberToggle"]}, + 5: { position: {x: 128, y: 363}, size: {width: 32, height:42 }, modifiers:["numberToggle"]}, + 6: { position: {x: 160, y: 363}, size: {width: 32, height:42 }, modifiers:["numberToggle"]}, + 7: { position: {x: 192, y: 363}, size: {width: 32, height:42 }, modifiers:["numberToggle"]}, + 8: { position: {x: 224, y: 363}, size: {width: 32, height:42 }, modifiers:["numberToggle"]}, + 9: { position: {x: 256, y: 363}, size: {width: 32, height:42 }, modifiers:["numberToggle"]}, + 0: { position: {x: 288, y: 363}, size: {width: 32, height:42 }, modifiers:["numberToggle"]}, + '-': { position: {x: 0, y: 417}, size: {width: 32, height:42 }, modifiers:["numberToggle"]}, + '/': { position: {x: 32, y: 417}, size: {width: 32, height:42 }, modifiers:["numberToggle"]}, + ':': { position: {x: 64, y: 417}, size: {width: 32, height:42 }, modifiers:["numberToggle"]}, + ';': { position: {x: 96, y: 417}, size: {width: 32, height:42 }, modifiers:["numberToggle"]}, + '(': { position: {x: 128, y: 417}, size: {width: 32, height:42 }, modifiers:["numberToggle"]}, + ')': { position: {x: 160, y: 417}, size: {width: 32, height:42 }, modifiers:["numberToggle"]}, + '$': { position: {x: 192, y: 417}, size: {width: 32, height:42 }, modifiers:["numberToggle"]}, + '&': { position: {x: 224, y: 417}, size: {width: 32, height:42 }, modifiers:["numberToggle"]}, + '@': { position: {x: 256, y: 417}, size: {width: 32, height:42 }, modifiers:["numberToggle"]}, + '"': { position: {x: 288, y: 417}, size: {width: 32, height:42 }, modifiers:["numberToggle"]}, + '.': { position: {x: 48, y: 471}, size: {width: 45, height:42 }, modifiers:["numberToggle"]}, + ',': { position: {x: 92, y: 471}, size: {width: 46, height:42 }, modifiers:["numberToggle"]}, + '?': { position: {x: 137, y: 471}, size: {width: 46, height:42 }, modifiers:["numberToggle"]}, + '!': { position: {x: 182, y: 471}, size: {width: 46, height:42 }, modifiers:["numberToggle"]}, + "'": { position: {x: 227, y: 471}, size: {width: 45, height:42 }, modifiers:["numberToggle"]}, + '[': { position: {x: 0, y: 363}, size: {width: 32, height:42 }, modifiers:["numberToggle", "shift"]}, + ']': { position: {x: 32, y: 363}, size: {width: 32, height:42 }, modifiers:["numberToggle", "shift"]}, + '{': { position: {x: 64, y: 363}, size: {width: 32, height:42 }, modifiers:["numberToggle", "shift"]}, + '}': { position: {x: 96, y: 363}, size: {width: 32, height:42 }, modifiers:["numberToggle", "shift"]}, + '#': { position: {x: 128, y: 363}, size: {width: 32, height:42 }, modifiers:["numberToggle", "shift"]}, + '%': { position: {x: 160, y: 363}, size: {width: 32, height:42 }, modifiers:["numberToggle", "shift"]}, + '^': { position: {x: 192, y: 363}, size: {width: 32, height:42 }, modifiers:["numberToggle", "shift"]}, + '*': { position: {x: 224, y: 363}, size: {width: 32, height:42 }, modifiers:["numberToggle", "shift"]}, + '+': { position: {x: 256, y: 363}, size: {width: 32, height:42 }, modifiers:["numberToggle", "shift"]}, + '=': { position: {x: 288, y: 363}, size: {width: 32, height:42 }, modifiers:["numberToggle", "shift"]}, + '_': { position: {x: 0, y: 417}, size: {width: 32, height:42 }, modifiers:["numberToggle", "shift"]}, + '\\': { position: {x: 32, y: 417}, size: {width: 32, height:42 }, modifiers:["numberToggle", "shift"]}, + '|': { position: {x: 64, y: 417}, size: {width: 32, height:42 }, modifiers:["numberToggle", "shift"]}, + '~': { position: {x: 96, y: 417}, size: {width: 32, height:42 }, modifiers:["numberToggle", "shift"]}, + '<': { position: {x: 128, y: 417}, size: {width: 32, height:42 }, modifiers:["numberToggle", "shift"]}, + '>': { position: {x: 160, y: 417}, size: {width: 32, height:42 }, modifiers:["numberToggle", "shift"]}, + '€': { position: {x: 192, y: 417}, size: {width: 32, height:42 }, modifiers:["numberToggle", "shift"]}, + '£': { position: {x: 224, y: 417}, size: {width: 32, height:42 }, modifiers:["numberToggle", "shift"]}, + '•': { position: {x: 256, y: 417}, size: {width: 32, height:42 }, modifiers:["numberToggle", "shift"]}, + '¥': { position: {x: 288, y: 417}, size: {width: 32, height:42 }, modifiers:["numberToggle", "shift"]}, + shift: { position: {x: 20, y: 471}, size: {width: 22, height:42 }, modifiers:[]}, + backspace: { position: {x: 278, y: 471}, size: {width: 42, height:42 }, modifiers:[]}, + numberToggle: { position: {x: 20, y: 525}, size: {width: 20, height:37 }, modifiers:[]}, + keyboardToggle: { position: {x: 40, y: 525}, size: {width: 40, height:37 }, modifiers:[]}, + dictation: { position: {x: 80, y: 525}, size: {width: 32, height:37 }, modifiers:[]}, + ' ': { position: {x: 112, y: 525}, size: {width: 128, height:37 }, modifiers:[]}, + done: { position: {x: 240, y: 525}, size: {width: 80, height:37 }, modifiers:[]} +}; + + +var keyboardModule = { + getKeyboard: function (deviceName) { + var keyboard = {}; + keyboard.keyMap = keymaps[deviceName]; + keyboard.getKeySequence = function (key) { + var sequence = []; + var postSequence = []; + var keyMapping = keyboard.keyMap[key]; + for (var modIndex=0; modIndex < keyMapping.modifiers.length; modIndex++) { + var modKeyName = keyMapping.modifiers[modIndex]; + var modKey = keyboard.keyMap[modKeyName]; + var keyPosition = { + x: modKey.position.x + (modKey.size.width / 2), + y: modKey.position.y + (modKey.size.height / 2) + }; + sequence.push(keyPosition); + if (modKeyName == "numberToggle") { + postSequence.push(keyPosition); + } + } + sequence.push({ + x: keyMapping.position.x + (keyMapping.size.width / 2), + y: keyMapping.position.y + (keyMapping.size.height / 2) + }); + sequence = sequence.concat(postSequence); + return sequence; + }; + return keyboard; + } +}; + +module.exports = keyboardModule; \ No newline at end of file diff --git a/software/src/lib/server/robot.js b/software/src/lib/server/robot.js index 198b00a..3671023 100644 --- a/software/src/lib/server/robot.js +++ b/software/src/lib/server/robot.js @@ -1,7 +1,7 @@ var kinematics = require("./../kinematics"); require("sylvester"); - +var keyboards = require("./keyboards"); var method = Robot.prototype; function Robot(servo1, servo2, servo3, calibration) { @@ -153,19 +153,61 @@ method.getPositionForScreenCoordinates = function(x,y) { */ }; -method.tap = function(screenX, screenY) { - var position = this.getPositionForScreenCoordinates(screenX, screenY); - var touchZ = 1.01 * Math.min( - this._calibration.device.contactPoint.position.z, - this._calibration.device.point1.position.z, - this._calibration.device.point2.position.z); +method.getContactZ = function() { + return 1.01 * Math.min( + this._calibration.device.contactPoint.position.z, + this._calibration.device.point1.position.z, + this._calibration.device.point2.position.z + ); +}; + +method.tap = function(screenX, screenY, cb) { + var position = this.getPositionForScreenCoordinates(screenX, screenY); + var touchZ = this.getContactZ(); this.setPosition(position.x, position.y, touchZ * 0.9); - setTimeout(function() { + return setTimeout(function() { this.setPosition(position.x, position.y, touchZ); - setTimeout(function() { - this.resetPosition(); - }.bind(this), 1000); - }.bind(this), 1500); + return setTimeout(function() { + this.setPosition(position.x, position.y, touchZ * 0.9); + return setTimeout(cb, 400); + }.bind(this), 400); + }.bind(this), 400); +}; + +method.swipe = function(startX, startY, endX, endY, cb) { + var startPosition = this.getPositionForScreenCoordinates(startX, startY); + var endPosition = this.getPositionForScreenCoordinates(endX, endY); + var touchZ = this.getContactZ(); + this.setPosition(startPosition.x, startPosition.y, touchZ * 0.9); + return setTimeout(function() { + this.setPosition(startPosition.x, startPosition.y, touchZ); + return setTimeout(function() { + this.setPosition(endPosition.x, endPosition.y, touchZ); + return setTimeout(function() { + this.setPosition(endPosition.x, endPosition.y, touchZ * 0.9); + return setTimeout(cb, 100); + }.bind(this), 400); + }.bind(this), 400); + }.bind(this), 400); +}; + +method.sendKeys = function(keys, cb) { + var keyboard = keyboards.getKeyboard("iPhone 6"/*this._calibration.device.name*/); + var keystrokeSequence = []; + for (var keyIndex=0; keyIndex < keys.length; keyIndex++) { + keystrokeSequence = keystrokeSequence.concat(keyboard.getKeySequence(keys[keyIndex])); + } + var tapKey = function(keystrokes, cb) { + if (keystrokes.length == 0) { + return cb(); + } else { + var currentKeyPosition = keystrokes.shift(); + this.tap(currentKeyPosition.x, currentKeyPosition.y, function() { + return tapKey(keystrokes, cb); + }); + } + }.bind(this); + return tapKey(keystrokeSequence, cb); }; method.startDancing = function() { diff --git a/software/src/server.js b/software/src/server.js index d7347c0..ab9a82f 100755 --- a/software/src/server.js +++ b/software/src/server.js @@ -173,8 +173,36 @@ board.on("ready", function() { 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"')); + return robot.tap(x,y,function() { + return reply(getCommonReponseObject(null, '"OK"')); + }); + } + }); + + server.route({ + method: 'POST', + path:'/swipe', + handler: function (request, reply) { + console.log("POST " + request.path + ": "); + var startX = parseFloat(request.payload.startX); + var startY = parseFloat(request.payload.startY); + var endX = parseFloat(request.payload.endX); + var endY = parseFloat(request.payload.endY); + return robot.swipe(startX,startY,endX,endY,function() { + return reply(getCommonReponseObject(null, '"OK"')); + }); + } + }); + + server.route({ + method: 'POST', + path:'/sendKeys', + handler: function (request, reply) { + console.log("POST " + request.path + ": "); + var keys = decodeURIComponent(request.payload.keys); + return robot.sendKeys(keys, function() { + return reply(getCommonReponseObject(null, '"OK"')); + }); } });