Skip to content

Commit

Permalink
Adding method to tap screen coordinates and device calibration
Browse files Browse the repository at this point in the history
  • Loading branch information
@penguinho
penguinho committed Mar 24, 2015
1 parent 306f79b commit 9c4215c
Show file tree
Hide file tree
Showing 7 changed files with 288 additions and 73 deletions.
4 changes: 3 additions & 1 deletion software/package.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
}
}
252 changes: 202 additions & 50 deletions software/src/calibrate.js
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 = {};
Expand Down Expand Up @@ -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();
});
});
});
});
});
};
Expand Down
1 change: 1 addition & 0 deletions software/src/calibration.json
View file
Original file line number Diff line number Diff line change
@@ -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}}
2 changes: 1 addition & 1 deletion software/src/lib/server/calibration.js
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@ module.exports.defaultData = {
restPoint : {
x : 0,
y : 0,
z : -120
z : -130
},
servo1 : {
minimumAngle : 20,
Expand Down
54 changes: 54 additions & 0 deletions software/src/lib/server/robot.js
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,7 @@
var kinematics = require("./../kinematics");

require("sylvester");

var method = Robot.prototype;

function Robot(servo1, servo2, servo3, calibration) {
Expand All @@ -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));
};
Expand Down Expand Up @@ -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() {
Expand Down
Loading

0 comments on commit 9c4215c

Please sign in to comment.