From 630cbd1bf7c7ef9660aa2fe8163f4ae9edb4a8a3 Mon Sep 17 00:00:00 2001 From: jackskalitzky Date: Fri, 12 Jun 2015 16:12:15 -0500 Subject: [PATCH 01/43] Create hi.js --- software/src/demo/hi.js | 70 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 70 insertions(+) create mode 100644 software/src/demo/hi.js diff --git a/software/src/demo/hi.js b/software/src/demo/hi.js new file mode 100644 index 0000000..768f5bd --- /dev/null +++ b/software/src/demo/hi.js @@ -0,0 +1,70 @@ +five = require("johnny-five"); +bot = require("bot"); +board = new five.Board({ + debug: false +}); + +board.on("ready", function() { + // Setup + servo1 = five.Servo({ + pin: 9, + range: [0,90] + }); + servo2 = five.Servo({ + pin: 10, + range: [0,90] + }); + 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); + }) + + board.repl.inject({ + servo1: servo1, + s1: servo1, + servo2: servo2, + s2: servo2, + servo3: servo3, + s3: servo3, + }); + + bot.go(0, 0, -140); + }); + +hi = function() { + setTimeout(function() { go(-20, 20, -150) }, 0); + setTimeout(function() { go(-20, -20, -150) }, 250); + setTimeout(function() { go(-20, 0, -150) }, 500); + setTimeout(function() { go(-10, 0, -150) }, 750) + setTimeout(function() { go(-10, 20, -150) }, 1000); + setTimeout(function() { go(-10, -20, -150) }, 1250); + + setTimeout(function() { go(-10, -20, -140) }, 1500); + setTimeout(function() { go(-5, -20, -140) }, 1750) + setTimeout(function() { go(-5, -20, -150) }, 2000); + setTimeout(function() { go(-5, 0, -150) }, 2250); + setTimeout(function() { go(-5, 0, -140) }, 2500); + setTimeout(function() { go(-5, 10, -140) }, 2750); + setTimeout(function() { go(-5, 10, -150) }, 3000); + setTimeout(function() { go(-5, 15, -150) }, 3250); + + setTimeout(function() { go(-5, 20, -140) }, 3500); + setTimeout(function() { go(5, 20, -140) }, 3750); + setTimeout(function() { go(5, 20, -150) }, 4000); + setTimeout(function() { go(5, -10, -150) }, 4250); + setTimeout(function() { go(5, -10, -140) }, 4500); + setTimeout(function() { go(5, -15, -140) }, 4750); + setTimeout(function() { go(5, -15, -150) }, 5000); + setTimeout(function() { go(5, -20, -150) }, 5250); + setTimeout(function() { go(5, 0, -140) }, 5500); + } From 5e4b41e172b7d2ddea8bd51d6b6dabb0bb9d27d5 Mon Sep 17 00:00:00 2001 From: jackskalitzky Date: Fri, 12 Jun 2015 16:16:28 -0500 Subject: [PATCH 02/43] Added test functions --- software/src/bot.js | 72 ++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 71 insertions(+), 1 deletion(-) diff --git a/software/src/bot.js b/software/src/bot.js index f313d6a..c88d703 100755 --- a/software/src/bot.js +++ b/software/src/bot.js @@ -66,7 +66,7 @@ board.on("ready", function() { } board.repl.inject({ - dance: start_dance, + dance: start_dance, chill: stop_dance }); @@ -120,3 +120,73 @@ position = function() { return ik.forward(servo1.last.degrees, servo2.last.degrees, servo3.last.degrees); } +//Draws a square in order to ensure that everything is working properly +testSquare = function() { + setTimeout(function() { go(-20, 20, -150); }, 0); //Top left + setTimeout(function() { go(20, 20, -150); }, 1000); //Top right + setTimeout(function() { go(20, -20, -150); }, 2000); //Bottom right + setTimeout(function() { go(-20, -20, -150); }, 3000); //Bottom left + setTimeout(function() { go(-20, 20, -150); }, 4000); //Return to start position +} + +//Draws a star to test that the Tapster bot is working properly +testStar = function() { + setTimeout(function() { go(-20, -20, -140); }, 0); //Bottom left + setTimeout(function() { go(0, 20, -140); }, 1000); //Top + setTimeout(function() { go(20, -20, -140); }, 2000); //Bottom right + setTimeout(function() { go(-20, 10, -140); }, 3000); //Left + setTimeout(function() { go(20, 10, -140); }, 4000); //Right + setTimeout(function() { go(-20, -20, -140); }, 5000); //Starting position +} + +testCircle = function() { + var centerX=0; + var centerY=0; + var radius=20; + ; + // an array to save your points + var points=[]; + + // populate array with points along a circle + for (var degree=0; degree<360; degree++){ + var radians = degree * Math.PI/180; + var x = centerX + radius * Math.cos(radians); + var y = centerY + radius * Math.sin(radians); + points.push({x:x,y:y}); + } + + circle = function() { + for (var i=0; i<360; i+=1) { + setTimeout( function(point) { go(point.x, point.y, -141) }, i*2, points[i]); + } + } + + circle(); + } + +//Draws an arbitrary amount of spirals to test that the Tapster bot is working properly +testSpiral = function(spirals) { + var centerX = 0; + var centerY = 0; + var radius = 30; + var x1 = 0; + var y1 = 0; + var points = []; + for (var degree = 0; degree < spirals * 360; degree++) + { + x1 = x1 + 30/(spirals * 360); + y1 = y1 + 30/(spirals * 360); + var radians = degree * Math.PI/180; + var x = centerX + x1 * Math.cos(radians); + var y = centerY + y1 * Math.sin(radians); + points.push({x:x, y:y}); + } + + spiral = function() { + for (var z = 0; z < spirals*360; z++) + { + setTimeout( function(point) { go(point.x, point.y, -140) }, z*2, points[z]); + } + } + spiral(); +} From 70c07dc5a3b4a16025f8b3dd9bc42d7f3179d9aa Mon Sep 17 00:00:00 2001 From: jackskalitzky Date: Tue, 16 Jun 2015 17:52:30 -0500 Subject: [PATCH 03/43] Draw.js, for testing Tapsterbot --- software/src/draw.js | 149 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 149 insertions(+) create mode 100644 software/src/draw.js diff --git a/software/src/draw.js b/software/src/draw.js new file mode 100644 index 0000000..91aeb71 --- /dev/null +++ b/software/src/draw.js @@ -0,0 +1,149 @@ +//Draws lines and shapes. +//> To test: +//> draw.drawSquare() +//> draw.drawStar() +//> draw.drawCircle() +//> draw.drawFromCoordinates() + +var fs = require('fs'); + +//Maps point from canvas to the Tapster coordinate plane +//The conversion is based on the canvas size and the size of the Tapster base +//These can be changed as needed +mapPoints = function(x, y) { + var newX = x; + var newY = y; + + newX = (newX - halfway.x) / widthRatio; + newY = (halfway.y - newY) / heightRatio; + return {x:newX, y:newY}; +}; + +//Adds delays while the Tapster is writing +//The specific delay can be changed if the bot has to go slower or faster +//for a particular segment +doSetTimeout = function(i, z, delay) +{ + setTimeout(function() { go(i.x, i.y, z) }, timer); + timer = timer + delay; +}; + +var jFile = fs.readFileSync('./data.json', 'utf8'); //Reads data from a JSON file of coordinates +var objArr = JSON.parse(jFile); //Creates an array out of the data + +//Specific dimensions of the canvas and Tapster. Change as needed. +//Uses a modified version of the canvas program to make it easier to map from canvas to Tapster. +var canvasHeight = 500; +var canvasWidth = 300; +var phoneHeight = 100; +var phoneWidth = 60; + +//The ratio between the sizes of the canvas and robot. +var heightRatio = canvasHeight / phoneHeight; +var widthRatio = canvasWidth / phoneWidth; + +//The center of the canvas +var halfway = {x:canvasWidth / 2, y:canvasHeight / 2}; + +//A timer variable for use with doSetTimeout() +var timer = 0; + +//Loops through the JSON array +exports.drawFromCoordinates = function() { +for (var i = 0; i < objArr.length; i++) +{ + var x = 0; + if (objArr[i].length > 0) //If there are multiple lines + { + var point = objArr[i][x]; + var transMap = mapPoints(point.x, point.y); + doSetTimeout(transMap, -130, 200); //Moves the arm vertically so that it does not draw a line between the last point of one line and the first point of another + + for (x = 0; x < objArr[i].length; x++) + { + point = objArr[i][x]; + var mapped = mapPoints(point.x, point.y); + doSetTimeout(mapped, -140, 100); + } + transMap = mapPoints(point.x, point.y); + doSetTimeout(transMap, -130, 200); + } + else //Only one line to be drawn + { + point = objArr[i]; + var mapped = mapPoints(point.x, point.y); + doSetTimeout(mapped, -140, 100); +} +} +}; + +//Draws a square in order to ensure that everything is working properly +exports.drawSquare = function() { + setTimeout(function() { go(-20, 20, -150); }, 0); //Top left + setTimeout(function() { go(20, 20, -150); }, 1000); //Top right + setTimeout(function() { go(20, -20, -150); }, 2000); //Bottom right + setTimeout(function() { go(-20, -20, -150); }, 3000); //Bottom left + setTimeout(function() { go(-20, 20, -150); }, 4000); //Return to start position +}; + +//Draws a star to test that the Tapster bot is working properly +exports.drawStar = function() { + setTimeout(function() { go(-20, -20, -140); }, 0); //Bottom left + setTimeout(function() { go(0, 20, -140); }, 1000); //Top + setTimeout(function() { go(20, -20, -140); }, 2000); //Bottom right + setTimeout(function() { go(-20, 10, -140); }, 3000); //Left + setTimeout(function() { go(20, 10, -140); }, 4000); //Right + setTimeout(function() { go(-20, -20, -140); }, 5000); //Starting position +}; + +exports.drawCircle = function() { + var centerX=0; + var centerY=0; + var radius=20; + ; + // an array to save your points + var points=[]; + + // populate array with points along a circle + for (var degree=0; degree<360; degree++){ + var radians = degree * Math.PI/180; + var x = centerX + radius * Math.cos(radians); + var y = centerY + radius * Math.sin(radians); + points.push({x:x,y:y}); + } + + circle = function() { + for (var i=0; i<360; i+=1) { + setTimeout( function(point) { go(point.x, point.y, -141) }, i*2, points[i]); + } + } + + circle(); + }; + +//Draws an arbitrary amount of spirals to test that the Tapster bot is working properly +exports.drawSpiral= function(spirals) { + var centerX = 0; + var centerY = 0; + var radius = 30; + var x1 = 0; + var y1 = 0; + var points = []; + for (var degree = 0; degree < spirals * 360; degree++) + { + x1 = x1 + 30/(spirals * 360); + y1 = y1 + 30/(spirals * 360); + var radians = degree * Math.PI/180; + var x = centerX + x1 * Math.cos(radians); + var y = centerY + y1 * Math.sin(radians); + points.push({x:x, y:y}); + } + + spiral = function() { + for (var z = 0; z < spirals*360; z++) + { + setTimeout( function(point) { go(point.x, point.y, -140) }, z*2, points[z]); + } + } + spiral(); +}; From d35e21178fba8c43e7c37b7e643c7d846170c719 Mon Sep 17 00:00:00 2001 From: jackskalitzky Date: Tue, 16 Jun 2015 17:53:43 -0500 Subject: [PATCH 04/43] Adds access to the Draw.js methods --- software/src/bot.js | 73 ++------------------------------------------- 1 file changed, 2 insertions(+), 71 deletions(-) diff --git a/software/src/bot.js b/software/src/bot.js index c88d703..329baa9 100755 --- a/software/src/bot.js +++ b/software/src/bot.js @@ -1,5 +1,7 @@ five = require("johnny-five"); ik = require("./ik"); +draw = require("./draw"); + board = new five.Board({ debug: false }); @@ -119,74 +121,3 @@ go = function(x, y, z) { position = function() { return ik.forward(servo1.last.degrees, servo2.last.degrees, servo3.last.degrees); } - -//Draws a square in order to ensure that everything is working properly -testSquare = function() { - setTimeout(function() { go(-20, 20, -150); }, 0); //Top left - setTimeout(function() { go(20, 20, -150); }, 1000); //Top right - setTimeout(function() { go(20, -20, -150); }, 2000); //Bottom right - setTimeout(function() { go(-20, -20, -150); }, 3000); //Bottom left - setTimeout(function() { go(-20, 20, -150); }, 4000); //Return to start position -} - -//Draws a star to test that the Tapster bot is working properly -testStar = function() { - setTimeout(function() { go(-20, -20, -140); }, 0); //Bottom left - setTimeout(function() { go(0, 20, -140); }, 1000); //Top - setTimeout(function() { go(20, -20, -140); }, 2000); //Bottom right - setTimeout(function() { go(-20, 10, -140); }, 3000); //Left - setTimeout(function() { go(20, 10, -140); }, 4000); //Right - setTimeout(function() { go(-20, -20, -140); }, 5000); //Starting position -} - -testCircle = function() { - var centerX=0; - var centerY=0; - var radius=20; - ; - // an array to save your points - var points=[]; - - // populate array with points along a circle - for (var degree=0; degree<360; degree++){ - var radians = degree * Math.PI/180; - var x = centerX + radius * Math.cos(radians); - var y = centerY + radius * Math.sin(radians); - points.push({x:x,y:y}); - } - - circle = function() { - for (var i=0; i<360; i+=1) { - setTimeout( function(point) { go(point.x, point.y, -141) }, i*2, points[i]); - } - } - - circle(); - } - -//Draws an arbitrary amount of spirals to test that the Tapster bot is working properly -testSpiral = function(spirals) { - var centerX = 0; - var centerY = 0; - var radius = 30; - var x1 = 0; - var y1 = 0; - var points = []; - for (var degree = 0; degree < spirals * 360; degree++) - { - x1 = x1 + 30/(spirals * 360); - y1 = y1 + 30/(spirals * 360); - var radians = degree * Math.PI/180; - var x = centerX + x1 * Math.cos(radians); - var y = centerY + y1 * Math.sin(radians); - points.push({x:x, y:y}); - } - - spiral = function() { - for (var z = 0; z < spirals*360; z++) - { - setTimeout( function(point) { go(point.x, point.y, -140) }, z*2, points[z]); - } - } - spiral(); -} From b421e7d8374158af5fa330c194e2c13fd14e33bb Mon Sep 17 00:00:00 2001 From: jackskalitzky Date: Tue, 16 Jun 2015 17:56:23 -0500 Subject: [PATCH 05/43] Webpage for canvas --- software/recorder/index.html | 167 +++++++++++++++++++++++++++++++++++ 1 file changed, 167 insertions(+) create mode 100644 software/recorder/index.html diff --git a/software/recorder/index.html b/software/recorder/index.html new file mode 100644 index 0000000..4602082 --- /dev/null +++ b/software/recorder/index.html @@ -0,0 +1,167 @@ + + + + + +Tracing a line with d3.js + + + + +
+
+ + + + + + + + + + From 178a809e06bc253953d97bc763704e8cf8534a1d Mon Sep 17 00:00:00 2001 From: jackskalitzky Date: Tue, 16 Jun 2015 17:56:58 -0500 Subject: [PATCH 06/43] Methods for simplifying drawing --- software/recorder/simplify.js | 133 ++++++++++++++++++++++++++++++++++ 1 file changed, 133 insertions(+) create mode 100644 software/recorder/simplify.js diff --git a/software/recorder/simplify.js b/software/recorder/simplify.js new file mode 100644 index 0000000..fa86746 --- /dev/null +++ b/software/recorder/simplify.js @@ -0,0 +1,133 @@ +/* + (c) 2013, Vladimir Agafonkin + Simplify.js, a high-performance JS polyline simplification library + mourner.github.io/simplify-js +*/ + +(function () { 'use strict'; + +// to suit your point format, run search/replace for '.x' and '.y'; +// for 3D version, see 3d branch (configurability would draw significant performance overhead) + +// square distance between 2 points +function getSqDist(p1, p2) { + + var dx = p1.x - p2.x, + dy = p1.y - p2.y; + + return dx * dx + dy * dy; +} + +// square distance from a point to a segment +function getSqSegDist(p, p1, p2) { + + var x = p1.x, + y = p1.y, + dx = p2.x - x, + dy = p2.y - y; + + if (dx !== 0 || dy !== 0) { + + var t = ((p.x - x) * dx + (p.y - y) * dy) / (dx * dx + dy * dy); + + if (t > 1) { + x = p2.x; + y = p2.y; + + } else if (t > 0) { + x += dx * t; + y += dy * t; + } + } + + dx = p.x - x; + dy = p.y - y; + + return dx * dx + dy * dy; +} +// rest of the code doesn't care about point format + +// basic distance-based simplification +function simplifyRadialDist(points, sqTolerance) { + + var prevPoint = points[0], + newPoints = [prevPoint], + point; + + for (var i = 1, len = points.length; i < len; i++) { + point = points[i]; + + if (getSqDist(point, prevPoint) > sqTolerance) { + newPoints.push(point); + prevPoint = point; + } + } + + if (prevPoint !== point) newPoints.push(point); + + return newPoints; +} + +// simplification using optimized Douglas-Peucker algorithm with recursion elimination +function simplifyDouglasPeucker(points, sqTolerance) { + + var len = points.length, + MarkerArray = typeof Uint8Array !== 'undefined' ? Uint8Array : Array, + markers = new MarkerArray(len), + first = 0, + last = len - 1, + stack = [], + newPoints = [], + i, maxSqDist, sqDist, index; + + markers[first] = markers[last] = 1; + + while (last) { + + maxSqDist = 0; + + for (i = first + 1; i < last; i++) { + sqDist = getSqSegDist(points[i], points[first], points[last]); + + if (sqDist > maxSqDist) { + index = i; + maxSqDist = sqDist; + } + } + + if (maxSqDist > sqTolerance) { + markers[index] = 1; + stack.push(first, index, index, last); + } + + last = stack.pop(); + first = stack.pop(); + } + + for (i = 0; i < len; i++) { + if (markers[i]) newPoints.push(points[i]); + } + + return newPoints; +} + +// both algorithms combined for awesome performance +function simplify(points, tolerance, highestQuality) { + + if (points.length <= 1) return points; + + var sqTolerance = tolerance !== undefined ? tolerance * tolerance : 1; + + points = highestQuality ? points : simplifyRadialDist(points, sqTolerance); + points = simplifyDouglasPeucker(points, sqTolerance); + + return points; +} + +// export as AMD module / Node module / browser or worker variable +if (typeof define === 'function' && define.amd) define(function() { return simplify; }); +else if (typeof module !== 'undefined') module.exports = simplify; +else if (typeof self !== 'undefined') self.simplify = simplify; +else window.simplify = simplify; + +})(); From 3ad0f6e72a072591fae40a16c6a2f05b969fb02a Mon Sep 17 00:00:00 2001 From: jackskalitzky Date: Tue, 23 Jun 2015 11:34:30 -0500 Subject: [PATCH 07/43] Configuration file for Tapster --- software/config.js | 27 +++++++++++++++++++++++++++ 1 file changed, 27 insertions(+) create mode 100644 software/config.js diff --git a/software/config.js b/software/config.js new file mode 100644 index 0000000..3f295f0 --- /dev/null +++ b/software/config.js @@ -0,0 +1,27 @@ +var config = {} + +//Side of end effector +config.e = 34.64101615137754; // Math.sqrt(3) * 10 * 2 + +//Side of top triangle +config.f = 110.85125168440814; // Math.sqrt(3) * 32 * 2 + +//Length of parallelogram joint +config.re = 153.5; // 145 + 8.5 + +//Length of upper joint +config.rf = 52.690131903421914; // Math.sqrt(52**2 + 8.5**2) + +//Calibrated servo values +config.servo1 = {in_min: 0, in_max: 90, out_min: 11, out_max: 92}; +config.servo2 = {in_min: 0, in_max: 90, out_min: 9, out_max: 88}; +config.servo3 = {in_min: 0, in_max: 90, out_min: 10, out_max: 90.5}; + +//Dimensions of the base plate +config.baseHeight = 95; +config.baseWidth = 80; + +//Default Z-level of the pen +config.penHeight = -140; + +module.exports = config; From 8c0a4263501f748485b28ceaf6ec18ef58aaa81b Mon Sep 17 00:00:00 2001 From: jackskalitzky Date: Tue, 23 Jun 2015 11:36:19 -0500 Subject: [PATCH 08/43] Script for parsing SVG path data --- software/src/SVGReader.js | 506 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 506 insertions(+) create mode 100644 software/src/SVGReader.js diff --git a/software/src/SVGReader.js b/software/src/SVGReader.js new file mode 100644 index 0000000..fcfdd4b --- /dev/null +++ b/software/src/SVGReader.js @@ -0,0 +1,506 @@ +//Draws stuff from SVG files +//Built with InkScape in mind, but should support: +//http://svg-edit.googlecode.com/svn/branches/stable/editor/svg-editor.html + +//> To test: +//> svg.draw("filename.svg") + +var parse = require('svg-path-parser'); +var fs = require('fs'); +var config = require('../config.js'); +var parseString = require('xml2js').parseString; + +//filename must be a string. ".svg" is optional +//Right now, the file is assumed to be in the software folder +//To-do: Add support for arbitrary file locations +exports.draw = function(filename) { + var parsed; + + if (filename.search(".svg") == -1) //Ensures that filename is valid and points to an .svg + filename += ".svg"; + + //Create a JSON string out of the SVG image data + //parseString strips away the XML data + + try { + parseString(fs.readFileSync("../" + filename, "utf8"), function(err, result) { + parsed = JSON.stringify(result, null, 1); + }); + } catch (e) { + if (e.code === "ENOENT") + console.log("File not found."); + else + throw e; + + return; + } + + //Parse the JSON string into an array + objArr = JSON.parse(parsed); + + var svgDimensions = dimensionConversion(objArr.svg.$.width, objArr.svg.$.height); + width = svgDimensions.width; + height = svgDimensions.height; + + transformX = 0; + transformY = 0; + + //Check for translation and account for it + //To-do: Do this more elegantly + if (objArr.svg.g[0].$ && objArr.svg.g[0].$.transform) { //Done in multiple checks to avoid errors being thrown + var transString = objArr.svg.g[0].$.transform; + var subX = transString.indexOf("("); + transformX = parseInt(transString.substring(subX + 1)); + var subY = transString.indexOf(","); + transformY = parseInt(transString.substring(subY + 1)); + } + + var phoneWidth = config.baseWidth; + var phoneHeight = config.baseHeight; + penHeight = config.penHeight; + + widthRatio = width / phoneWidth; + heightRatio = height / phoneHeight; + + halfway = {x:width / 2, y:height / 2}; + currentPoint = {x:halfway.x, y:halfway.y}; //Start at the center of the canvas, which corresponds to (0,0) on the Tapster + + timer = 0; + + if (objArr.svg.g[0].g) { //If there are multiple groups + for (var i = 0; i < objArr.svg.g[0].g.length; i++) { + pathArray = objArr.svg.g[0].g[i].path; + drawSVG(); + } + } + + else if (objArr.svg.g[0].path) { + pathArray = objArr.svg.g[0].path; + drawSVG(); + } +} + +drawSVG = function() { + var d = ""; + for (var i = 0; i < pathArray.length; i++) { //When drawing multiple lines, there are multiple paths + firstPoint = ""; + d = pathArray[i].$.d; + var commands = parse(d); + interpretCommands(commands); + if (i < (pathArray.length - 1)) { //Smooth transition to the next path + doSetTimeout(mapX(currentPoint.x), mapY(currentPoint.y), penHeight + 10, 150); //Moves the pen up and over so no line is drawn between the two + doSetTimeout(mapX(parse(pathArray[i+1].$.d)[0].x), mapY(parse(pathArray[i+1].$.d)[0].y), penHeight + 10, 300); + doSetTimeout(mapX(parse(pathArray[i+1].$.d)[0].x), mapY(parse(pathArray[i+1].$.d)[0].y), penHeight, 450); + } + } +} + +//Move from one point to (x, y) +move = function(x, y) { + doSetTimeout(mapX(x), mapY(y), penHeight, 150); + currentPoint = {x:x, y:y}; //Update the current point (done every time an SVG command is called) + if (!firstPoint) { //Keeps track of the first point, for use with the Z/z command + firstPoint = {x:currentPoint.x, y:currentPoint.y}; //Since the first command of a path is always to Move, this check only occurs here + } +} + +//Move from one point to that that point + x, y +relMove = function(x, y) { + x = currentPoint.x + x; + y = currentPoint.y + y; + + move(x, y); +} + +//Draw a line from one point to (x, y) +line = function(x, y) { + doSetTimeout(mapX(x), mapY(y), penHeight, 150); + currentPoint = {x:x, y:y}; +} + +//Draw a line from one point to that point + x, y +relLine = function(x, y) { + x = currentPoint.x + x; + y = currentPoint.y + y; + + line(x, y); +} + +//Draws a cubic Bezier curve. +//(x1,y1) is the first control point +//(x2, y2) is the second +//(x, y) is the end point +cubicCurve = function(x1, y1, x2, y2, x, y) { + + //Function for calculating the coordinates of points on the curve + //Calculates t+1 points + b = function(x1, y1, x2, y2, x, y, t) { + var ptArray = new Array(); + for (var i = 0; i <= t; i++) { + var newI = i/t; //Converts i to a decimal, to satisfy 0 <= i <= 1 + var ptX = (Math.pow((1-newI), 3) * currentPoint.x) + (3 * Math.pow((1-newI), 2) * newI * x1) //Formula from Wikipedia page for Bezier curves + + (3 * (1-newI) * Math.pow(newI, 2) * x2) + (Math.pow(newI, 3) * x); + var ptY = (Math.pow((1-newI), 3) * currentPoint.y) + (3 * Math.pow((1-newI), 2) * newI * y1) + + (3 * (1-newI) * Math.pow(newI, 2) * y2) + (Math.pow(newI, 3) * y); + var newPt = {x:ptX, y:ptY}; + ptArray.push(newPt); //Populates the array with points + } + currentPoint = {x:ptArray[t].x, y:ptArray[t].y}; + return ptArray; + } + + var curvePts = new Array(); + curvePts = b(x1, y1, x2, y2, x, y, 5); //5 is an arbitrarily-chosen value. It creates a smooth-looking curve without calculating too many points + for (var i = 0;i < curvePts.length; i++) + doSetTimeout(mapX(curvePts[i].x), mapY(curvePts[i].y), penHeight, 2); +} + +//Draws a relative cubic Bezier curve +relCubicCurve = function(x1, y1, x2, y2, x, y) +{ + var tempX = currentPoint.x; + var tempY = currentPoint.y; + cubicCurve(tempX + x1, tempY + y1, tempX + x2, tempY + y2, tempX + x, tempY + y); +} + +//Draws a quadratic Bezier curve +//(x1, y1) is the control point +//(x, y) is the end point +quadraticCurve = function(x1, y1, x, y) { + + //Helper function for generating the points + q = function(x1, y1, x, y, t) { + var ptArray = new Array(); + for (var i = 0; i <= t; i++) { + var newI = i/t; //Converts i to a decimal, to satisfy 0 <= i <= 1 + var ptX = Math.pow((1-newI), 2)*currentPoint.x + (2 * (1-newI) * newI * x1) + (Math.pow(newI, 2) * x); //From Wikipedia page for Bezier curves + var ptY = Math.pow((1-newI), 2)*currentPoint.y + (2 * (1-newI) * newI * y1) + (Math.pow(newI, 2) * y); + var newPt = {x:ptX, y:ptY}; + ptArray.push(newPt); + } + currentPoint = {x:ptArray[t].x, y:ptArray[t].y}; + return ptArray; + } + + var curvePts = new Array(); + curvePts = q(x1, y1, x, y, 5); + for (var i = 0; i < curvePts.length; i++) + doSetTimeout(mapX(curvePts[i].x), mapY(curvePts[i].y), penHeight, 2); +} + +//Draws a relative quadratic Bezier curve +relQuadraticCurve = function(x1, y1, x, y) { + var tempX = currentPoint.x; + var tempY = currentPoint.y; + quadraticCurve(tempX + x1, tempY + y1, tempX + x, tempY + y); +} + +//Draws an elliptical arc +//rx and ry are the radii +//rotation is the angle (in degrees) between the rotated x-axis and the original x-axis +//largeArc is a flag that determines if the arc is greater than or less than or equal to 180 degrees +//sweep is a flag that determines if the arc is drawn in a positive direction or a negative direction +//(x, y) is the final point of the arc +//From: http://www.w3.org/TR/SVG/implnote.html#ArcImplementationNotes +arc = function(rx, ry, rotation, largeArc, sweep, x, y) { + + //Helper function for calculating the points + a = function(rx, ry, largeArc, sweep, x, y, t) { + var ptArray = new Array(); + + for (var i = 0; i <= t; i++) { + var newI = i / t; + var angle = startAngle + sweepAngle * newI; + var newPt = {x: cx + rx * cos(angle), y: cy + ry * sin(angle)}; + ptArray.push(newPt); + } + + currentPoint = {x: ptArray[t].x, y: ptArray[t].y}; + return ptArray; + } + + //Helper function for calculating the angle between two vectors + angleBetween = function(v1, v2) { + var p = v1.x*v2.x + v1.y*v2.y; + var n = Math.sqrt((Math.pow(v1.x, 2)+Math.pow(v1.y, 2)) * (Math.pow(v2.x, 2)+Math.pow(v2.y, 2))); + var sign = v1.x*v2.y - v1.y*v2.x < 0 ? -1 : 1; + var angle = sign*Math.acos(p/n) * 180 / Math.PI; + + return angle; + } + + tempX = currentPoint.x; + tempY = currentPoint.y; + + var xPrime = (cos(rotation) * ((tempX - x) / 2)) + (sin(rotation) * ((tempY - y) / 2)); + var yPrime = (-sin(rotation) * ((tempX - x) / 2)) + (cos(rotation) * ((tempY - y) / 2)); + + //Checks to ensure radii are as they should be + rx = Math.abs(rx); //Ensures they are non-zero and positive + ry = Math.abs(ry); + + var lambda = (Math.pow(xPrime, 2) / Math.pow(rx, 2)) + (Math.pow(yPrime, 2) / Math.pow(ry, 2)); + + if (lambda > 1) { //Ensures they are large enough + rx = Math.sqrt(lambda) * rx; + ry = Math.sqrt(lambda) * ry; + } + + var sign = 1; + + if (largeArc == sweep) //If they are equal, cPrime is negative + sign = -1; + + //For some reason this would occasionally result in NaN + //Implemented this check at the suggestion of: + //http://users.ecs.soton.ac.uk/rfp07r/interactive-svg-examples/arc.html + var cPrimeNumerator = ((Math.pow(rx, 2) * Math.pow(ry, 2)) - (Math.pow(rx, 2) * Math.pow(yPrime, 2)) - (Math.pow(ry, 2) * Math.pow(xPrime, 2))); + var cPrimeDenom = ((Math.pow(rx, 2) * Math.pow(yPrime, 2)) + (Math.pow(ry, 2) * Math.pow(xPrime, 2))); + + if ((cPrimeNumerator / cPrimeDenom) < 1e-7) + cPrime = 0; + else + cPrime = Math.sqrt(cPrimeNumerator / cPrimeDenom); + + //Calculates the transformed center + var cxPrime = sign * cPrime * ((rx * yPrime) / ry); + var cyPrime = sign * cPrime * (-(ry * xPrime) / rx); + + //Calculates the original center + var cx = ((cos(rotation) * cxPrime) + (-sin(rotation) * cyPrime)) + ((tempX + x) / 2); + var cy = ((sin(rotation) * cxPrime) + (cos(rotation) * cyPrime)) + ((tempY + y) / 2); + + //Calculates the start angle of the arc and the total change in the angle + var startVector = {x: (xPrime - cxPrime) / rx, y: (yPrime - cyPrime) / ry}; + var startAngle = angleBetween({x:1, y:0}, startVector); + var endVector = {x: (-xPrime - cxPrime) / rx, y: (-yPrime - cyPrime) / ry}; + var sweepAngle = angleBetween(startVector, endVector); + + if (!sweep && sweepAngle > 0) { + sweepAngle -= 360; + } + + else if (sweep && sweepAngle < 0) { + sweepAngle += 360; + } + + sweepAngle %= 360; + + var ptArray = a(rx, ry, largeArc, sweep, x, y, 7); + + for (var i = 0; i < ptArray.length; i++) { + doSetTimeout(mapX(ptArray[i].x), mapY(ptArray[i].y), penHeight, 75); + } +} + +//Function for drawing a relative elliptical arc +relArc = function(rx, ry, rotation, largeArc, sweep, x, y) { + x = currentPoint.x + x; + y = currentPoint.y + y; + + arc(rx, ry, rotation, largeArc, sweep, x, y); +} + +//A separate setTimeout method so that delays work properly +doSetTimeout = function(x, y, z, delay) { + setTimeout(function() { go(x, y, z) }, timer); + timer = timer + delay; +}; + +//Reflects the point (x,y) across the point (x1, y1) +//For use with smooth curves +reflect = function(x, y, x1, y1) { + var tempX = x; + var tempY = y; + + tempX = x1 - (tempX - x1); + tempY = y1 - (tempY - y1); + + var point = {x:tempX, y:tempY}; + + return point; +} + +//Convert points pixel coordinates to Tapster coordinates +//Done in two methods for ease of use +mapX = function(x) { + var newX = x + transformX; + newX = (newX - halfway.x) / widthRatio; //The center of the canvas corresponds to (0, 0) on the Tapster + return newX; +}; + +mapY = function(y) { + var newY = y + transformY ; + newY = (halfway.y - newY) / heightRatio; + return newY; +} + +// A sine function for working with degrees, not radians +sin = function(degree) { + return Math.sin(Math.PI * (degree/180)); +} + +// A cosine function for working with degrees, not radians +cos = function(degree) { + return Math.cos(Math.PI * (degree/180)); +} + +//Function for converting Inkscape dimensions into Tapster-friendly pixels +dimensionConversion = function(width, height) { + width = String(width); + if (width.search("mm") != -1) { + var dimension = {width: parseInt(width) * 3.779527559, height: parseInt(height) * 3.779527559}; //The px:mm ratio is ~ 3.8:1 + } + else if (width.search("in") != -1) { + var dimension = {width: parseInt(width) * 96, height: parseInt(height) * 96}; //The px:in ratio is 96:1 + } + else if (width.search("ft") != -1) { + var dimension = {width: parseInt(width) * 96 * 12, height: parseInt(height) * 96 * 12}; //The px:ft ratio is 96*12:1 + } + else if (width.search("m") != -1) { + var dimension = {width: parseInt(width) * 3.779527559 * 1000, height: parseInt(height) * 3.779527559 * 1000}; //The px:m ratio is ~3.8*1000:1 + } + else if (width.search("cm") != -1) { + var dimension = {width: parseInt(width) * 3.779527559 * 100, height: parseInt(height) * 3.779527559 * 100}; //The px:cm ratio is ~3.8*100:1 + } + else if (width.search("pt") != -1) { + var dimension = {width: parseInt(width) * 1.3333, height: parseInt(height) * 1.3333}; //The px:pt ratio is ~1.3:1 + } + else if (width.search("pc") != -1) { + var dimension = {width: parseInt(width) * 16, height: parseInt(height) * 16}; //The px:pc ratio is 16:1 + } + else //No unit specified == pixels + var dimension = {width: width, height: height}; + + return dimension; +} + +//Goes through the list of commands an generated by the SVG-Path-Parser and calls the corresponding functions +interpretCommands = function(commands) { + for (var i = 0; i < commands.length; i++) { + var cmdCode = commands[i].code; + if (cmdCode == 'M') { + move(commands[i].x, commands[i].y); + } + + else if (cmdCode == 'm') { + relMove(commands[i].x, commands[i].y); + } + + else if (cmdCode == 'L') { + line(commands[i].x, commands[i].y); + } + + else if (cmdCode == 'l') { + relLine(commands[i].x, commands[i].y); + } + + else if (cmdCode == 'V') { + line(currentPoint.x, commands[i].y); + } + + else if (cmdCode == 'v') { + relLine(currentPoint.x, commands[i].y); + } + + else if (cmdCode == 'H') { + line(commands[i].x, currentPoint.y); + } + + else if (cmdCode == 'h') { + relLine(commands[i].x, currentPoint.y); + } + + else if (cmdCode == 'C') { + cubicCurve(commands[i].x1, commands[i].y1, commands[i].x2, commands[i].y2, commands[i].x, commands[i].y); + } + + else if (cmdCode == 'c') { + relCubicCurve(commands[i].x1, commands[i].y1, commands[i].x2, commands[i].y2, commands[i].x, commands[i].y); + } + + //Smooth cubic curve + else if (cmdCode == 'S') { + if (i > 1 && (commands[i-1].code == 's' || commands[i-1].code == 'c' || commands[i-1].code == 'C' || commands[i-1].code == 'S')) { + var reflected = reflect(commands[i].x, commands[i].y, commands[i-1].x, commands[i-1].y); + var ctrl = {x:reflected.x, y:reflected.y}; + } + else + var ctrl = {x:currentPoint.x, y:currentPoint.y}; + + cubicCurve(ctrl.x, ctrl.y, commands[i].x2, commands[i].y2, commands[i].x, commands[i].y); + } + + //Smooth relative cubic curve + else if (cmdCode == 's') { + if (i > 1 && (commands[i-1].code == 's' || commands[i-1].code == 'c' || commands[i-1].code == 'C' || commands[i-1].code == 'S')) { + var reflected = reflect(commands[i].x, commands[i].y, commands[i-1].x, commands[i-1].y); + var ctrl = {x:reflect(commands[i-1].x2).x, y:reflect(commands[i-1].y2).y}; + } + else + var ctrl = {x:currentPoint.x, y:currentPoint.y}; + + relCubicCurve(ctrl.x, ctrl.y, commands[i].x2, commands[i].y2, commands[i].x, commands[i].y); + } + + else if (cmdCode == 'Q') { + quadraticCurve(commands[i].x1, commands[i].y1, commands[i].x, commands[i].y); + } + + else if (cmdCode == 'q') { + relQuadraticCurve(commands[i].x1, commands[i].y1, commands[i].x, commands[i].y); + } + + //Smooth quadratic curve + else if (cmdCode == 'T') { + if (i > 1 && (commands[i-1].code == 't' || commands[i-1].code == 'q' || commands[i-1].code == 'Q' || commands[i-1].code == 'T')) { + var reflected = reflect(commands[i].x, commands[i].y, commands[i-1].x, commands[i-1].y); + var ctrl = {x:reflect(commands[i-1].x1).x, y:reflect(commands[i-1].y1).y}; + } + else + var ctrl = {x:currentPoint.x, y:currentPoint.y}; + + quadraticCurve(ctrl.x, ctrl.y, commands[i].x, commands[i].y); + } + + //Smooth relative quadratic curve + else if (cmdCode == 't') { + if (i > 1 && (commands[i-1].code == 't' || commands[i-1].code == 'q' || commands[i-1].code == 'Q' || commands[i-1].code == 'T')) { + var reflected = reflect(commands[i].x, commands[i].y, commands[i-1].x, commands[i-1].y); + var ctrl = {x:reflect(commands[i-1].x1).x, y:reflect(commands[i-1].y1).y}; + } + else + var ctrl = {x:currentPoint.x, y:currentPoint.y}; + + relQuadraticCurve(ctrl.x, ctrl.y, commands[i].x, commands[i].y); + } + + else if (cmdCode == 'A') + arc(commands[i].rx, commands[i].ry, commands[i].xAxisRotation, commands[i].largeArc, commands[i].sweep, commands[i].x, commands[i].y); + + else if (cmdCode == 'a') + relArc(commands[i].rx, commands[i].ry, commands[i].xAxisRotation, commands[i].largeArc, commands[i].sweep, commands[i].x, commands[i].y); + + else if (cmdCode == 'Z' || cmdCode == 'z') { + line(firstPoint.x, firstPoint.y); + } + } +} + +//Timer variable for use with the doSetTimeout method +var timer = 0; + +//Saves the coordinates of the first point drawn +var firstPoint; + +var objArr; +var pathArray; + +var widthRatio; +var heightRatio; +var halfway; +var currentPoint; +var penHeight; + +var transformX; +var transformY; From 7de33694744cb6de54b2a6d470c4be2b59d36ec7 Mon Sep 17 00:00:00 2001 From: jackskalitzky Date: Tue, 23 Jun 2015 11:38:29 -0500 Subject: [PATCH 09/43] Pulls values from a config --- software/src/bot.js | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/software/src/bot.js b/software/src/bot.js index 329baa9..2c1cdb7 100755 --- a/software/src/bot.js +++ b/software/src/bot.js @@ -1,6 +1,7 @@ five = require("johnny-five"); ik = require("./ik"); draw = require("./draw"); +svg = require("./SVGReader"); board = new five.Board({ debug: false @@ -112,9 +113,9 @@ go = function(x, y, z) { rotated = rotate(reflected[0],reflected[1]); angles = ik.inverse(rotated[0], rotated[1], z); - servo1.to((angles[1]).map( 0 , 90 , 8 , 90 )); - servo2.to((angles[2]).map( 0 , 90 , 8 , 90 )); - servo3.to((angles[3]).map( 0 , 90 , 8 , 90 )); + servo1.to((angles[1]).map(config.servo1.in_min, config.servo1.in_max, config.servo1.out_min, config.servo1.out_max)); + servo2.to((angles[2]).map(config.servo2.in_min, config.servo2.in_max, config.servo2.out_min, config.servo2.out_max)); + servo3.to((angles[3]).map(config.servo3.in_min, config.servo3.in_max, config.servo3.out_min, config.servo3.out_max)); console.log(angles); } From 58b0097b0e0957965076a86539816ccfd65570dc Mon Sep 17 00:00:00 2001 From: jackskalitzky Date: Tue, 23 Jun 2015 11:40:11 -0500 Subject: [PATCH 10/43] Pulls values from a config --- software/src/ik.js | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/software/src/ik.js b/software/src/ik.js index e129746..7b184b7 100755 --- a/software/src/ik.js +++ b/software/src/ik.js @@ -1,13 +1,15 @@ // Original code from // http://forums.trossenrobotics.com/tutorials/introduction-129/delta-robot-kinematics-3276/ +var config = require("../config.js"); + (function(exports) { // Specific geometry for Tapster: - var e = 34.64101615137754; // Math.sqrt(3) * 10 * 2 - var f = 110.85125168440814; // Math.sqrt(3) * 32 * 2 - var re = 153.5; // 145 + 8.5 - var rf = 52.690131903421914; // Math.sqrt(52**2 + 8.5**2) + var e = config.e; // Math.sqrt(3) * 10 * 2 + var f = config.f; // Math.sqrt(3) * 32 * 2 + var re = config.re; // 145 + 8.5 + var rf = config.rf; // Math.sqrt(52**2 + 8.5**2) exports.updateSize = function(parameters) { e = parameters[0]; From 8ba6fe7d7ed82d07a313f49e4891ad9b0a42b704 Mon Sep 17 00:00:00 2001 From: jackskalitzky Date: Tue, 23 Jun 2015 11:41:27 -0500 Subject: [PATCH 11/43] Update bot.js --- software/src/bot.js | 1 + 1 file changed, 1 insertion(+) diff --git a/software/src/bot.js b/software/src/bot.js index 2c1cdb7..478a22b 100755 --- a/software/src/bot.js +++ b/software/src/bot.js @@ -2,6 +2,7 @@ five = require("johnny-five"); ik = require("./ik"); draw = require("./draw"); svg = require("./SVGReader"); +config = require("../config"); board = new five.Board({ debug: false From d028c249a4a0c97d84578893468ca86efc08f474 Mon Sep 17 00:00:00 2001 From: jackskalitzky Date: Tue, 30 Jun 2015 11:02:04 -0500 Subject: [PATCH 12/43] data.json for draw.js --- software/src/data.json | 240 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 240 insertions(+) create mode 100644 software/src/data.json diff --git a/software/src/data.json b/software/src/data.json new file mode 100644 index 0000000..d77a6af --- /dev/null +++ b/software/src/data.json @@ -0,0 +1,240 @@ +[[ + { + "x": 2.013888359069824, + "y": -0.9861116409301758 + }, + { + "x": 0.013888359069824219, + "y": -0.9861116409301758 + }, + { + "x": 0.013888359069824219, + "y": 4.013888359069824 + }, + { + "x": 9.013888359069824, + "y": 23.01388931274414 + }, + { + "x": 24.01388931274414, + "y": 36.01388931274414 + }, + { + "x": 35.01388931274414, + "y": 48.01388931274414 + }, + { + "x": 44.01388931274414, + "y": 65.01388549804688 + }, + { + "x": 47.01388931274414, + "y": 76.01388549804688 + }, + { + "x": 55.01388931274414, + "y": 93.01388549804688 + }, + { + "x": 67.01388549804688, + "y": 104.01388549804688 + }, + { + "x": 85.01388549804688, + "y": 117.01388549804688 + }, + { + "x": 106.01388549804688, + "y": 149.01388549804688 + }, + { + "x": 107.01388549804688, + "y": 154.01388549804688 + }, + { + "x": 114.01388549804688, + "y": 165.01388549804688 + }, + { + "x": 123.01388549804688, + "y": 173.01388549804688 + }, + { + "x": 132.01388549804688, + "y": 178.01388549804688 + }, + { + "x": 163.01388549804688, + "y": 214.01388549804688 + }, + { + "x": 187.01388549804688, + "y": 233.01388549804688 + }, + { + "x": 226.01388549804688, + "y": 276.0138854980469 + }, + { + "x": 252.01388549804688, + "y": 290.0138854980469 + }, + { + "x": 255.01388549804688, + "y": 294.0138854980469 + }, + { + "x": 260.0138854980469, + "y": 312.0138854980469 + }, + { + "x": 262.0138854980469, + "y": 313.0138854980469 + }, + { + "x": 266.0138854980469, + "y": 323.0138854980469 + }, + { + "x": 270.0138854980469, + "y": 326.0138854980469 + }, + { + "x": 279.0138854980469, + "y": 329.0138854980469 + }, + { + "x": 287.0138854980469, + "y": 337.0138854980469 + } +] +, [ + { + "x": 294.0138854980469, + "y": 6.013888359069824 + }, + { + "x": 255.01388549804688, + "y": 49.01388931274414 + }, + { + "x": 253.01388549804688, + "y": 53.01388931274414 + }, + { + "x": 245.01388549804688, + "y": 59.01388931274414 + }, + { + "x": 242.01388549804688, + "y": 68.01388549804688 + }, + { + "x": 239.01388549804688, + "y": 70.01388549804688 + }, + { + "x": 235.01388549804688, + "y": 82.01388549804688 + }, + { + "x": 225.01388549804688, + "y": 92.01388549804688 + }, + { + "x": 216.01388549804688, + "y": 97.01388549804688 + }, + { + "x": 209.01388549804688, + "y": 106.01388549804688 + }, + { + "x": 203.01388549804688, + "y": 123.01388549804688 + }, + { + "x": 195.01388549804688, + "y": 134.01388549804688 + }, + { + "x": 178.01388549804688, + "y": 145.01388549804688 + }, + { + "x": 169.01388549804688, + "y": 154.01388549804688 + }, + { + "x": 156.01388549804688, + "y": 176.01388549804688 + }, + { + "x": 153.01388549804688, + "y": 185.01388549804688 + }, + { + "x": 126.01388549804688, + "y": 229.01388549804688 + }, + { + "x": 115.01388549804688, + "y": 244.01388549804688 + }, + { + "x": 102.01388549804688, + "y": 258.0138854980469 + }, + { + "x": 88.01388549804688, + "y": 269.0138854980469 + }, + { + "x": 87.01388549804688, + "y": 272.0138854980469 + }, + { + "x": 68.01388549804688, + "y": 282.0138854980469 + }, + { + "x": 54.01388931274414, + "y": 287.0138854980469 + }, + { + "x": 50.01388931274414, + "y": 292.0138854980469 + }, + { + "x": 47.01388931274414, + "y": 305.0138854980469 + }, + { + "x": 43.01388931274414, + "y": 310.0138854980469 + }, + { + "x": 35.01388931274414, + "y": 315.0138854980469 + }, + { + "x": 30.01388931274414, + "y": 315.0138854980469 + }, + { + "x": 20.01388931274414, + "y": 320.0138854980469 + }, + { + "x": 20.01388931274414, + "y": 322.0138854980469 + }, + { + "x": 16.01388931274414, + "y": 326.0138854980469 + }, + { + "x": 13.013888359069824, + "y": 337.0138854980469 + } +]] From aee29045a10c5252279efdf5abf41ee34c0d2e70 Mon Sep 17 00:00:00 2001 From: Jason Huggins Date: Tue, 30 Jun 2015 11:54:47 -0500 Subject: [PATCH 13/43] Kinematics is now configurable. --- software/package.json | 6 +- software/src/bot.js | 13 +++- software/src/kinematics.js | 135 +++++++++++++++++++++++++++++++++++++ 3 files changed, 149 insertions(+), 5 deletions(-) create mode 100644 software/src/kinematics.js diff --git a/software/package.json b/software/package.json index 0463414..d5b7665 100755 --- a/software/package.json +++ b/software/package.json @@ -16,7 +16,9 @@ "engines":{ "node":"0.x.x" }, - "dependencies":{ - "johnny-five":"0.7.x" + "dependencies": { + "johnny-five": "0.8.x", + "svg-path-parser": "^1.0.1", + "xml2js": "^0.4.9" } } diff --git a/software/src/bot.js b/software/src/bot.js index 478a22b..97cbc04 100755 --- a/software/src/bot.js +++ b/software/src/bot.js @@ -1,5 +1,5 @@ five = require("johnny-five"); -ik = require("./ik"); +kinematics = require("./kinematics"); draw = require("./draw"); svg = require("./SVGReader"); config = require("../config"); @@ -8,6 +8,13 @@ board = new five.Board({ debug: false }); +k = new kinematics.Kinematics({ + e: config.e, + f: config.f, + re: config.re, + rf: config.rf +}); + board.on("ready", function() { // Setup servo1 = five.Servo({ @@ -113,7 +120,7 @@ go = function(x, y, z) { reflected = reflect(x,y); rotated = rotate(reflected[0],reflected[1]); - angles = ik.inverse(rotated[0], rotated[1], z); + angles = k.inverse(rotated[0], rotated[1], z); servo1.to((angles[1]).map(config.servo1.in_min, config.servo1.in_max, config.servo1.out_min, config.servo1.out_max)); servo2.to((angles[2]).map(config.servo2.in_min, config.servo2.in_max, config.servo2.out_min, config.servo2.out_max)); servo3.to((angles[3]).map(config.servo3.in_min, config.servo3.in_max, config.servo3.out_min, config.servo3.out_max)); @@ -121,5 +128,5 @@ go = function(x, y, z) { } position = function() { - return ik.forward(servo1.last.degrees, servo2.last.degrees, servo3.last.degrees); + return k.forward(servo1.last.degrees, servo2.last.degrees, servo3.last.degrees); } diff --git a/software/src/kinematics.js b/software/src/kinematics.js new file mode 100644 index 0000000..b4d6476 --- /dev/null +++ b/software/src/kinematics.js @@ -0,0 +1,135 @@ +// Trigonometric constants +var s = 165 * 2; +var sqrt3 = Math.sqrt(3.0); +var pi = 3.141592653; +var sin120 = sqrt3 / 2.0; +var cos120 = -0.5; +var tan60 = sqrt3; +var sin30 = 0.5; +var tan30 = 1.0 / sqrt3; + +function Kinematics(args) { + //Side of end effector + this.e = 0; + + //Side of top triangle + this.f = 0; + + //Length of parallelogram joint + this.re = 0; + + //Length of upper joint + this.rf = 0; + + if (args) { + var keys = Object.keys(args) + keys.forEach(function(key){ + this[key] = args[key] + }, this) + } +} + +// Forward kinematics: (theta1, theta2, theta3) -> (x0, y0, z0) +// Returned {error code,theta1,theta2,theta3} +Kinematics.prototype.forward = function(theta1, theta2, theta3) { + var x0 = 0.0; + var y0 = 0.0; + var z0 = 0.0; + + var t = (this.f - this.e) * tan30 / 2.0; + var dtr = pi / 180.0; + + theta1 *= dtr; + theta2 *= dtr; + theta3 *= dtr; + + var y1 = -(t + this.rf * Math.cos(theta1)); + var z1 = -this.rf * Math.sin(theta1); + + var y2 = (t + this.rf * Math.cos(theta2)) * sin30; + var x2 = y2 * tan60; + var z2 = -this.rf * Math.sin(theta2); + + var y3 = (t + this.rf * Math.cos(theta3)) * sin30; + var x3 = -y3 * tan60; + var z3 = -this.rf * Math.sin(theta3); + + var dnm = (y2 - y1) * x3 - (y3 - y1) * x2; + + var w1 = y1 * y1 + z1 * z1; + var w2 = x2 * x2 + y2 * y2 + z2 * z2; + var w3 = x3 * x3 + y3 * y3 + z3 * z3; + + // x = (a1*z + b1)/dnm + var a1 = (z2 - z1) * (y3 - y1) - (z3 - z1) * (y2 - y1); + var b1 = -((w2 - w1) * (y3 - y1) - (w3 - w1) * (y2 - y1)) / 2.0; + + // y = (a2*z + b2)/dnm; + var a2 = -(z2 - z1) * x3 + (z3 - z1) * x2; + var b2 = ((w2 - w1) * x3 - (w3 - w1) * x2) / 2.0; + + // a*z^2 + b*z + c = 0 + var a = a1 * a1 + a2 * a2 + dnm * dnm; + var b = 2.0 * (a1 * b1 + a2 * (b2 - y1 * dnm) - z1 * dnm * dnm); + var c = (b2 - y1 * dnm) * (b2 - y1 * dnm) + b1 * b1 + dnm * dnm * (z1 * z1 - this.re * this.re); + + // discriminant + var d = b * b - 4.0 * a * c; + if (d < 0.0) { + return new Array(1, 0, 0, 0); // non-existing povar. return error,x,y,z + } + + z0 = -0.5 * (b + Math.sqrt(d)) / a; + x0 = (a1 * z0 + b1) / dnm; + y0 = (a2 * z0 + b2) / dnm; + + return new Array(0, x0, y0, z0); +}; + + + + +// Inverse kinematics + +// Helper functions, calculates angle theta1 (for YZ-pane) +Kinematics.prototype.delta_calcAngleYZ = function(x0, y0, z0) { + var y1 = -0.5 * 0.57735 * this.f; // f/2 * tg 30 + y0 -= 0.5 * 0.57735 * this.e; // shift center to edge + // z = a + b*y + var a = (x0 * x0 + y0 * y0 + z0 * z0 + this.rf * this.rf - this.re * this.re - y1 * y1) / (2.0 * z0); + var b = (y1 - y0) / z0; + + // discriminant + var d = -(a + b * y1) * (a + b * y1) + this.rf * (b * b * this.rf + this.rf); + if (d < 0) { + return new Array(1, 0); // non-existing povar. return error, theta + } + + var yj = (y1 - a * b - Math.sqrt(d)) / (b * b + 1); // choosing outer povar + var zj = a + b * yj; + var theta = Math.atan(-zj / (y1 - yj)) * 180.0 / pi + ((yj > y1) ? 180.0 : 0.0); + + return new Array(0, theta); // return error, theta + }; + + +Kinematics.prototype.inverse = function(x0, y0, z0) { + var theta1 = 0; + var theta2 = 0; + var theta3 = 0; + var status = this.delta_calcAngleYZ(x0, y0, z0); + + if (status[0] === 0) { + theta1 = status[1]; + status = this.delta_calcAngleYZ(x0 * cos120 + y0 * sin120, y0 * cos120 - x0 * sin120, z0, theta2); + } + if (status[0] === 0) { + theta2 = status[1]; + status = this.delta_calcAngleYZ(x0 * cos120 - y0 * sin120, y0 * cos120 + x0 * sin120, z0, theta3); + } + theta3 = status[1]; + + return new Array(status[0], theta1, theta2, theta3); +}; + +module.exports.Kinematics = Kinematics; \ No newline at end of file From 8adbe0575b5d43c356e306e907abfd545ff245a6 Mon Sep 17 00:00:00 2001 From: Jason Huggins Date: Tue, 30 Jun 2015 12:09:22 -0500 Subject: [PATCH 14/43] ik.js is now kinematics.js --- software/src/ik.js | 136 --------------------------------------------- 1 file changed, 136 deletions(-) delete mode 100755 software/src/ik.js diff --git a/software/src/ik.js b/software/src/ik.js deleted file mode 100755 index 7b184b7..0000000 --- a/software/src/ik.js +++ /dev/null @@ -1,136 +0,0 @@ -// Original code from -// http://forums.trossenrobotics.com/tutorials/introduction-129/delta-robot-kinematics-3276/ - -var config = require("../config.js"); - -(function(exports) { - - // Specific geometry for Tapster: - var e = config.e; // Math.sqrt(3) * 10 * 2 - var f = config.f; // Math.sqrt(3) * 32 * 2 - var re = config.re; // 145 + 8.5 - var rf = config.rf; // Math.sqrt(52**2 + 8.5**2) - - exports.updateSize = function(parameters) { - e = parameters[0]; - f = parameters[1]; - re = parameters[2]; - rf = parameters[3]; - exports.e = e; - exports.f = f; - exports.re = re; - exports.rf = rf; - }; - - exports.getSize = function() { - return new Array(e, f, re, rf); - }; - - // Trigonometric constants - var s = 165 * 2; - var sqrt3 = Math.sqrt(3.0); - var pi = 3.141592653; - var sin120 = sqrt3 / 2.0; - var cos120 = -0.5; - var tan60 = sqrt3; - var sin30 = 0.5; - var tan30 = 1.0 / sqrt3; - - // Forward kinematics: (theta1, theta2, theta3) -> (x0, y0, z0) - // Returned {error code,theta1,theta2,theta3} - exports.forward = function(theta1, theta2, theta3) { - var x0 = 0.0; - var y0 = 0.0; - var z0 = 0.0; - - var t = (f - e) * tan30 / 2.0; - var dtr = pi / 180.0; - - theta1 *= dtr; - theta2 *= dtr; - theta3 *= dtr; - - var y1 = -(t + rf * Math.cos(theta1)); - var z1 = -rf * Math.sin(theta1); - - var y2 = (t + rf * Math.cos(theta2)) * sin30; - var x2 = y2 * tan60; - var z2 = -rf * Math.sin(theta2); - - var y3 = (t + rf * Math.cos(theta3)) * sin30; - var x3 = -y3 * tan60; - var z3 = -rf * Math.sin(theta3); - - var dnm = (y2 - y1) * x3 - (y3 - y1) * x2; - - var w1 = y1 * y1 + z1 * z1; - var w2 = x2 * x2 + y2 * y2 + z2 * z2; - var w3 = x3 * x3 + y3 * y3 + z3 * z3; - - // x = (a1*z + b1)/dnm - var a1 = (z2 - z1) * (y3 - y1) - (z3 - z1) * (y2 - y1); - var b1 = -((w2 - w1) * (y3 - y1) - (w3 - w1) * (y2 - y1)) / 2.0; - - // y = (a2*z + b2)/dnm; - var a2 = -(z2 - z1) * x3 + (z3 - z1) * x2; - var b2 = ((w2 - w1) * x3 - (w3 - w1) * x2) / 2.0; - - // a*z^2 + b*z + c = 0 - var a = a1 * a1 + a2 * a2 + dnm * dnm; - var b = 2.0 * (a1 * b1 + a2 * (b2 - y1 * dnm) - z1 * dnm * dnm); - var c = (b2 - y1 * dnm) * (b2 - y1 * dnm) + b1 * b1 + dnm * dnm * (z1 * z1 - re * re); - - // discriminant - var d = b * b - 4.0 * a * c; - if (d < 0.0) { - return new Array(1, 0, 0, 0); // non-existing povar. return error,x,y,z - } - - z0 = -0.5 * (b + Math.sqrt(d)) / a; - x0 = (a1 * z0 + b1) / dnm; - y0 = (a2 * z0 + b2) / dnm; - - return new Array(0, x0, y0, z0); - }; - - // Inverse kinematics - // Helper functions, calculates angle theta1 (for YZ-pane) - var delta_calcAngleYZ = function(x0, y0, z0) { - var y1 = -0.5 * 0.57735 * f; // f/2 * tg 30 - y0 -= 0.5 * 0.57735 * e; // shift center to edge - // z = a + b*y - var a = (x0 * x0 + y0 * y0 + z0 * z0 + rf * rf - re * re - y1 * y1) / (2.0 * z0); - var b = (y1 - y0) / z0; - - // discriminant - var d = -(a + b * y1) * (a + b * y1) + rf * (b * b * rf + rf); - if (d < 0) { - return new Array(1, 0); // non-existing povar. return error, theta - } - - var yj = (y1 - a * b - Math.sqrt(d)) / (b * b + 1); // choosing outer povar - var zj = a + b * yj; - var theta = Math.atan(-zj / (y1 - yj)) * 180.0 / pi + ((yj > y1) ? 180.0 : 0.0); - - return new Array(0, theta); // return error, theta - }; - - exports.inverse = function(x0, y0, z0) { - var theta1 = 0; - var theta2 = 0; - var theta3 = 0; - var status = delta_calcAngleYZ(x0, y0, z0); - - if (status[0] === 0) { - theta1 = status[1]; - status = delta_calcAngleYZ(x0 * cos120 + y0 * sin120, y0 * cos120 - x0 * sin120, z0, theta2); - } - if (status[0] === 0) { - theta2 = status[1]; - status = delta_calcAngleYZ(x0 * cos120 - y0 * sin120, y0 * cos120 + x0 * sin120, z0, theta3); - } - theta3 = status[1]; - - return new Array(status[0], theta1, theta2, theta3); - }; -}(typeof exports === 'undefined' ? this.ik = {} : exports)); From 3c195bb8c47f774e3a9789c365b80485d9776700 Mon Sep 17 00:00:00 2001 From: jackskalitzky Date: Tue, 30 Jun 2015 14:15:27 -0500 Subject: [PATCH 15/43] Created an SVGReader object --- software/src/SVGReader.js | 49 +++++++++++++++++++++++---------------- 1 file changed, 29 insertions(+), 20 deletions(-) diff --git a/software/src/SVGReader.js b/software/src/SVGReader.js index fcfdd4b..214a9b0 100644 --- a/software/src/SVGReader.js +++ b/software/src/SVGReader.js @@ -1,29 +1,35 @@ //Draws stuff from SVG files //Built with InkScape in mind, but should support: //http://svg-edit.googlecode.com/svn/branches/stable/editor/svg-editor.html - //> To test: -//> svg.draw("filename.svg") +//> svgRead.drawSVG(filepath) var parse = require('svg-path-parser'); var fs = require('fs'); -var config = require('../config.js'); var parseString = require('xml2js').parseString; -//filename must be a string. ".svg" is optional -//Right now, the file is assumed to be in the software folder -//To-do: Add support for arbitrary file locations -exports.draw = function(filename) { - var parsed; +function SVGReader(args) { + this.baseWidth = 0; + this.baseHeight = 0; - if (filename.search(".svg") == -1) //Ensures that filename is valid and points to an .svg - filename += ".svg"; + if (args) { + var keys = Object.keys(args) + keys.forEach(function(key){ + this[key] = args[key] + }, this) + } +} + +//Draws from an SVG image specified by filepath +//> Usage: +//> drawSVG("C:/Projects/Tapsterbot/software/src/drawing.svg"); +SVGReader.prototype.drawSVG = function(filePath) { + var parsed; //Create a JSON string out of the SVG image data //parseString strips away the XML data - try { - parseString(fs.readFileSync("../" + filename, "utf8"), function(err, result) { + parseString(fs.readFileSync(filePath, "utf8"), function(err, result) { parsed = JSON.stringify(result, null, 1); }); } catch (e) { @@ -32,12 +38,13 @@ exports.draw = function(filename) { else throw e; - return; + return; //If the file is not found stop execution } //Parse the JSON string into an array objArr = JSON.parse(parsed); + //Extract width and height data from the drawing var svgDimensions = dimensionConversion(objArr.svg.$.width, objArr.svg.$.height); width = svgDimensions.width; height = svgDimensions.height; @@ -55,9 +62,9 @@ exports.draw = function(filename) { transformY = parseInt(transString.substring(subY + 1)); } - var phoneWidth = config.baseWidth; - var phoneHeight = config.baseHeight; - penHeight = config.penHeight; + var phoneWidth = this.baseWidth; + var phoneHeight = this.baseHeight; + penHeight = -140 widthRatio = width / phoneWidth; heightRatio = height / phoneHeight; @@ -70,17 +77,17 @@ exports.draw = function(filename) { if (objArr.svg.g[0].g) { //If there are multiple groups for (var i = 0; i < objArr.svg.g[0].g.length; i++) { pathArray = objArr.svg.g[0].g[i].path; - drawSVG(); - } + drawImage(); + } } else if (objArr.svg.g[0].path) { pathArray = objArr.svg.g[0].path; - drawSVG(); + drawImage(); } } -drawSVG = function() { +drawImage = function() { var d = ""; for (var i = 0; i < pathArray.length; i++) { //When drawing multiple lines, there are multiple paths firstPoint = ""; @@ -504,3 +511,5 @@ var penHeight; var transformX; var transformY; + +module.exports.SVGReader = SVGReader; From f32fda4f38b50f8ffb224f47899c3fb85dd73cd0 Mon Sep 17 00:00:00 2001 From: jackskalitzky Date: Tue, 30 Jun 2015 14:17:00 -0500 Subject: [PATCH 16/43] Instantiation and config location specification --- software/src/bot.js | 57 +++++++++++++++++++++++++++++++++++++++++---- 1 file changed, 52 insertions(+), 5 deletions(-) diff --git a/software/src/bot.js b/software/src/bot.js index 97cbc04..6858f18 100755 --- a/software/src/bot.js +++ b/software/src/bot.js @@ -1,12 +1,45 @@ five = require("johnny-five"); kinematics = require("./kinematics"); -draw = require("./draw"); svg = require("./SVGReader"); -config = require("../config"); +drawing = require("./draw"); + +//If a filepath is specified, load that config +//Otherwise, resort to the default config +//> Usage: +//> node bot.js "C:\Projects\Tapsterbot\software\config.js" +if (process.argv[2]) { + try { + var config = require(process.argv[2]); + console.log("Config found and loaded."); + } catch (e) { + console.log("Config not found. Loading default."); + var config = require("../config.js"); + } +} +else { + console.log("Config not specified. Loading default."); + var config = require("../config.js"); +} -board = new five.Board({ - debug: false -}); +//Alternate config loading code +//If a Tapster version is specified, load that config +//Otherwise resort to the default config +//> Usage: +//> node bot.js "Tapster-2-plus" + +/*if (process.argv[2]) { + try { + var config = require("../" + process.argv[2] + ".js"); + console.log("Config found and loaded."); + } catch (e) { + console.log("Config not found. Loading default."); + var config = require("../config.js"); + } +} +else { + console.log("Config not specified. Loading default."); + var config = require("../config.js"); +} */ k = new kinematics.Kinematics({ e: config.e, @@ -15,6 +48,20 @@ k = new kinematics.Kinematics({ rf: config.rf }); +svgRead = new svg.SVGReader({ + baseWidth: config.baseWidth, + baseHeight: config.baseHeight +}); + +draw = new drawing.Draw({ + baseWidth: config.baseWidth, + baseHeight: config.baseHeight +}); + +board = new five.Board({ + debug: false +}); + board.on("ready", function() { // Setup servo1 = five.Servo({ From 9beafd5c4d544d2727eafd6c55dd87e446569c55 Mon Sep 17 00:00:00 2001 From: jackskalitzky Date: Tue, 30 Jun 2015 14:17:33 -0500 Subject: [PATCH 17/43] Created a Draw object --- software/src/draw.js | 189 +++++++++++++++++++++++++++---------------- 1 file changed, 118 insertions(+), 71 deletions(-) diff --git a/software/src/draw.js b/software/src/draw.js index 91aeb71..79151bc 100644 --- a/software/src/draw.js +++ b/software/src/draw.js @@ -1,12 +1,24 @@ //Draws lines and shapes. //> To test: //> draw.drawSquare() -//> draw.drawStar() +//> draw.drawStar(sideLength, n) //> draw.drawCircle() -//> draw.drawFromCoordinates() +//> draw.drawFromCoordinates() var fs = require('fs'); +function Draw(args) { + this.baseWidth = 0; + this.baseHeight = 0; + + if (args) { + var keys = Object.keys(args); + keys.forEach(function(key){ + this[key] = args[key]; + }, this) + } +} + //Maps point from canvas to the Tapster coordinate plane //The conversion is based on the canvas size and the size of the Tapster base //These can be changed as needed @@ -22,90 +34,125 @@ mapPoints = function(x, y) { //Adds delays while the Tapster is writing //The specific delay can be changed if the bot has to go slower or faster //for a particular segment -doSetTimeout = function(i, z, delay) -{ - setTimeout(function() { go(i.x, i.y, z) }, timer); +doSetTimeout = function(x, y, z, delay) { + setTimeout(function() { go(x, y, z) }, timer); timer = timer + delay; }; -var jFile = fs.readFileSync('./data.json', 'utf8'); //Reads data from a JSON file of coordinates -var objArr = JSON.parse(jFile); //Creates an array out of the data - -//Specific dimensions of the canvas and Tapster. Change as needed. -//Uses a modified version of the canvas program to make it easier to map from canvas to Tapster. -var canvasHeight = 500; -var canvasWidth = 300; -var phoneHeight = 100; -var phoneWidth = 60; - -//The ratio between the sizes of the canvas and robot. -var heightRatio = canvasHeight / phoneHeight; -var widthRatio = canvasWidth / phoneWidth; - -//The center of the canvas -var halfway = {x:canvasWidth / 2, y:canvasHeight / 2}; +//Initialized out here so that they are accessible from the mapPoints function +var baseHeight, baseWidth, canvasHeight, canvasWidth, penHeight, heightRatio, widthRatio, halfway; //A timer variable for use with doSetTimeout() var timer = 0; +//Set the penHeight from the command line +Draw.prototype.setPenHeight = function(height) { + penHeight = height; + console.log("The pen is now set at: " + height); +} + //Loops through the JSON array -exports.drawFromCoordinates = function() { -for (var i = 0; i < objArr.length; i++) -{ - var x = 0; - if (objArr[i].length > 0) //If there are multiple lines - { - var point = objArr[i][x]; - var transMap = mapPoints(point.x, point.y); - doSetTimeout(transMap, -130, 200); //Moves the arm vertically so that it does not draw a line between the last point of one line and the first point of another - - for (x = 0; x < objArr[i].length; x++) - { - point = objArr[i][x]; - var mapped = mapPoints(point.x, point.y); - doSetTimeout(mapped, -140, 100); - } - transMap = mapPoints(point.x, point.y); - doSetTimeout(transMap, -130, 200); +Draw.prototype.drawFromCoordinates = function() { + + baseHeight = this.baseHeight; + baseWidth = this.baseWidth; + canvasHeight = baseHeight * 3.779527559; //Set ratio of ~1:3.8 mm:px + canvasWidth = baseWidth * 3.779527559; + + //The Z coordinate of the pen, for drawing + penHeight = -140; + + //The ratio between the sizes of the canvas and robot. + //It will always be 3.779527559 because the canvas size is set + //according to that ratio + heightRatio = 3.779527559; + widthRatio = 3.779527559; + + //The center of the canvas + halfway = {x:canvasWidth / 2, y:canvasHeight / 2}; + + + var jFile = fs.readFileSync('.//data.json', 'utf8'); //Reads data from a JSON file of coordinates + var objArr = JSON.parse(jFile); //Creates an array out of the data + + for (var i = 0; i < objArr.length; i++) { + var x = 0; + if (objArr[i].length > 0) { //If there are multiple lines + var point = objArr[i][x]; + var transMap = mapPoints(point.x, point.y); + doSetTimeout(transMap.x, transMap.y, penHeight + 20, 200); //Moves the arm vertically so that it does not draw a line between the last point of one line and the first point of another + + for (x = 0; x < objArr[i].length; x++) { + point = objArr[i][x]; + var mapped = mapPoints(point.x, point.y); + doSetTimeout(mapped.x, mapped.y, penHeight, 100); + } + transMap = mapPoints(point.x, point.y); + doSetTimeout(transMap.x, transMap.y, penHeight + 20, 200); + } + + else { //Only one line to be drawn + point = objArr[i]; + var mapped = mapPoints(point.x, point.y); + doSetTimeout(mapped.x, mapped.y, penHeight, 100); + } } - else //Only one line to be drawn - { - point = objArr[i]; - var mapped = mapPoints(point.x, point.y); - doSetTimeout(mapped, -140, 100); -} -} }; //Draws a square in order to ensure that everything is working properly -exports.drawSquare = function() { - setTimeout(function() { go(-20, 20, -150); }, 0); //Top left - setTimeout(function() { go(20, 20, -150); }, 1000); //Top right - setTimeout(function() { go(20, -20, -150); }, 2000); //Bottom right - setTimeout(function() { go(-20, -20, -150); }, 3000); //Bottom left - setTimeout(function() { go(-20, 20, -150); }, 4000); //Return to start position +//sideLength is the length of the sides +//Draws every nth point +Draw.prototype.drawSquare = function(sideLength, n) { + + timer = 0; //Reset the timer so that there isn't unnecessary delay when calling the function multiple times + + var halfSide = sideLength / 2; + var points = sideLength / n; + + doSetTimeout(-halfSide, halfSide, penHeight + 10, 0) + doSetTimeout(-halfSide, halfSide, penHeight, 500); //Top left corner + + for (var i = 0; i < points; i++) { //To bottom left + doSetTimeout(-halfSide, halfSide - (n * i), penHeight, i * 5); + } + + for (var i = 0; i < points; i++) { //To bottom right + doSetTimeout(-halfSide + (n * i), -halfSide, penHeight, i * 5); + } + + for (var i = 0; i < points; i++) { //To top right + doSetTimeout(halfSide, -halfSide + (n * i), penHeight, i * 5); + } + + for (var i = 0; i < points; i++) { //To top left + doSetTimeout(halfSide - (n * i), halfSide, penHeight, i * 5); + } + + doSetTimeout(0, 0, -140, timer + 100); + }; //Draws a star to test that the Tapster bot is working properly -exports.drawStar = function() { - setTimeout(function() { go(-20, -20, -140); }, 0); //Bottom left - setTimeout(function() { go(0, 20, -140); }, 1000); //Top - setTimeout(function() { go(20, -20, -140); }, 2000); //Bottom right - setTimeout(function() { go(-20, 10, -140); }, 3000); //Left - setTimeout(function() { go(20, 10, -140); }, 4000); //Right - setTimeout(function() { go(-20, -20, -140); }, 5000); //Starting position +Draw.prototype.drawStar = function() { + setTimeout(function() { go(-20, -20, penHeight); }, 0); //Bottom left + setTimeout(function() { go(0, 20, penHeight); }, 1000); //Top + setTimeout(function() { go(20, -20, penHeight); }, 2000); //Bottom right + setTimeout(function() { go(-20, 10, penHeight); }, 3000); //Left + setTimeout(function() { go(20, 10, penHeight); }, 4000); //Right + setTimeout(function() { go(-20, -20, penHeight); }, 5000); //Starting position }; -exports.drawCircle = function() { +Draw.prototype.drawCircle = function() { var centerX=0; var centerY=0; var radius=20; - ; + // an array to save your points var points=[]; // populate array with points along a circle - for (var degree=0; degree<360; degree++){ + //Goes to 390 degrees so that the circle is actually completed + for (var degree=0; degree<390; degree++) { var radians = degree * Math.PI/180; var x = centerX + radius * Math.cos(radians); var y = centerY + radius * Math.sin(radians); @@ -113,8 +160,8 @@ exports.drawCircle = function() { } circle = function() { - for (var i=0; i<360; i+=1) { - setTimeout( function(point) { go(point.x, point.y, -141) }, i*2, points[i]); + for (var i=0; i<390; i+=1) { + setTimeout( function(point) { go(point.x, point.y, penHeight) }, i*2, points[i]); } } @@ -122,15 +169,14 @@ exports.drawCircle = function() { }; //Draws an arbitrary amount of spirals to test that the Tapster bot is working properly -exports.drawSpiral= function(spirals) { +Draw.prototype.drawSpiral= function(spirals) { var centerX = 0; var centerY = 0; var radius = 30; var x1 = 0; var y1 = 0; var points = []; - for (var degree = 0; degree < spirals * 360; degree++) - { + for (var degree = 0; degree < spirals * 360; degree++) { x1 = x1 + 30/(spirals * 360); y1 = y1 + 30/(spirals * 360); var radians = degree * Math.PI/180; @@ -140,10 +186,11 @@ exports.drawSpiral= function(spirals) { } spiral = function() { - for (var z = 0; z < spirals*360; z++) - { - setTimeout( function(point) { go(point.x, point.y, -140) }, z*2, points[z]); + for (var z = 0; z < spirals*360; z++) { + setTimeout( function(point) { go(point.x, point.y, penHeight) }, z*2, points[z]); } } spiral(); -}; +}; + +module.exports.Draw = Draw; From c21e37912afe00c316639639fcef7fa39a3be7a6 Mon Sep 17 00:00:00 2001 From: jackskalitzky Date: Tue, 30 Jun 2015 14:24:43 -0500 Subject: [PATCH 18/43] Fixed an issue with penHeight --- software/src/draw.js | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/software/src/draw.js b/software/src/draw.js index 79151bc..b31ccbf 100644 --- a/software/src/draw.js +++ b/software/src/draw.js @@ -40,7 +40,9 @@ doSetTimeout = function(x, y, z, delay) { }; //Initialized out here so that they are accessible from the mapPoints function -var baseHeight, baseWidth, canvasHeight, canvasWidth, penHeight, heightRatio, widthRatio, halfway; +var baseHeight, baseWidth, canvasHeight, canvasWidth, heightRatio, widthRatio, halfway; + +var penHeight = -140; //A timer variable for use with doSetTimeout() var timer = 0; @@ -59,9 +61,6 @@ Draw.prototype.drawFromCoordinates = function() { canvasHeight = baseHeight * 3.779527559; //Set ratio of ~1:3.8 mm:px canvasWidth = baseWidth * 3.779527559; - //The Z coordinate of the pen, for drawing - penHeight = -140; - //The ratio between the sizes of the canvas and robot. //It will always be 3.779527559 because the canvas size is set //according to that ratio @@ -90,7 +89,7 @@ Draw.prototype.drawFromCoordinates = function() { transMap = mapPoints(point.x, point.y); doSetTimeout(transMap.x, transMap.y, penHeight + 20, 200); } - + else { //Only one line to be drawn point = objArr[i]; var mapped = mapPoints(point.x, point.y); From 8fd2264b3b8cfe1313e75da8494ac00facf325a6 Mon Sep 17 00:00:00 2001 From: jackskalitzky Date: Wed, 1 Jul 2015 16:42:20 -0500 Subject: [PATCH 19/43] Added erase functionality --- software/src/draw.js | 130 ++++++++++++++++++++++++++++++++++++++----- 1 file changed, 116 insertions(+), 14 deletions(-) diff --git a/software/src/draw.js b/software/src/draw.js index b31ccbf..58362d7 100644 --- a/software/src/draw.js +++ b/software/src/draw.js @@ -1,9 +1,10 @@ //Draws lines and shapes. //> To test: -//> draw.drawSquare() -//> draw.drawStar(sideLength, n) +//> draw.drawSquare(sideLength, n) +//> draw.drawStar() //> draw.drawCircle() //> draw.drawFromCoordinates() +//> draw.erase() var fs = require('fs'); @@ -32,16 +33,42 @@ mapPoints = function(x, y) { }; //Adds delays while the Tapster is writing -//The specific delay can be changed if the bot has to go slower or faster -//for a particular segment +//The specific delay can be changed if the bot has to go slower or faster for a particular segment doSetTimeout = function(x, y, z, delay) { setTimeout(function() { go(x, y, z) }, timer); + currentPoint = {x: x, y: y, z: z}; timer = timer + delay; }; -//Initialized out here so that they are accessible from the mapPoints function +//A go method that iterates over points rather than jumping from a to b +//Assumes that z stays the same +//Draws points + 1 points +//Delay is the total amount of time that it takes +//Each point takes delay / points time to draw +itGo = function(x, y, z, points, delay) { + var x1 = currentPoint.x; + var y1 = currentPoint.y; + var deltaX = x - x1; + var deltaY = y - y1; + var slope = ((y - y1) / (x - x1)); + var pointArray = new Array(); + + for (var i = 0; i <= points; i++) { + var newX = x1 + deltaX / points * i; + var newY = y1 + deltaY / points * i; + var point = {x: newX, y: newY}; + pointArray.push(point); + } + + for (var i = 0; i < pointArray.length; i++) { + doSetTimeout(pointArray[i].x, pointArray[i].y, z, delay / points); + } +} + +//Initialized here so that they are accessible from the mapPoints function var baseHeight, baseWidth, canvasHeight, canvasWidth, heightRatio, widthRatio, halfway; +var currentPoint = {x: 0, y: 0, z: -140}; var penHeight = -140; //A timer variable for use with doSetTimeout() @@ -53,7 +80,7 @@ Draw.prototype.setPenHeight = function(height) { console.log("The pen is now set at: " + height); } -//Loops through the JSON array +//Draws an image from a JSON file of coordinates Draw.prototype.drawFromCoordinates = function() { baseHeight = this.baseHeight; @@ -74,12 +101,14 @@ Draw.prototype.drawFromCoordinates = function() { var jFile = fs.readFileSync('.//data.json', 'utf8'); //Reads data from a JSON file of coordinates var objArr = JSON.parse(jFile); //Creates an array out of the data + //Loops through the JSON array for (var i = 0; i < objArr.length; i++) { var x = 0; if (objArr[i].length > 0) { //If there are multiple lines var point = objArr[i][x]; var transMap = mapPoints(point.x, point.y); - doSetTimeout(transMap.x, transMap.y, penHeight + 20, 200); //Moves the arm vertically so that it does not draw a line between the last point of one line and the first point of another + doSetTimeout(transMap.x, transMap.y, penHeight + 20, 200); //Moves the arm vertically so that it does not draw a line between the last point of one line + //and the first point of another for (x = 0; x < objArr[i].length; x++) { point = objArr[i][x]; @@ -133,15 +162,25 @@ Draw.prototype.drawSquare = function(sideLength, n) { //Draws a star to test that the Tapster bot is working properly Draw.prototype.drawStar = function() { - setTimeout(function() { go(-20, -20, penHeight); }, 0); //Bottom left + timer = 0; + doSetTimeout(-20, -20, penHeight, 1000); + doSetTimeout(0, 20, penHeight, 1000); + doSetTimeout(20, -20, penHeight, 1000); + doSetTimeout(-20, 10, penHeight, 1000); + doSetTimeout(20, 10, penHeight, 1000); + doSetTimeout(-20, -20, penHeight, 1000); + + /*setTimeout(function() { go(-20, -20, penHeight); }, 0); //Bottom left setTimeout(function() { go(0, 20, penHeight); }, 1000); //Top setTimeout(function() { go(20, -20, penHeight); }, 2000); //Bottom right setTimeout(function() { go(-20, 10, penHeight); }, 3000); //Left setTimeout(function() { go(20, 10, penHeight); }, 4000); //Right setTimeout(function() { go(-20, -20, penHeight); }, 5000); //Starting position + */ }; Draw.prototype.drawCircle = function() { + timer = 0; var centerX=0; var centerY=0; var radius=20; @@ -160,7 +199,8 @@ Draw.prototype.drawCircle = function() { circle = function() { for (var i=0; i<390; i+=1) { - setTimeout( function(point) { go(point.x, point.y, penHeight) }, i*2, points[i]); + point = points[i]; + doSetTimeout(point.x, point.y, penHeight, 2); } } @@ -168,16 +208,25 @@ Draw.prototype.drawCircle = function() { }; //Draws an arbitrary amount of spirals to test that the Tapster bot is working properly -Draw.prototype.drawSpiral= function(spirals) { +//Spirals is the amount of spirals to draw +//Radius is the diameter(?) of the largest spiral +//zLevel is optional -- it is the penHeight to draw the spiral at (mainly used for the erase function) +Draw.prototype.drawSpiral= function(spirals, radius, zLevel) { + timer = 0; var centerX = 0; var centerY = 0; - var radius = 30; var x1 = 0; var y1 = 0; var points = []; + + if (zLevel) //If a zLevel is specified set the penHeight at that level + penHeight = zLevel; + + go(centerX, centerY, penHeight); + for (var degree = 0; degree < spirals * 360; degree++) { - x1 = x1 + 30/(spirals * 360); - y1 = y1 + 30/(spirals * 360); + x1 = x1 + radius/(spirals * 360); + y1 = y1 + radius/(spirals * 360); var radians = degree * Math.PI/180; var x = centerX + x1 * Math.cos(radians); var y = centerY + y1 * Math.sin(radians); @@ -186,10 +235,63 @@ Draw.prototype.drawSpiral= function(spirals) { spiral = function() { for (var z = 0; z < spirals*360; z++) { - setTimeout( function(point) { go(point.x, point.y, penHeight) }, z*2, points[z]); + point = points[z]; + doSetTimeout(point.x, point.y, penHeight, 5); } } spiral(); }; +Draw.prototype.pickUpEraser = function() { + timer = 0; + doSetTimeout(currentPoint.x, currentPoint.y, -130, 500); + doSetTimeout(65, 25, -130, 500); + doSetTimeout(55, 20, -148, 1000); + var x = currentPoint.x; + var y = currentPoint.y; + while (x > 0 || y > 0) { + if (x / 10 > 1) + x -= 10; + else if (y / 10 > 1) + y -= 10; + else { + x = 0; + y = 0; + } + + doSetTimeout(x, y, -148, 100); + } +} + +Draw.prototype.eraseBoard = function() { + this.drawSpiral(5, 60, -148); //Draws a spiral to erase most of the board + + //Erases the edges of the board + doSetTimeout(70, 10, -148, 7500); + + itGo(40, 60, -149, 5, 500); + itGo(10, 55, -149, 5, 500); + itGo(-40, 55, -149, 5, 500); + itGo(-50, 27.5, -149, 5, 500); + itGo(-60, 0, -149, 5, 500); + itGo(-40, -40, -149, 5, 500); + itGo(-30, -60, -149, 5, 500); + itGo(0, -60, -149, 5, 500); + itGo(30, -60, -149, 5, 500); + itGo(50, -20, -149, 5, 500); +} + +Draw.prototype.dropOffEraser = function() { + doSetTimeout(55, 20, -148, 500); + doSetTimeout(63, 25, -130, 500); + doSetTimeout(0, 0, -130, 500); +} + +Draw.prototype.erase = function() { + var objRef = this; + setTimeout(function() { objRef.pickUpEraser() }, 0); + setTimeout(function() { objRef.eraseBoard() }, 5000); + setTimeout(function() { objRef.dropOffEraser() }, 5001); +} + module.exports.Draw = Draw; From 75eaa39fa3a7e3d29e16dfad9917c3506a8a87df Mon Sep 17 00:00:00 2001 From: jackskalitzky Date: Mon, 6 Jul 2015 10:12:35 -0500 Subject: [PATCH 20/43] Changed the erase function to support clock --- software/src/draw.js | 34 ++++++++++++++++++++++++---------- 1 file changed, 24 insertions(+), 10 deletions(-) diff --git a/software/src/draw.js b/software/src/draw.js index 58362d7..30c1c87 100644 --- a/software/src/draw.js +++ b/software/src/draw.js @@ -224,7 +224,7 @@ Draw.prototype.drawSpiral= function(spirals, radius, zLevel) { go(centerX, centerY, penHeight); - for (var degree = 0; degree < spirals * 360; degree++) { + for (var degree = 0; degree < spirals * 360 + 94; degree++) { x1 = x1 + radius/(spirals * 360); y1 = y1 + radius/(spirals * 360); var radians = degree * Math.PI/180; @@ -234,7 +234,7 @@ Draw.prototype.drawSpiral= function(spirals, radius, zLevel) { } spiral = function() { - for (var z = 0; z < spirals*360; z++) { + for (var z = 0; z < spirals*360 + 94; z++) { point = points[z]; doSetTimeout(point.x, point.y, penHeight, 5); } @@ -243,6 +243,12 @@ Draw.prototype.drawSpiral= function(spirals, radius, zLevel) { }; Draw.prototype.pickUpEraser = function() { + doSetTimeout(currentPoint.x, currentPoint.y, -130, 500); + doSetTimeout(0, 50, -130, 500); + doSetTimeout(0, 50, -148, 500); + doSetTimeout(0, 0, -148, 500); + + /* timer = 0; doSetTimeout(currentPoint.x, currentPoint.y, -130, 500); doSetTimeout(65, 25, -130, 500); @@ -260,15 +266,16 @@ Draw.prototype.pickUpEraser = function() { } doSetTimeout(x, y, -148, 100); - } + } */ } Draw.prototype.eraseBoard = function() { - this.drawSpiral(5, 60, -148); //Draws a spiral to erase most of the board + this.drawSpiral(5, 55, -148); //Draws a spiral to erase most of the board - //Erases the edges of the board + //WIP code for erasing the very edges of the board + //Commented by default because it's not reliable + /* doSetTimeout(70, 10, -148, 7500); - itGo(40, 60, -149, 5, 500); itGo(10, 55, -149, 5, 500); itGo(-40, 55, -149, 5, 500); @@ -279,19 +286,26 @@ Draw.prototype.eraseBoard = function() { itGo(0, -60, -149, 5, 500); itGo(30, -60, -149, 5, 500); itGo(50, -20, -149, 5, 500); + */ + } Draw.prototype.dropOffEraser = function() { - doSetTimeout(55, 20, -148, 500); + // doSetTimeout(0, 50, -148, 500); + doSetTimeout(-1, 50, -130, 500); + doSetTimeout(0, 0, -130, 500); + + /* doSetTimeout(55, 20, -148, 500); doSetTimeout(63, 25, -130, 500); - doSetTimeout(0, 0, -130, 500); + doSetTimeout(0, 0, -130, 500); */ } Draw.prototype.erase = function() { var objRef = this; setTimeout(function() { objRef.pickUpEraser() }, 0); - setTimeout(function() { objRef.eraseBoard() }, 5000); - setTimeout(function() { objRef.dropOffEraser() }, 5001); + setTimeout(function() { objRef.eraseBoard() }, 3000); + setTimeout(function() { objRef.dropOffEraser() }, 10000); + console.log(timer); } module.exports.Draw = Draw; From a20844bd5404f54313bc1e04dde0455aca6fb4f7 Mon Sep 17 00:00:00 2001 From: jackskalitzky Date: Mon, 6 Jul 2015 10:12:59 -0500 Subject: [PATCH 21/43] Created functioning clock --- software/src/SVGReader.js | 172 +++++++++++++++++++++++++++++++++----- 1 file changed, 153 insertions(+), 19 deletions(-) diff --git a/software/src/SVGReader.js b/software/src/SVGReader.js index 214a9b0..cfb2fa7 100644 --- a/software/src/SVGReader.js +++ b/software/src/SVGReader.js @@ -6,6 +6,7 @@ var parse = require('svg-path-parser'); var fs = require('fs'); +var draw = require("./draw"); var parseString = require('xml2js').parseString; function SVGReader(args) { @@ -24,8 +25,11 @@ function SVGReader(args) { //> Usage: //> drawSVG("C:/Projects/Tapsterbot/software/src/drawing.svg"); SVGReader.prototype.drawSVG = function(filePath) { + timer = 0; var parsed; + objRef = this; + //Create a JSON string out of the SVG image data //parseString strips away the XML data try { @@ -64,7 +68,7 @@ SVGReader.prototype.drawSVG = function(filePath) { var phoneWidth = this.baseWidth; var phoneHeight = this.baseHeight; - penHeight = -140 + penHeight = -152.5; widthRatio = width / phoneWidth; heightRatio = height / phoneHeight; @@ -72,43 +76,45 @@ SVGReader.prototype.drawSVG = function(filePath) { halfway = {x:width / 2, y:height / 2}; currentPoint = {x:halfway.x, y:halfway.y}; //Start at the center of the canvas, which corresponds to (0,0) on the Tapster - timer = 0; - - if (objArr.svg.g[0].g) { //If there are multiple groups + if (objArr.svg.g[0].g && objArr.svg.g[0].g[0].path) { //If there are multiple groups. Additional check to make sure that there is actually path data for (var i = 0; i < objArr.svg.g[0].g.length; i++) { pathArray = objArr.svg.g[0].g[i].path; - drawImage(); + drawImage(pathArray); } } else if (objArr.svg.g[0].path) { pathArray = objArr.svg.g[0].path; - drawImage(); + drawImage(pathArray); } } -drawImage = function() { +drawImage = function(pathArray) { var d = ""; + console.log(pathArray); for (var i = 0; i < pathArray.length; i++) { //When drawing multiple lines, there are multiple paths - firstPoint = ""; + firstPoint = null; d = pathArray[i].$.d; var commands = parse(d); - interpretCommands(commands); - if (i < (pathArray.length - 1)) { //Smooth transition to the next path - doSetTimeout(mapX(currentPoint.x), mapY(currentPoint.y), penHeight + 10, 150); //Moves the pen up and over so no line is drawn between the two + objRef.interpretCommands(commands); + /* if (i < (pathArray.length - 1)) { //Smooth transition to the next path + doSetTimeout(mapX(currentPoint.x), mapY(currentPoint.y), penHeight + 10, 300); //Moves the pen up and over so no line is drawn between the two doSetTimeout(mapX(parse(pathArray[i+1].$.d)[0].x), mapY(parse(pathArray[i+1].$.d)[0].y), penHeight + 10, 300); - doSetTimeout(mapX(parse(pathArray[i+1].$.d)[0].x), mapY(parse(pathArray[i+1].$.d)[0].y), penHeight, 450); - } + doSetTimeout(mapX(parse(pathArray[i+1].$.d)[0].x), mapY(parse(pathArray[i+1].$.d)[0].y), penHeight, 300); + } */ } + doSetTimeout(mapX(currentPoint.x), mapY(currentPoint.y), penHeight + 10, 300); } //Move from one point to (x, y) move = function(x, y) { + doSetTimeout(mapX(currentPoint.x), mapY(currentPoint.y), penHeight + 10, 150); + doSetTimeout(mapX(x), mapY(y), penHeight + 10, 150); doSetTimeout(mapX(x), mapY(y), penHeight, 150); currentPoint = {x:x, y:y}; //Update the current point (done every time an SVG command is called) - if (!firstPoint) { //Keeps track of the first point, for use with the Z/z command + + if (!firstPoint) //Keeps track of the first point, for use with the Z/z command firstPoint = {x:currentPoint.x, y:currentPoint.y}; //Since the first command of a path is always to Move, this check only occurs here - } } //Move from one point to that that point + x, y @@ -314,6 +320,10 @@ doSetTimeout = function(x, y, z, delay) { timer = timer + delay; }; +setPenHeight = function(penHeight) { + penHeight = penHeight; +} + //Reflects the point (x,y) across the point (x1, y1) //For use with smooth curves reflect = function(x, y, x1, y1) { @@ -331,13 +341,19 @@ reflect = function(x, y, x1, y1) { //Convert points pixel coordinates to Tapster coordinates //Done in two methods for ease of use mapX = function(x) { - var newX = x + transformX; + if (transformX) + var newX = x + transformX; + else + var newX = x; newX = (newX - halfway.x) / widthRatio; //The center of the canvas corresponds to (0, 0) on the Tapster return newX; }; mapY = function(y) { - var newY = y + transformY ; + if (transformY) + var newY = y + transformY; + else + var newY = y; newY = (halfway.y - newY) / heightRatio; return newY; } @@ -383,7 +399,7 @@ dimensionConversion = function(width, height) { } //Goes through the list of commands an generated by the SVG-Path-Parser and calls the corresponding functions -interpretCommands = function(commands) { +SVGReader.prototype.interpretCommands = function(commands) { for (var i = 0; i < commands.length; i++) { var cmdCode = commands[i].code; if (cmdCode == 'M') { @@ -489,11 +505,128 @@ interpretCommands = function(commands) { relArc(commands[i].rx, commands[i].ry, commands[i].xAxisRotation, commands[i].largeArc, commands[i].sweep, commands[i].x, commands[i].y); else if (cmdCode == 'Z' || cmdCode == 'z') { - line(firstPoint.x, firstPoint.y); + (firstPoint.x, firstPoint.y); + firstPoint = null; } } } +SVGReader.prototype.clock = function() { + var dimensions = dimensionConversion("80mm", "95mm"); + width = dimensions.width; + height = dimensions.height; + + timer = 0; + + var drawing = new draw.Draw({ + baseWidth: this.baseWidth, + baseHeight: this.baseHeight + }); + + var phoneWidth = this.baseWidth; + var phoneHeight = this.baseHeight; + + widthRatio = width / phoneWidth; + heightRatio = height / phoneHeight; + + penHeight = -152.5; + halfway = {x:width / 2, y:height / 2}; + currentPoint = {x:halfway.x, y:halfway.y}; //Start at the center of the canvas, which corresponds to (0,0) on the Tapster + + var zero = "M 40.890286,241.44557 30.687127,245.57254 23.885021,257.95342 20.483968,278.58823 20.483968,290.96912 23.885021,311.60393 30.687127,323.98482 40.890286,328.11178 47.692392,328.11178 57.895551,323.98482 64.697657,311.60393 68.09871,290.96912 68.09871,278.58823 64.697657,257.95342 57.895551,245.57254 47.692392,241.44557 40.890286,241.44557"; + var one = "M 50.289453,243.07635 50.289453,329.52761"; + var two = "M 27.618803,262.08031 27.618803,257.95271 30.945521,249.69749 34.27224,245.56989 40.925676,241.44228 54.23255,241.44228 60.885986,245.56989 64.212704,249.69749 67.539423,257.95271 67.539423,266.20792 64.212704,274.46313 57.559268,286.84595 24.292085,328.12202 70.866141,328.12202"; + var three = "M 27.042715,241.44643 64.666988,241.44643 44.144657,274.4608 54.405822,274.4608 61.2466,278.58759 64.666988,282.71439 68.087377,295.09478 68.087377,303.34837 64.666988,315.72875 57.826211,323.98234 47.565046,328.10914 37.30388,328.10914 27.042715,323.98234 23.622326,319.85555 20.201938,311.60196"; + var four = "M 54.845427,241.51709 22.803997,300.19076 70.866142,300.19076 M 54.845427,241.51709 54.845427,329.5276"; + var five = "M 61.700462,246.77512 26.654546,246.77512 23.149954,281.56734 26.654546,277.70154 37.168321,273.83574 47.682096,273.83574 58.19587,277.70154 65.205054,285.43315 68.709645,297.03055 68.709645,304.76216 65.205054,316.35956 58.19587,324.09117 47.682096,327.95697 37.168321,327.95697 26.654546,324.09117 23.149954,320.22537 19.645363,312.49376"; + var six = "M 64.294901,253.7887 60.65789,245.53396 49.746856,241.40659 42.472833,241.40659 31.561799,245.53396 24.287776,257.91607 20.650765,278.55291 20.650765,299.18976 24.287776,315.69923 31.561799,323.95397 42.472833,328.08134 46.109844,328.08134 57.020878,323.95397 64.294901,315.69923 67.931912,303.31713 67.931912,299.18976 64.294901,286.80765 57.020878,278.55291 46.109844,274.42554 42.472833,274.42554 31.561799,278.55291 24.287776,286.80765 20.650765,299.18976"; + var seven = "M 68.262659,241.73033 32.158284,328.90301 M 17.716535,241.73033 68.262659,241.73033"; + var eight = "M 37.489233,241.44557 27.286074,245.57254 23.885021,253.82646 23.885021,262.08039 27.286074,270.33431 34.08818,274.46127 47.692392,278.58823 57.895551,282.7152 64.697657,290.96912 68.09871,299.22304 68.09871,311.60393 64.697657,319.85785 61.296604,323.98482 51.093445,328.11178 37.489233,328.11178 27.286074,323.98482 23.885021,319.85785 20.483968,311.60393 20.483968,299.22304 23.885021,290.96912 30.687127,282.7152 40.890286,278.58823 54.494498,274.46127 61.296604,270.33431 64.697657,262.08039 64.697657,253.82646 61.296604,245.57254 51.093445,241.44557 37.489233,241.44557"; + var nine = "M 67.931912,270.29818 64.2949,282.68028 57.020878,290.93502 46.109844,295.06239 42.472833,295.06239 31.561799,290.93502 24.287776,282.68028 20.650765,270.29818 20.650765,266.17081 24.287776,253.7887 31.561799,245.53396 42.472833,241.40659 46.109844,241.40659 57.020878,245.53396 64.2949,253.7887 67.931912,270.29818 67.931912,290.93502 64.2949,311.57186 57.020878,323.95397 46.109844,328.08134 38.835821,328.08134 27.924787,323.95397 24.287776,315.69923"; + + pathData = [zero, one, two, three, four, five, six, seven, eight, nine]; + + var colon = "M 175.73227,307.21546 175.81889,307.21546 175.81889,307.21262 175.73227,307.21262 175.73227,307.21546 Z M 175.73227,253.34648 175.81889,253.34648 175.81889,270.34364 175.73227,270.34364 175.73227,253.34648 Z"; + + objRef = this; + + toPixels = function(mm) { + return mm * 3.779527559; + } + + drawTime = function(arrayOfPaths) { + for (var i = 0; i < arrayOfPaths.length; i++) { + objRef.interpretCommands(arrayOfPaths[i]); + + if (i == 1) + objRef.interpretCommands(parse(colon)); + } + doSetTimeout(0, 0, -140, 250); + } + + getTheTime = function() { + var currentTime = new Date() + var hours = String(currentTime.getHours()); + var minutes = String(currentTime.getMinutes()); + + if (hours < 10) + hours = "0" + hours; + 1 + if (minutes < 10) + minutes = "0" + minutes; + + var timeArray = new Array(); + for (var i = 0; i < hours.length; i++) { + timeArray.push(hours.charAt(i)); + } + + for (var i = 0; i < minutes.length; i++) { + timeArray.push(minutes.charAt(i)); + } + + return timeArray; + } + + convertToPath = function(timeArray) { + var pathArray = new Array(); + var offset = toPixels(5); + var commandArray = new Array(); + for (var i = 0; i < timeArray.length; i++) { + var data = pathData[timeArray[i]]; + var commands = parse(data); + + for (var x = 0; x < commands.length; x++) { + if (commands[x].code != 'Z' || commands[x].code != 'z') { + commands[x].x += offset; + if (commands[x].x1) + commands[x].x1 += offset; + if (commands[x].x2) + commands[x].x2 += offset; + } + } + + commandArray.push(commands); + + offset += toPixels(18); + if (i == 1) + offset += toPixels(4); + } + return commandArray; + } + + tellTime = function() { + timer = 0; + setTimeout(function() { drawing.erase() }, 0); + setTimeout(function() { drawTime(convertToPath(getTheTime())) }, 12500); + //drawTime(convertToPath([1, 7, 1, 7])); + } + + tellTime(); + setInterval(function() { tellTime() }, 60000); +} + + + //Timer variable for use with the doSetTimeout method var timer = 0; @@ -511,5 +644,6 @@ var penHeight; var transformX; var transformY; +var objRef; module.exports.SVGReader = SVGReader; From f7e4d79cc4ce00e612671de5ba3d92dbcc16e662 Mon Sep 17 00:00:00 2001 From: jackskalitzky Date: Tue, 7 Jul 2015 16:48:01 -0500 Subject: [PATCH 22/43] Updated clock-related functions --- software/src/SVGReader.js | 228 ++++++++++++++++++++++++++++++-------- 1 file changed, 182 insertions(+), 46 deletions(-) diff --git a/software/src/SVGReader.js b/software/src/SVGReader.js index cfb2fa7..f3b12f3 100644 --- a/software/src/SVGReader.js +++ b/software/src/SVGReader.js @@ -14,9 +14,9 @@ function SVGReader(args) { this.baseHeight = 0; if (args) { - var keys = Object.keys(args) - keys.forEach(function(key){ - this[key] = args[key] + var keys = Object.keys(args) + keys.forEach(function(key){ + this[key] = args[key] }, this) } } @@ -24,10 +24,17 @@ function SVGReader(args) { //Draws from an SVG image specified by filepath //> Usage: //> drawSVG("C:/Projects/Tapsterbot/software/src/drawing.svg"); -SVGReader.prototype.drawSVG = function(filePath) { +//connect is a special flag that indicates that each path should be drawn connected to each other +//It is really only used for drawing in cursive and does not need to be specified otherwise +SVGReader.prototype.drawSVG = function(filePath, connect) { timer = 0; var parsed; + if (connect) + connected = connect; + else + connected = null; + objRef = this; //Create a JSON string out of the SVG image data @@ -68,7 +75,7 @@ SVGReader.prototype.drawSVG = function(filePath) { var phoneWidth = this.baseWidth; var phoneHeight = this.baseHeight; - penHeight = -152.5; + penHeight = this.drawHeight; widthRatio = width / phoneWidth; heightRatio = height / phoneHeight; @@ -83,15 +90,24 @@ SVGReader.prototype.drawSVG = function(filePath) { } } - else if (objArr.svg.g[0].path) { + else if (objArr.svg.g.length > 0) { //Depending on how the paths are grouped, it is possible that this value can be greater than zero + for (var i = 0; i < objArr.svg.g.length; i++) { //If this is the case, loop through the array to get the path data + pathArray = objArr.svg.g[i].path; + drawImage(pathArray); + } + } + + else if (objArr.svg.g[0].path) { //If there is only one group pathArray = objArr.svg.g[0].path; drawImage(pathArray); } + + //svg.drawSVG("C:/Projects/Tapsterbot/software/src/drawing.svg", true) } drawImage = function(pathArray) { var d = ""; - console.log(pathArray); + firstMove = null; //After a group is done being drawn, firstMove is reset as there has not yet been a first move made in the next group for (var i = 0; i < pathArray.length; i++) { //When drawing multiple lines, there are multiple paths firstPoint = null; d = pathArray[i].$.d; @@ -108,11 +124,24 @@ drawImage = function(pathArray) { //Move from one point to (x, y) move = function(x, y) { - doSetTimeout(mapX(currentPoint.x), mapY(currentPoint.y), penHeight + 10, 150); - doSetTimeout(mapX(x), mapY(y), penHeight + 10, 150); - doSetTimeout(mapX(x), mapY(y), penHeight, 150); + if (!connected) { //If the paths should not be connected, lift up the pen and move over so that a line is not drawn + doSetTimeout(mapX(currentPoint.x), mapY(currentPoint.y), penHeight + 10, 150); + doSetTimeout(mapX(x), mapY(y), penHeight + 10, 150); + doSetTimeout(mapX(x), mapY(y), penHeight, 150); + } + else if (connected && !firstMove) { //If the paths should be connected and a move has not been made, lift up the pen and move to the first point + doSetTimeout(mapX(currentPoint.x), mapY(currentPoint.y), penHeight + 10, 150); + doSetTimeout(mapX(x), mapY(y), penHeight + 10, 150); + doSetTimeout(mapX(x), mapY(y), penHeight, 150); + } + else //If the paths should be connected and a move has been made, just draw a line between the two paths + doSetTimeout(mapX(x), mapY(y), penHeight, 150); + currentPoint = {x:x, y:y}; //Update the current point (done every time an SVG command is called) + if (!firstMove) //Keeps track of if a move has been made or not. + firstMove = true; + if (!firstPoint) //Keeps track of the first point, for use with the Z/z command firstPoint = {x:currentPoint.x, y:currentPoint.y}; //Since the first command of a path is always to Move, this check only occurs here } @@ -165,7 +194,7 @@ cubicCurve = function(x1, y1, x2, y2, x, y) { var curvePts = new Array(); curvePts = b(x1, y1, x2, y2, x, y, 5); //5 is an arbitrarily-chosen value. It creates a smooth-looking curve without calculating too many points for (var i = 0;i < curvePts.length; i++) - doSetTimeout(mapX(curvePts[i].x), mapY(curvePts[i].y), penHeight, 2); + doSetTimeout(mapX(curvePts[i].x), mapY(curvePts[i].y), penHeight, 15); } //Draws a relative cubic Bezier curve @@ -320,6 +349,7 @@ doSetTimeout = function(x, y, z, delay) { timer = timer + delay; }; +//A function for setting the penHeight from the command line setPenHeight = function(penHeight) { penHeight = penHeight; } @@ -505,22 +535,26 @@ SVGReader.prototype.interpretCommands = function(commands) { relArc(commands[i].rx, commands[i].ry, commands[i].xAxisRotation, commands[i].largeArc, commands[i].sweep, commands[i].x, commands[i].y); else if (cmdCode == 'Z' || cmdCode == 'z') { - (firstPoint.x, firstPoint.y); + line(firstPoint.x, firstPoint.y); firstPoint = null; } } } +//Creates a working clock +//To-do: Add support for user-specified fonts SVGReader.prototype.clock = function() { - var dimensions = dimensionConversion("80mm", "95mm"); + var dimensions = dimensionConversion("80mm", "95mm"); //Since no dimensions are specified, assume the default + //To-do: Pul this from a config file width = dimensions.width; height = dimensions.height; timer = 0; - var drawing = new draw.Draw({ + var drawing = new draw.Draw({ //draw is used for the erase function baseWidth: this.baseWidth, - baseHeight: this.baseHeight + baseHeight: this.baseHeight, + drawHeight: this.drawHeight }); var phoneWidth = this.baseWidth; @@ -529,10 +563,12 @@ SVGReader.prototype.clock = function() { widthRatio = width / phoneWidth; heightRatio = height / phoneHeight; - penHeight = -152.5; + penHeight = this.drawHeight; halfway = {x:width / 2, y:height / 2}; currentPoint = {x:halfway.x, y:halfway.y}; //Start at the center of the canvas, which corresponds to (0,0) on the Tapster + //Currently hardcoded path data + //To-do: Allow users to specify fonts var zero = "M 40.890286,241.44557 30.687127,245.57254 23.885021,257.95342 20.483968,278.58823 20.483968,290.96912 23.885021,311.60393 30.687127,323.98482 40.890286,328.11178 47.692392,328.11178 57.895551,323.98482 64.697657,311.60393 68.09871,290.96912 68.09871,278.58823 64.697657,257.95342 57.895551,245.57254 47.692392,241.44557 40.890286,241.44557"; var one = "M 50.289453,243.07635 50.289453,329.52761"; var two = "M 27.618803,262.08031 27.618803,257.95271 30.945521,249.69749 34.27224,245.56989 40.925676,241.44228 54.23255,241.44228 60.885986,245.56989 64.212704,249.69749 67.539423,257.95271 67.539423,266.20792 64.212704,274.46313 57.559268,286.84595 24.292085,328.12202 70.866141,328.12202"; @@ -546,32 +582,53 @@ SVGReader.prototype.clock = function() { pathData = [zero, one, two, three, four, five, six, seven, eight, nine]; - var colon = "M 175.73227,307.21546 175.81889,307.21546 175.81889,307.21262 175.73227,307.21262 175.73227,307.21546 Z M 175.73227,253.34648 175.81889,253.34648 175.81889,270.34364 175.73227,270.34364 175.73227,253.34648 Z"; + var colon = "M 165.73227,300.21546 Z M 165.73227,253.34648 Z"; + var arrayTime = new Array(); objRef = this; + //Simple function for converting units in millimeters to units in pixels + //Based on the fixed ratio between mm and px toPixels = function(mm) { return mm * 3.779527559; } - drawTime = function(arrayOfPaths) { + //Draws the time + //Takes an array of path data and a callback function + drawTime = function(arrayOfPaths, callback) { for (var i = 0; i < arrayOfPaths.length; i++) { - objRef.interpretCommands(arrayOfPaths[i]); + objRef.interpretCommands(arrayOfPaths[i]); //Interprets the path data and draws the number - if (i == 1) + firstPoint = null; + + if (i == 1) //Inserts the colon between the second and third number objRef.interpretCommands(parse(colon)); } doSetTimeout(0, 0, -140, 250); + setTimeout(function() { endTime = new Date().getTime() }, timer + 1); //Gets the time after drawTime finishes executing + //Because of the way doSetTimeout works, timer + 1 occurs (is meant to occur) a millisecond after the doSetTimeout call + setTimeout(function() { difference = endTime - startTime }, timer + 3); + setTimeout(function() { callback() }, timer + 4); } + //Gets the curent time and converts it into an array of four digits + //The first two digits are the hours, the last two are the minutes getTheTime = function() { var currentTime = new Date() var hours = String(currentTime.getHours()); var minutes = String(currentTime.getMinutes()); + //Converts from 24 hour time to 12 hour time + if (hours > 12) + hours -= 12; + else if (hours === 0) + hours = 12; + + //Adds a placeholder zero to ensure that there are four digits in the time + //The zero is not actually drawn if (hours < 10) hours = "0" + hours; - 1 + if (minutes < 10) minutes = "0" + minutes; @@ -583,46 +640,119 @@ SVGReader.prototype.clock = function() { for (var i = 0; i < minutes.length; i++) { timeArray.push(minutes.charAt(i)); } - + arrayTime = timeArray; //Stores the array in another variable so it can be accessed elsewhere without calling the function again return timeArray; } + //Converts the digits in the timeArray into path data convertToPath = function(timeArray) { - var pathArray = new Array(); - var offset = toPixels(5); - var commandArray = new Array(); - for (var i = 0; i < timeArray.length; i++) { - var data = pathData[timeArray[i]]; - var commands = parse(data); - - for (var x = 0; x < commands.length; x++) { - if (commands[x].code != 'Z' || commands[x].code != 'z') { - commands[x].x += offset; - if (commands[x].x1) - commands[x].x1 += offset; - if (commands[x].x2) - commands[x].x2 += offset; + var pathArray = new Array(); + var offset = toPixels(3); //The first digit is drawn three mm from the left side + var commandArray = new Array(); + //Loops through the digits in timeArray + for (var i = 0; i < timeArray.length; i++) { + if (i == 0 && timeArray[i] == 0) { //If the first digit is 0 (if the hours < 10) AND the loop is on the first digit + offset += toPixels(5); //Changes the offset so that the three digits that will be drawn will be centered + commandArray.push(["Z"]); //The Z command, without a firstPoint value, will not draw anything, but can still be interpreted without errors + colon = "M 118.73227,307.21546 Z M 118.73227,253.34648 Z"; //Changes the path data of the colon so that it will still be in the correct location + } + else { + var data = pathData[timeArray[i]]; + var commands = parse(data); + + //Loops through the commands in the array and adds the offset to each x value + for (var x = 0; x < commands.length; x++) { + if (commands[x].code != 'Z' || commands[x].code != 'z') { //Every command but the close path commands have an x value + commands[x].x += offset; + if (commands[x].x1) //Some commands (curves and arcs) have an x1 value + commands[x].x1 += offset; + if (commands[x].x2) //Some commands (cubic curves) have an x2 value + commands[x].x2 += offset; + } + } + + commandArray.push(commands); + + if (timeArray[0] != 0) + colon = "M 165.73227,300.21546 Z M 165.73227,253.34648 Z"; //Ensures that the colon has the correct path data + //Withotu this check, once the time changes from hours < 10 to hours >= 10, the colon would be in the incorrect place + + offset += toPixels(18); //Adds an offset to each digit + //Each digit should be 15mm wide, with 3mm space between each digit + if (i == 1) + offset += toPixels(4); //Adds an extra 4mm of space to account for the colon } } - commandArray.push(commands); + return commandArray; + } - offset += toPixels(18); - if (i == 1) - offset += toPixels(4); - } - return commandArray; + //Draws a circle to indicate the amount of time left in the minute + timeCircle = function() { + timer = 0; + + //Draws a circle, given an array of points and the amount of delay in between each point + circle = function(array, delay) { + for (var i=0; i -2; degree--) { + var radians = (degree + 90) * Math.PI/180; //Add 90 to degree so that the circle starts at the top, not at the right + var x = centerX + radius * Math.cos(radians); + var y = centerY + radius * Math.sin(radians); + points.push({x:x,y:y}); + } + + setTimeout(function() { circle(points, calcDelay(calcTimeLeft(), points.length)) }, 0); } + //Tells the time tellTime = function() { timer = 0; - setTimeout(function() { drawing.erase() }, 0); - setTimeout(function() { drawTime(convertToPath(getTheTime())) }, 12500); - //drawTime(convertToPath([1, 7, 1, 7])); + + startTime = new Date().getTime(); + + drawing.erase(function() { + drawTime(convertToPath(getTheTime()), function() { + timeCircle(); + }); + }); + //setTimeout(function() { drawTime(convertToPath(getTheTime())) }, 12500); + //setTimeout(function() { drawTime(convertToPath([0, 8, 5, 8]))}, 12500); + //setTimeout(function() { timeCircle() }, 12501); } tellTime(); - setInterval(function() { tellTime() }, 60000); + clockTimer = setInterval(function() { tellTime() }, 60000); //Repeat every 60 seconds +} + +//A function to stop the clock +//The clock will finish erasing, drawing, and moving the arms in a circle, but it will not Repeat +SVGReader.prototype.clearClock = function() { + clearInterval(clockTimer); } @@ -645,5 +775,11 @@ var penHeight; var transformX; var transformY; var objRef; +var connected; +var startTime; +var endTime; +var difference; +var clockTimer; +var firstMove; module.exports.SVGReader = SVGReader; From c3d003dc42e69455f3427ee2b80696b08bc86a72 Mon Sep 17 00:00:00 2001 From: jackskalitzky Date: Tue, 7 Jul 2015 16:48:22 -0500 Subject: [PATCH 23/43] Updated erase functions --- software/src/draw.js | 44 +++++++++++++++++++++++++------------------- 1 file changed, 25 insertions(+), 19 deletions(-) diff --git a/software/src/draw.js b/software/src/draw.js index 30c1c87..ddefe4b 100644 --- a/software/src/draw.js +++ b/software/src/draw.js @@ -45,7 +45,7 @@ doSetTimeout = function(x, y, z, delay) { //Draws points + 1 points //Delay is the total amount of time that it takes //Each point takes delay / points time to draw -itGo = function(x, y, z, points, delay) { +Draw.prototype.itGo = function(x, y, z, points, delay) { var x1 = currentPoint.x; var y1 = currentPoint.y; var deltaX = x - x1; @@ -69,7 +69,7 @@ itGo = function(x, y, z, points, delay) { var baseHeight, baseWidth, canvasHeight, canvasWidth, heightRatio, widthRatio, halfway; var currentPoint = {x: 0, y: 0, z: -140}; -var penHeight = -140; +var penHeight = this.drawHeight; //A timer variable for use with doSetTimeout() var timer = 0; @@ -179,26 +179,26 @@ Draw.prototype.drawStar = function() { */ }; -Draw.prototype.drawCircle = function() { +Draw.prototype.drawCircle = function(radius) { timer = 0; var centerX=0; var centerY=0; - var radius=20; + var radius=radius; // an array to save your points var points=[]; // populate array with points along a circle //Goes to 390 degrees so that the circle is actually completed - for (var degree=0; degree<390; degree++) { - var radians = degree * Math.PI/180; + for (var degree=0; degree < 365; degree++) { + var radians = (degree + 90) * Math.PI/180; var x = centerX + radius * Math.cos(radians); var y = centerY + radius * Math.sin(radians); points.push({x:x,y:y}); } circle = function() { - for (var i=0; i<390; i+=1) { + for (var i=0; i Date: Thu, 9 Jul 2015 16:38:03 -0500 Subject: [PATCH 24/43] Updated test function --- software/src/draw.js | 151 ++++++++++++++++++++++--------------------- 1 file changed, 76 insertions(+), 75 deletions(-) diff --git a/software/src/draw.js b/software/src/draw.js index ddefe4b..397bd7a 100644 --- a/software/src/draw.js +++ b/software/src/draw.js @@ -7,6 +7,8 @@ //> draw.erase() var fs = require('fs'); +var objRef; +var calculated, spiralPts; function Draw(args) { this.baseWidth = 0; @@ -18,6 +20,8 @@ function Draw(args) { this[key] = args[key]; }, this) } + penHeight = this.drawHeight; + objRef = this; } //Maps point from canvas to the Tapster coordinate plane @@ -161,41 +165,44 @@ Draw.prototype.drawSquare = function(sideLength, n) { }; //Draws a star to test that the Tapster bot is working properly -Draw.prototype.drawStar = function() { - timer = 0; - doSetTimeout(-20, -20, penHeight, 1000); - doSetTimeout(0, 20, penHeight, 1000); - doSetTimeout(20, -20, penHeight, 1000); - doSetTimeout(-20, 10, penHeight, 1000); - doSetTimeout(20, 10, penHeight, 1000); - doSetTimeout(-20, -20, penHeight, 1000); - - /*setTimeout(function() { go(-20, -20, penHeight); }, 0); //Bottom left - setTimeout(function() { go(0, 20, penHeight); }, 1000); //Top - setTimeout(function() { go(20, -20, penHeight); }, 2000); //Bottom right - setTimeout(function() { go(-20, 10, penHeight); }, 3000); //Left - setTimeout(function() { go(20, 10, penHeight); }, 4000); //Right - setTimeout(function() { go(-20, -20, penHeight); }, 5000); //Starting position - */ +Draw.prototype.drawStar = function(x, y, x1, y1, x2, y2) { + doSetTimeout(x, y, penHeight, 1000); + doSetTimeout(x1, y1, penHeight, 1000); + doSetTimeout(x2, y2, penHeight, 1000); + doSetTimeout(x, y1 * 3 / 4, penHeight, 1000); + doSetTimeout(x2, y1 * 3 / 4, penHeight, 1000); + doSetTimeout(x, y, penHeight, 1000); }; -Draw.prototype.drawCircle = function(radius) { + +Draw.prototype.drawTriangle = function(x, y, x1, y1, x2, y2) { + timer = 0; + doSetTimeout(x, y, penHeight, 1000); + doSetTimeout(x1, y1, penHeight, 1000); + doSetTimeout(x2, y2, penHeight, 1000); + doSetTimeout(x, y, penHeight, 1000); +} + +Draw.prototype.drawCircle = function(radius, x, y) { timer = 0; - var centerX=0; - var centerY=0; + var centerX=x; + var centerY=y; var radius=radius; // an array to save your points var points=[]; // populate array with points along a circle - //Goes to 390 degrees so that the circle is actually completed + //Goes slightly over so that the circle is actually completed for (var degree=0; degree < 365; degree++) { var radians = (degree + 90) * Math.PI/180; var x = centerX + radius * Math.cos(radians); var y = centerY + radius * Math.sin(radians); points.push({x:x,y:y}); } + + doSetTimeout(points[0].x, points[0].y, penHeight + 10, 20); + doSetTimeout(points[0].x, points[0].y, penHeight, 20); circle = function() { for (var i=0; i 0 || y > 0) { - if (x / 10 > 1) - x -= 10; - else if (y / 10 > 1) - y -= 10; - else { - x = 0; - y = 0; - } - - doSetTimeout(x, y, -148, 100); - } */ } Draw.prototype.eraseBoard = function() { eraseHeight = this.drawHeight + 5.25; - this.drawSpiral(4, 55, eraseHeight); //Draws a spiral to erase most of the board - - //WIP code for erasing the very edges of the board - //Commented by default because it's not reliable - /* - doSetTimeout(70, 10, -148, 7500); - itGo(40, 60, -149, 5, 500); - itGo(10, 55, -149, 5, 500); - itGo(-40, 55, -149, 5, 500); - itGo(-50, 27.5, -149, 5, 500); - itGo(-60, 0, -149, 5, 500); - itGo(-40, -40, -149, 5, 500); - itGo(-30, -60, -149, 5, 500); - itGo(0, -60, -149, 5, 500); - itGo(30, -60, -149, 5, 500); - itGo(50, -20, -149, 5, 500); - */ + if (!calculated) { + spiralPts = this.drawSpiral(4, 55, eraseHeight); + calculated = true; + } + else { + this.drawSpiral(4, 55, eraseHeight, spiralPts); + } } Draw.prototype.dropOffEraser = function() { eraseHeight = this.drawHeight + 5.25; doSetTimeout(0, 50, eraseHeight, 500); - doSetTimeout(-2, 50, -130, 500); + doSetTimeout(-4, 50, -130, 500); doSetTimeout(0, 0, -130, 500); - - /* doSetTimeout(55, 20, -148, 500); - doSetTimeout(63, 25, -130, 500); - doSetTimeout(0, 0, -130, 500); */ } Draw.prototype.erase = function(callback) { @@ -310,6 +310,7 @@ Draw.prototype.erase = function(callback) { setTimeout(function() { objRef.pickUpEraser() }, 0); setTimeout(function() { objRef.eraseBoard() }, 3000); setTimeout(function() { objRef.dropOffEraser() }, 10000); + if (callback) setTimeout(function() {callback() }, 12500); } From f68b1b4dd6482bad319159f96a972ce4e60b9926 Mon Sep 17 00:00:00 2001 From: jackskalitzky Date: Thu, 9 Jul 2015 16:40:25 -0500 Subject: [PATCH 25/43] Updated hello functions --- software/src/SVGReader.js | 175 +++++++++++++++++++++++++++++--------- 1 file changed, 135 insertions(+), 40 deletions(-) diff --git a/software/src/SVGReader.js b/software/src/SVGReader.js index f3b12f3..e47be74 100644 --- a/software/src/SVGReader.js +++ b/software/src/SVGReader.js @@ -12,15 +12,22 @@ var parseString = require('xml2js').parseString; function SVGReader(args) { this.baseWidth = 0; this.baseHeight = 0; + this.drawHeight = 0; if (args) { var keys = Object.keys(args) keys.forEach(function(key){ - this[key] = args[key] - }, this) - } + this[key] = args[key] + }, this) + } + objRef = this; } +//The time, in milliseconds, for each command to execute +//Note: Some commands, such as Move, can take more than this amount of time to execute +//To-do: Pull this from a config file? +var delay = 150; + //Draws from an SVG image specified by filepath //> Usage: //> drawSVG("C:/Projects/Tapsterbot/software/src/drawing.svg"); @@ -40,7 +47,7 @@ SVGReader.prototype.drawSVG = function(filePath, connect) { //Create a JSON string out of the SVG image data //parseString strips away the XML data try { - parseString(fs.readFileSync(filePath, "utf8"), function(err, result) { + parseString(fs.readFileSync(filePath, "utf8"), function(err, result) { parsed = JSON.stringify(result, null, 1); }); } catch (e) { @@ -64,7 +71,6 @@ SVGReader.prototype.drawSVG = function(filePath, connect) { transformY = 0; //Check for translation and account for it - //To-do: Do this more elegantly if (objArr.svg.g[0].$ && objArr.svg.g[0].$.transform) { //Done in multiple checks to avoid errors being thrown var transString = objArr.svg.g[0].$.transform; var subX = transString.indexOf("("); @@ -102,7 +108,8 @@ SVGReader.prototype.drawSVG = function(filePath, connect) { drawImage(pathArray); } - //svg.drawSVG("C:/Projects/Tapsterbot/software/src/drawing.svg", true) + doSetTimeout(0, 49, -130, delay); + setTimeout(function() { timer = 0 }, timer + 5); } drawImage = function(pathArray) { @@ -119,23 +126,23 @@ drawImage = function(pathArray) { doSetTimeout(mapX(parse(pathArray[i+1].$.d)[0].x), mapY(parse(pathArray[i+1].$.d)[0].y), penHeight, 300); } */ } - doSetTimeout(mapX(currentPoint.x), mapY(currentPoint.y), penHeight + 10, 300); + doSetTimeout(mapX(currentPoint.x), mapY(currentPoint.y), penHeight + 10, delay); } //Move from one point to (x, y) move = function(x, y) { if (!connected) { //If the paths should not be connected, lift up the pen and move over so that a line is not drawn - doSetTimeout(mapX(currentPoint.x), mapY(currentPoint.y), penHeight + 10, 150); - doSetTimeout(mapX(x), mapY(y), penHeight + 10, 150); - doSetTimeout(mapX(x), mapY(y), penHeight, 150); + doSetTimeout(mapX(currentPoint.x), mapY(currentPoint.y), penHeight + 10, delay); + doSetTimeout(mapX(x), mapY(y), penHeight + 10, delay); + doSetTimeout(mapX(x), mapY(y), penHeight, delay); } else if (connected && !firstMove) { //If the paths should be connected and a move has not been made, lift up the pen and move to the first point - doSetTimeout(mapX(currentPoint.x), mapY(currentPoint.y), penHeight + 10, 150); - doSetTimeout(mapX(x), mapY(y), penHeight + 10, 150); - doSetTimeout(mapX(x), mapY(y), penHeight, 150); + doSetTimeout(mapX(currentPoint.x), mapY(currentPoint.y), penHeight + 10, delay); + doSetTimeout(mapX(x), mapY(y), penHeight + 10, delay); + doSetTimeout(mapX(x), mapY(y), penHeight, delay); } else //If the paths should be connected and a move has been made, just draw a line between the two paths - doSetTimeout(mapX(x), mapY(y), penHeight, 150); + doSetTimeout(mapX(x), mapY(y), penHeight, delay); currentPoint = {x:x, y:y}; //Update the current point (done every time an SVG command is called) @@ -156,7 +163,7 @@ relMove = function(x, y) { //Draw a line from one point to (x, y) line = function(x, y) { - doSetTimeout(mapX(x), mapY(y), penHeight, 150); + doSetTimeout(mapX(x), mapY(y), penHeight, delay); currentPoint = {x:x, y:y}; } @@ -194,7 +201,7 @@ cubicCurve = function(x1, y1, x2, y2, x, y) { var curvePts = new Array(); curvePts = b(x1, y1, x2, y2, x, y, 5); //5 is an arbitrarily-chosen value. It creates a smooth-looking curve without calculating too many points for (var i = 0;i < curvePts.length; i++) - doSetTimeout(mapX(curvePts[i].x), mapY(curvePts[i].y), penHeight, 15); + doSetTimeout(mapX(curvePts[i].x), mapY(curvePts[i].y), penHeight, delay / curvePts.length); } //Draws a relative cubic Bezier curve @@ -227,7 +234,7 @@ quadraticCurve = function(x1, y1, x, y) { var curvePts = new Array(); curvePts = q(x1, y1, x, y, 5); for (var i = 0; i < curvePts.length; i++) - doSetTimeout(mapX(curvePts[i].x), mapY(curvePts[i].y), penHeight, 2); + doSetTimeout(mapX(curvePts[i].x), mapY(curvePts[i].y), penHeight, delay / curvePts.length); } //Draws a relative quadratic Bezier curve @@ -331,7 +338,7 @@ arc = function(rx, ry, rotation, largeArc, sweep, x, y) { var ptArray = a(rx, ry, largeArc, sweep, x, y, 7); for (var i = 0; i < ptArray.length; i++) { - doSetTimeout(mapX(ptArray[i].x), mapY(ptArray[i].y), penHeight, 75); + doSetTimeout(mapX(ptArray[i].x), mapY(ptArray[i].y), penHeight, delay / ptArray.length); } } @@ -344,9 +351,9 @@ relArc = function(rx, ry, rotation, largeArc, sweep, x, y) { } //A separate setTimeout method so that delays work properly -doSetTimeout = function(x, y, z, delay) { +doSetTimeout = function(x, y, z, timeDelay) { setTimeout(function() { go(x, y, z) }, timer); - timer = timer + delay; + timer = timer + timeDelay; }; //A function for setting the penHeight from the command line @@ -545,16 +552,20 @@ SVGReader.prototype.interpretCommands = function(commands) { //To-do: Add support for user-specified fonts SVGReader.prototype.clock = function() { var dimensions = dimensionConversion("80mm", "95mm"); //Since no dimensions are specified, assume the default - //To-do: Pul this from a config file + //To-do: Pull this from a config file width = dimensions.width; height = dimensions.height; + objRef = this; + timer = 0; + connected = false; - var drawing = new draw.Draw({ //draw is used for the erase function - baseWidth: this.baseWidth, - baseHeight: this.baseHeight, - drawHeight: this.drawHeight + //Used to access the erase functions + var drawing = new draw.Draw({ + baseWidth: objRef.baseWidth, + baseHeight: objRef.baseHeight, + drawHeight: objRef.drawHeight }); var phoneWidth = this.baseWidth; @@ -604,7 +615,7 @@ SVGReader.prototype.clock = function() { if (i == 1) //Inserts the colon between the second and third number objRef.interpretCommands(parse(colon)); } - doSetTimeout(0, 0, -140, 250); + doSetTimeout(0, 0, -140, delay); setTimeout(function() { endTime = new Date().getTime() }, timer + 1); //Gets the time after drawTime finishes executing //Because of the way doSetTimeout works, timer + 1 occurs (is meant to occur) a millisecond after the doSetTimeout call setTimeout(function() { difference = endTime - startTime }, timer + 3); @@ -675,7 +686,7 @@ SVGReader.prototype.clock = function() { if (timeArray[0] != 0) colon = "M 165.73227,300.21546 Z M 165.73227,253.34648 Z"; //Ensures that the colon has the correct path data - //Withotu this check, once the time changes from hours < 10 to hours >= 10, the colon would be in the incorrect place + //Without this check, once the hours changed from single to double digits, the colon would be in the incorrect place offset += toPixels(18); //Adds an offset to each digit //Each digit should be 15mm wide, with 3mm space between each digit @@ -692,10 +703,10 @@ SVGReader.prototype.clock = function() { timer = 0; //Draws a circle, given an array of points and the amount of delay in between each point - circle = function(array, delay) { + circle = function(array, timeDelay) { for (var i=0; i -2; degree--) { var radians = (degree + 90) * Math.PI/180; //Add 90 to degree so that the circle starts at the top, not at the right var x = centerX + radius * Math.cos(radians); @@ -750,9 +760,102 @@ SVGReader.prototype.clock = function() { } //A function to stop the clock -//The clock will finish erasing, drawing, and moving the arms in a circle, but it will not Repeat +//The clock will finish erasing, drawing, and moving the arms in a circle, but it will not repeat SVGReader.prototype.clearClock = function() { clearInterval(clockTimer); + console.log("Stopping clock."); +} + +//Says 'hello' in multiple languages +SVGReader.prototype.hello = function() { + timer = 0; + var fileArray = fs.readdirSync("./hello"); + var fileNum; + objRef = this; + + //Used to access the erase functions + var drawing = new draw.Draw({ + baseWidth: objRef.baseWidth, + baseHeight: objRef.baseHeight, + drawHeight: objRef.drawHeight + }); + + //Picks a file at random from a folder of 'hello's + pickFile = function() { + do { + fileNum = Math.floor(Math.random() * fileArray.length); + } + while (fileNum == lastNum1 || fileNum == lastNum2); //Generates numbers until a number that has not been used in the past two calls is picked + //A language can only be used every third time + + lastNum2 = lastNum1; //Adjusts the last two numbers used + lastNum1 = fileNum; + + //Checks to see if the characters should be drawn connected or not + //Specified in file name + if (fileArray[fileNum].charAt(7) === 'T') + connect = true; + else + connect = false; + + return "./hello/" + fileArray[fileNum]; + } + + //Says hello. + sayHello = function() { + var fileChoice = pickFile(); + drawing.erase(function() { + setTimeout(function() { objRef.drawSVG(fileChoice, connect) }, 10000); + }); + } + + sayHello(); + helloTimer = setInterval(function() { sayHello() }, 60000); //Repeat every minute +} + +//A function to cancel saying hello +//The robot will finish writing/erasing if it has already started but after that it will stop +SVGReader.prototype.sayGoodbye = function() { + clearInterval(helloTimer); + console.log("Goodbye!"); +} + +//Cycles through all the languages rather than picking one at random +SVGReader.prototype.helloCycle = function() { + timer = 0; + var fileArray = fs.readdirSync("./hello"); + objRef = this; + var fileNum = 0; + + //Used to access the erase functions + var drawing = new draw.Draw({ + baseWidth: objRef.baseWidth, + baseHeight: objRef.baseHeight, + drawHeight: objRef.drawHeight + }); + + //Function to actually write hello + sayHello = function() { + var fileChoice = "./hello/" + fileArray[fileNum]; + + if (fileChoice.charAt(15) === 'T') + connect = true; + else + connect = false; + + drawing.erase(function() { + setTimeout(function() { objRef.drawSVG(fileChoice, connect) }, 10000); + }); + + fileNum++; + + if (fileNum >= fileArray.length) //Once the end of the list is reached, stop writing + clearInterval(helloTimer); + } + + sayHello(); + helloTimer = setInterval(function() { sayHello() }, 60000); + } @@ -772,14 +875,6 @@ var halfway; var currentPoint; var penHeight; -var transformX; -var transformY; -var objRef; -var connected; -var startTime; -var endTime; -var difference; -var clockTimer; -var firstMove; +var transformX, transformY, objRef, connected, startTime, endTime, difference, clockTimer, firstMove, lastNum1, lastNum2, connect, baseWidth, baseHeight; module.exports.SVGReader = SVGReader; From 603f32becb5d67e9c97215da3244ae1cf612ad20 Mon Sep 17 00:00:00 2001 From: jackskalitzky Date: Thu, 9 Jul 2015 16:41:19 -0500 Subject: [PATCH 26/43] Create helloEnT.svg --- software/src/hello/helloEnT.svg | 86 +++++++++++++++++++++++++++++++++ 1 file changed, 86 insertions(+) create mode 100644 software/src/hello/helloEnT.svg diff --git a/software/src/hello/helloEnT.svg b/software/src/hello/helloEnT.svg new file mode 100644 index 0000000..c5fa472 --- /dev/null +++ b/software/src/hello/helloEnT.svg @@ -0,0 +1,86 @@ + + + + + + + + + + + + image/svg+xml + + + + + + + + + + + + + From 7eb1a71f6bfbc5b9b5baa4804f7f1478dfe4881a Mon Sep 17 00:00:00 2001 From: jackskalitzky Date: Thu, 9 Jul 2015 16:41:55 -0500 Subject: [PATCH 27/43] Create helloChF.svg --- software/src/hello/helloChF.svg | 149 ++++++++++++++++++++++++++++++++ 1 file changed, 149 insertions(+) create mode 100644 software/src/hello/helloChF.svg diff --git a/software/src/hello/helloChF.svg b/software/src/hello/helloChF.svg new file mode 100644 index 0000000..56c4a7b --- /dev/null +++ b/software/src/hello/helloChF.svg @@ -0,0 +1,149 @@ + + + + + + + + + + image/svg+xml + + + + + + + + + + + + + + + + + + + + + + + + + From a57f7bf6ee65228a3a21cb34cfc6a633e04ed82f Mon Sep 17 00:00:00 2001 From: jackskalitzky Date: Thu, 9 Jul 2015 16:42:14 -0500 Subject: [PATCH 28/43] Create helloFrF.svg --- software/src/hello/helloFrF.svg | 101 ++++++++++++++++++++++++++++++++ 1 file changed, 101 insertions(+) create mode 100644 software/src/hello/helloFrF.svg diff --git a/software/src/hello/helloFrF.svg b/software/src/hello/helloFrF.svg new file mode 100644 index 0000000..e6997af --- /dev/null +++ b/software/src/hello/helloFrF.svg @@ -0,0 +1,101 @@ + + + + + + + + + + image/svg+xml + + + + + + + + + + + + + + + + + From d3f18f1cc97054123ca9e839e46e93913b0d8f12 Mon Sep 17 00:00:00 2001 From: jackskalitzky Date: Thu, 9 Jul 2015 16:42:32 -0500 Subject: [PATCH 29/43] Create helloItF.svg --- software/src/hello/helloItF.svg | 83 +++++++++++++++++++++++++++++++++ 1 file changed, 83 insertions(+) create mode 100644 software/src/hello/helloItF.svg diff --git a/software/src/hello/helloItF.svg b/software/src/hello/helloItF.svg new file mode 100644 index 0000000..c4c939a --- /dev/null +++ b/software/src/hello/helloItF.svg @@ -0,0 +1,83 @@ + + + + + + + + + + image/svg+xml + + + + + + + + + + + + + + From c2c886b175e2130d8fa86a9b16d06ec34aa068b9 Mon Sep 17 00:00:00 2001 From: jackskalitzky Date: Thu, 9 Jul 2015 16:42:49 -0500 Subject: [PATCH 30/43] Create helloJpF.svg --- software/src/hello/helloJpF.svg | 97 +++++++++++++++++++++++++++++++++ 1 file changed, 97 insertions(+) create mode 100644 software/src/hello/helloJpF.svg diff --git a/software/src/hello/helloJpF.svg b/software/src/hello/helloJpF.svg new file mode 100644 index 0000000..bf9232e --- /dev/null +++ b/software/src/hello/helloJpF.svg @@ -0,0 +1,97 @@ + + + + + + + + + + image/svg+xml + + + + + + + + + + + + + + + + + + + From cd633b6af57dcdf10f4930269b26a0223625294b Mon Sep 17 00:00:00 2001 From: jackskalitzky Date: Thu, 9 Jul 2015 16:43:11 -0500 Subject: [PATCH 31/43] Create helloSpT.svg --- software/src/hello/helloSpT.svg | 80 +++++++++++++++++++++++++++++++++ 1 file changed, 80 insertions(+) create mode 100644 software/src/hello/helloSpT.svg diff --git a/software/src/hello/helloSpT.svg b/software/src/hello/helloSpT.svg new file mode 100644 index 0000000..2ab141a --- /dev/null +++ b/software/src/hello/helloSpT.svg @@ -0,0 +1,80 @@ + + + + + + + + + + image/svg+xml + + + + + + + + + + + + + + From 5020ac871a51596b1452a30e005f9a39229a21f6 Mon Sep 17 00:00:00 2001 From: jackskalitzky Date: Fri, 10 Jul 2015 15:21:17 -0500 Subject: [PATCH 32/43] Updated characters --- software/src/hello/helloJpF.svg | 105 ++++++++++++++++++++------------ 1 file changed, 67 insertions(+), 38 deletions(-) diff --git a/software/src/hello/helloJpF.svg b/software/src/hello/helloJpF.svg index bf9232e..aff0c36 100644 --- a/software/src/hello/helloJpF.svg +++ b/software/src/hello/helloJpF.svg @@ -15,7 +15,7 @@ id="svg2" version="1.1" inkscape:version="0.91 r13725" - sodipodi:docname="helloJp.svg"> + sodipodi:docname="helloJpF.svg"> image/svg+xml - + + id="g3688"> + inkscape:connector-curvature="0" + id="path3348" + d="M 6.3874748,216.43666 15.640573,216.11786 24.893671,215.79906 34.146769,215.48027 43.399868,215.16147" + style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1.51638877px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" + sodipodi:nodetypes="ccccc" /> + sodipodi:nodetypes="csssc" + inkscape:connector-curvature="0" + id="path3350" + d="M 6.3874748,264.89415 C 4.8976864,271.60047 4.4375965,277.14191 4.7545596,281.7114 5.2079657,288.24794 7.2514121,292.79566 10.145382,295.91943 14.13605,300.22699 19.744016,301.82686 25.030167,302.20013 32.789021,302.74801 39.854576,300.65338 40.095191,300.59965" + style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1.51638877px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" /> + inkscape:connector-curvature="0" + id="path3352" + d="M 54.002119,301.71087 66.121143,276.60995 78.240165,251.50903 90.359189,226.40813 102.47821,201.30723" + style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1.9545244px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" + sodipodi:nodetypes="ccccc" /> + sodipodi:nodetypes="cssssc" + inkscape:connector-curvature="0" + id="path3354" + d="M 73.833247,258.49918 C 78.17793,255.6352 80.250185,257.29523 81.106464,261.32782 82.112339,266.06494 81.440251,274.07609 80.802712,281.87385 79.595601,296.63802 74.879826,308.21556 85.54439,297.77851 88.550128,294.8369 88.856531,292.98127 93.9889,285.62748 96.44067,282.11452 95.532243,275.61599 98.557074,270.91745" + style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1.9545244px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" /> + sodipodi:nodetypes="csssc" + inkscape:connector-curvature="0" + id="path3356" + d="M 128.59457,199.25369 C 123.94164,208.50799 120.89718,217.46203 119.10457,226.16264 117.42318,234.32338 116.84311,242.26117 117.07007,250.01467 117.32337,258.66809 118.58192,267.09195 120.4366,275.33999 122.42044,284.16238 125.08634,292.7836 127.93364,301.26942" + style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1.51638877px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" /> + inkscape:connector-curvature="0" + id="path3358" + d="M 148.53366,211.94131 164.04859,211.35436 182.24138,210.66611" + style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1.51638877px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" + sodipodi:nodetypes="ccc" /> + sodipodi:nodetypes="csssc" + inkscape:connector-curvature="0" + id="path3360" + d="M 147.88901,262.71421 C 145.38854,270.45217 146.40867,276.074 149.37648,280.15264 151.87521,283.58661 155.75459,285.92666 160.07586,287.51471 165.15463,289.38113 170.84379,290.20881 175.61931,290.55284 181.56254,290.98099 186.09069,290.66007 186.2661,290.66007" + style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:2.11195755px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" /> + sodipodi:nodetypes="cccccssssc" + inkscape:connector-curvature="0" + id="path3362" + d="M 206.26059,196.66974 203.95081,212.59907 201.64104,228.5284 199.33127,244.45774 197.02148,260.38706 C 201.56646,252.86442 208.86149,250.44861 215.62018,251.30464 221.72598,252.07796 227.39405,255.52153 230.20142,260.28241 232.77162,264.64108 232.94405,270.10391 228.85935,275.63268 227.17374,277.91421 224.50329,279.86747 221.49701,282.43801 215.31355,287.72522 202.84738,286.57423 199.28163,287.42313" + style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1.63610315px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" /> + inkscape:connector-curvature="0" + id="path3364" + d="M 190.09215,211.96192 210.11077,211.34833 231.66816,210.68757" + style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1.63610315px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" + sodipodi:nodetypes="ccc" /> + style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1.51638877px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" + d="M 256.47054,196.65909 C 251.81761,205.91339 248.77315,214.86743 246.98054,223.56804 245.29915,231.72878 244.71908,239.66657 244.94604,247.42007 245.19934,256.07349 246.45789,264.49735 248.31257,272.74539 250.29641,281.56778 252.96231,290.189 255.80961,298.67482" + id="path3356-2" + inkscape:connector-curvature="0" + sodipodi:nodetypes="csssc" /> + sodipodi:nodetypes="cssscsssssc" + inkscape:connector-curvature="0" + id="path3383" + d="M 297.25031,201.89704 C 297.46145,225.77758 297.61982,243.59472 297.73859,256.8876 297.85736,270.18048 297.93653,278.94912 297.98933,284.73269 298.04212,290.51627 298.06851,293.31476 298.08171,294.66736 298.0949,296.01999 298.0949,295.9267 298.0949,295.9267 297.07975,295.61569 293.78564,297.01569 292.86495,296.7005 289.47802,295.54107 285.53812,294.85158 283.95814,296.77035 276.07997,306.33779 267.67872,283.07085 267.99192,279.92398 268.32495,276.5779 279.28841,270.65047 284.50578,268.93008 288.75446,267.5291 296.37804,274.77635 301.40394,275.31583 307.868,276.00969 314.0409,278.62688 317.52015,283.98641" + style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1.85448825px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" /> + From afb4fdd37debf4da8a33f094002e9d7eb1d4ac53 Mon Sep 17 00:00:00 2001 From: jackskalitzky Date: Fri, 10 Jul 2015 15:22:16 -0500 Subject: [PATCH 33/43] Created helloRuF --- software/src/hello/helloRuF.svg | 90 +++++++++++++++++++++++++++++++++ 1 file changed, 90 insertions(+) create mode 100644 software/src/hello/helloRuF.svg diff --git a/software/src/hello/helloRuF.svg b/software/src/hello/helloRuF.svg new file mode 100644 index 0000000..3205637 --- /dev/null +++ b/software/src/hello/helloRuF.svg @@ -0,0 +1,90 @@ + + + + + + + + + + image/svg+xml + + + + + + + + + + + + + + From 6b1a1792cd04edbe108d54b7f02581c1186dd1e1 Mon Sep 17 00:00:00 2001 From: jackskalitzky Date: Fri, 10 Jul 2015 15:24:08 -0500 Subject: [PATCH 34/43] Fixed translation issue (heh) Because there was an issue where the translation transformation wasn't removed properly and I fixed that but the file is also a Spanish translation so it's a joke --- software/src/hello/helloSpT.svg | 58 +++++++++++++++------------------ 1 file changed, 26 insertions(+), 32 deletions(-) diff --git a/software/src/hello/helloSpT.svg b/software/src/hello/helloSpT.svg index 2ab141a..ae19b57 100644 --- a/software/src/hello/helloSpT.svg +++ b/software/src/hello/helloSpT.svg @@ -15,7 +15,7 @@ id="svg3474" version="1.1" inkscape:version="0.91 r13725" - sodipodi:docname="helloSp.svg"> + sodipodi:docname="helloSpT.svg"> image/svg+xml - + - - - - - - + id="g3346"> + + + + From 17a3b2834aca1fd7165e92e954c40ce83f1be226 Mon Sep 17 00:00:00 2001 From: jackskalitzky Date: Fri, 10 Jul 2015 15:24:34 -0500 Subject: [PATCH 35/43] Fixed translation issue --- software/src/hello/helloItF.svg | 23 ++++++++++------------- 1 file changed, 10 insertions(+), 13 deletions(-) diff --git a/software/src/hello/helloItF.svg b/software/src/hello/helloItF.svg index c4c939a..b7dac35 100644 --- a/software/src/hello/helloItF.svg +++ b/software/src/hello/helloItF.svg @@ -24,10 +24,10 @@ inkscape:pageopacity="0.0" inkscape:pageshadow="2" inkscape:zoom="1.6517427" - inkscape:cx="141.73228" - inkscape:cy="168.30709" + inkscape:cx="35.783575" + inkscape:cy="167.70167" inkscape:document-units="mm" - inkscape:current-layer="layer1" + inkscape:current-layer="svg4274" showgrid="false" inkscape:window-width="1308" inkscape:window-height="749" @@ -44,37 +44,34 @@ image/svg+xml - + + id="g3369"> + id="g3358"> From 3ccdfd20b8630e3294a45b96080a2b55c8a40f5b Mon Sep 17 00:00:00 2001 From: Jack Skalitzky Date: Thu, 16 Jul 2015 22:32:20 -0500 Subject: [PATCH 36/43] Cleanup Bot.js pulls in motion.js; cleaned up SVGReader and Draw; fixed some hello files; added some config values --- software/config.js | 40 ++++- software/src/SVGReader.js | 212 +++++++++++++++-------- software/src/bot.js | 112 +++++++++--- software/src/draw.js | 132 ++++++++------ software/src/example/cursiveTutorial.svg | 85 +++++++++ software/src/example/textTutorial.svg | 74 ++++++++ software/src/example/tutorial.svg | 77 ++++++++ software/src/hello/helloChF.svg | 207 +++++++++++----------- software/src/hello/helloRuF.svg | 30 ++-- software/src/kinematics.js | 2 +- software/src/motion.js | 90 ++++++++++ software/src/shrug.svg | 132 ++++++++++++++ software/src/tutorial.svg | 60 +++++++ 13 files changed, 979 insertions(+), 274 deletions(-) create mode 100644 software/src/example/cursiveTutorial.svg create mode 100644 software/src/example/textTutorial.svg create mode 100644 software/src/example/tutorial.svg create mode 100644 software/src/motion.js create mode 100644 software/src/shrug.svg create mode 100644 software/src/tutorial.svg diff --git a/software/config.js b/software/config.js index 3f295f0..fc2a48c 100644 --- a/software/config.js +++ b/software/config.js @@ -1,27 +1,55 @@ var config = {} //Side of end effector +//~~Do not touch~~ config.e = 34.64101615137754; // Math.sqrt(3) * 10 * 2 //Side of top triangle +//~~Do not touch~~ config.f = 110.85125168440814; // Math.sqrt(3) * 32 * 2 //Length of parallelogram joint +//~~Do not touch~~ config.re = 153.5; // 145 + 8.5 //Length of upper joint +//~~Do not touch~~ config.rf = 52.690131903421914; // Math.sqrt(52**2 + 8.5**2) -//Calibrated servo values -config.servo1 = {in_min: 0, in_max: 90, out_min: 11, out_max: 92}; -config.servo2 = {in_min: 0, in_max: 90, out_min: 9, out_max: 88}; -config.servo3 = {in_min: 0, in_max: 90, out_min: 10, out_max: 90.5}; +//Input ranges for servos +//~~Do not touch~~ +config.servo1 = {in_min: 0, in_max: 90}; +config.servo2 = {in_min: 0, in_max: 90}; +config.servo3 = {in_min: 0, in_max: 90}; + +//Default output ranges for servos +//CHANGE THESE +config.servo1.out_min = 12; +config.servo1.out_max = 93; +config.servo2.out_min = 8; +config.servo2.out_max = 90; +config.servo3.out_min = 14; +config.servo3.out_max = 96; //Dimensions of the base plate config.baseHeight = 95; config.baseWidth = 80; -//Default Z-level of the pen +//Default Z-Level of the pen config.penHeight = -140; -module.exports = config; +//Default drawing height of the pen +config.drawHeight = -152.75; + +//Delay for commands in SVGReader +//Note that some commands will take longer than this +//Default value is 150 +config.delay = 200; + +//The default easing type to be used +//When no easing is specified, this is the type that will be used +//"none" means that if no easing is specified, do not ease +//For a list of possible easing types, look in motion.js +config.defaultEaseType = "linear"; + +module.exports = config; \ No newline at end of file diff --git a/software/src/SVGReader.js b/software/src/SVGReader.js index e47be74..9060605 100644 --- a/software/src/SVGReader.js +++ b/software/src/SVGReader.js @@ -10,9 +10,11 @@ var draw = require("./draw"); var parseString = require('xml2js').parseString; function SVGReader(args) { - this.baseWidth = 0; - this.baseHeight = 0; - this.drawHeight = 0; + this.baseWidth = 80; + this.baseHeight = 95; + this.drawHeight = -140; + this.delay = 150; + this.defaultEaseType = "linear"; if (args) { var keys = Object.keys(args) @@ -21,20 +23,16 @@ function SVGReader(args) { }, this) } objRef = this; + defaultEaseType = this.defaultEaseType; } -//The time, in milliseconds, for each command to execute -//Note: Some commands, such as Move, can take more than this amount of time to execute -//To-do: Pull this from a config file? -var delay = 150; - //Draws from an SVG image specified by filepath //> Usage: //> drawSVG("C:/Projects/Tapsterbot/software/src/drawing.svg"); //connect is a special flag that indicates that each path should be drawn connected to each other //It is really only used for drawing in cursive and does not need to be specified otherwise SVGReader.prototype.drawSVG = function(filePath, connect) { - timer = 0; + resetTimer(); var parsed; if (connect) @@ -67,10 +65,12 @@ SVGReader.prototype.drawSVG = function(filePath, connect) { width = svgDimensions.width; height = svgDimensions.height; + //Check for translation and account for it + //Commented out because going to stop supporting transformations + /* transformX = 0; transformY = 0; - //Check for translation and account for it if (objArr.svg.g[0].$ && objArr.svg.g[0].$.transform) { //Done in multiple checks to avoid errors being thrown var transString = objArr.svg.g[0].$.transform; var subX = transString.indexOf("("); @@ -78,6 +78,7 @@ SVGReader.prototype.drawSVG = function(filePath, connect) { var subY = transString.indexOf(","); transformY = parseInt(transString.substring(subY + 1)); } + */ var phoneWidth = this.baseWidth; var phoneHeight = this.baseHeight; @@ -108,8 +109,8 @@ SVGReader.prototype.drawSVG = function(filePath, connect) { drawImage(pathArray); } - doSetTimeout(0, 49, -130, delay); - setTimeout(function() { timer = 0 }, timer + 5); + doSetTimeout(0, 48, -130, delay); + //setTimeout(function() { resetTimer() }, timer + 5); } drawImage = function(pathArray) { @@ -131,18 +132,27 @@ drawImage = function(pathArray) { //Move from one point to (x, y) move = function(x, y) { + var ptArray = []; if (!connected) { //If the paths should not be connected, lift up the pen and move over so that a line is not drawn - doSetTimeout(mapX(currentPoint.x), mapY(currentPoint.y), penHeight + 10, delay); - doSetTimeout(mapX(x), mapY(y), penHeight + 10, delay); - doSetTimeout(mapX(x), mapY(y), penHeight, delay); + doSetTimeout(mapX(currentPoint.x), mapY(currentPoint.y), penHeight + 10, delay, "none"); + doSetTimeout(mapX(x), mapY(y), penHeight + 10, delay, "none"); + doSetTimeout(mapX(x), mapY(y), penHeight, delay, "none"); + //svg.drawSVG("C:/Projects/Tapsterbot/software/src/hello/helloChF.svg") + //ptArray.push({x:mapX(currentPoint.x), y:mapY(currentPoint.y), z:penHeight + 10}); + //ptArray.push({x:mapX(x), y:mapY(y), z:penHeight + 10}); + //ptArray.push({x:mapX(x), y:mapY(y), z:penHeight}); } else if (connected && !firstMove) { //If the paths should be connected and a move has not been made, lift up the pen and move to the first point - doSetTimeout(mapX(currentPoint.x), mapY(currentPoint.y), penHeight + 10, delay); - doSetTimeout(mapX(x), mapY(y), penHeight + 10, delay); - doSetTimeout(mapX(x), mapY(y), penHeight, delay); + doSetTimeout(mapX(currentPoint.x), mapY(currentPoint.y), penHeight + 10, delay, "none"); + doSetTimeout(mapX(x), mapY(y), penHeight + 10, delay, "none"); + doSetTimeout(mapX(x), mapY(y), penHeight, delay, "none"); + //ptArray.push({x:mapX(currentPoint.x), y:mapY(currentPoint.y), z:penHeight + 10}); + //ptArray.push({x:mapX(x), y:mapY(y), z:penHeight + 10}); + //ptArray.push({x:mapX(x), y:mapY(y), z:penHeight}); } else //If the paths should be connected and a move has been made, just draw a line between the two paths doSetTimeout(mapX(x), mapY(y), penHeight, delay); + //ptArray.push({x:mapX(x), y:mapY(y), z:penHeight}); currentPoint = {x:x, y:y}; //Update the current point (done every time an SVG command is called) @@ -151,6 +161,8 @@ move = function(x, y) { if (!firstPoint) //Keeps track of the first point, for use with the Z/z command firstPoint = {x:currentPoint.x, y:currentPoint.y}; //Since the first command of a path is always to Move, this check only occurs here + + //return ptArray; } //Move from one point to that that point + x, y @@ -158,13 +170,17 @@ relMove = function(x, y) { x = currentPoint.x + x; y = currentPoint.y + y; - move(x, y); + //return move(x, y); + move(x, y); } //Draw a line from one point to (x, y) line = function(x, y) { - doSetTimeout(mapX(x), mapY(y), penHeight, delay); + var ptArray = []; + doSetTimeout(mapX(x), mapY(y), penHeight, delay, "linear"); + //ptArray.push({x:mapX(x), y:mapY(y), z:penHeight}); currentPoint = {x:x, y:y}; + } //Draw a line from one point to that point + x, y @@ -172,6 +188,7 @@ relLine = function(x, y) { x = currentPoint.x + x; y = currentPoint.y + y; + //return line(x, y); line(x, y); } @@ -187,11 +204,11 @@ cubicCurve = function(x1, y1, x2, y2, x, y) { var ptArray = new Array(); for (var i = 0; i <= t; i++) { var newI = i/t; //Converts i to a decimal, to satisfy 0 <= i <= 1 - var ptX = (Math.pow((1-newI), 3) * currentPoint.x) + (3 * Math.pow((1-newI), 2) * newI * x1) //Formula from Wikipedia page for Bezier curves + var ptX = (Math.pow((1-newI), 3) * currentPoint.x) + (3 * Math.pow((1-newI), 2) * newI * x1) //From https://en.wikipedia.org/wiki/B%C3%A9zier_curve#Cubic_B.C3.A9zier_curves + (3 * (1-newI) * Math.pow(newI, 2) * x2) + (Math.pow(newI, 3) * x); var ptY = (Math.pow((1-newI), 3) * currentPoint.y) + (3 * Math.pow((1-newI), 2) * newI * y1) + (3 * (1-newI) * Math.pow(newI, 2) * y2) + (Math.pow(newI, 3) * y); - var newPt = {x:ptX, y:ptY}; + var newPt = {x:ptX, y:ptY, z:penHeight}; ptArray.push(newPt); //Populates the array with points } currentPoint = {x:ptArray[t].x, y:ptArray[t].y}; @@ -199,9 +216,10 @@ cubicCurve = function(x1, y1, x2, y2, x, y) { } var curvePts = new Array(); - curvePts = b(x1, y1, x2, y2, x, y, 5); //5 is an arbitrarily-chosen value. It creates a smooth-looking curve without calculating too many points + curvePts = b(x1, y1, x2, y2, x, y, 5); //Arbitrarily-chosen value. It creates a smooth-looking curve without calculating too many points for (var i = 0;i < curvePts.length; i++) - doSetTimeout(mapX(curvePts[i].x), mapY(curvePts[i].y), penHeight, delay / curvePts.length); + doSetTimeout(mapX(curvePts[i].x), mapY(curvePts[i].y), penHeight, delay*2 / curvePts.length, "none"); //The cubic curve command takes 2*delay ms to complete so that an accurate curve is created + //return b(x1, y1, x2, y2, x, y, 8); } //Draws a relative cubic Bezier curve @@ -209,6 +227,7 @@ relCubicCurve = function(x1, y1, x2, y2, x, y) { var tempX = currentPoint.x; var tempY = currentPoint.y; + //return cubicCurve(tempX + x1, tempY + y1, tempX + x2, tempY + y2, tempX + x, tempY + y); cubicCurve(tempX + x1, tempY + y1, tempX + x2, tempY + y2, tempX + x, tempY + y); } @@ -222,9 +241,9 @@ quadraticCurve = function(x1, y1, x, y) { var ptArray = new Array(); for (var i = 0; i <= t; i++) { var newI = i/t; //Converts i to a decimal, to satisfy 0 <= i <= 1 - var ptX = Math.pow((1-newI), 2)*currentPoint.x + (2 * (1-newI) * newI * x1) + (Math.pow(newI, 2) * x); //From Wikipedia page for Bezier curves + var ptX = Math.pow((1-newI), 2)*currentPoint.x + (2 * (1-newI) * newI * x1) + (Math.pow(newI, 2) * x); //From https://en.wikipedia.org/wiki/B%C3%A9zier_curve#Quadratic_B.C3.A9zier_curves var ptY = Math.pow((1-newI), 2)*currentPoint.y + (2 * (1-newI) * newI * y1) + (Math.pow(newI, 2) * y); - var newPt = {x:ptX, y:ptY}; + var newPt = {x:ptX, y:ptY, z:penHeight}; ptArray.push(newPt); } currentPoint = {x:ptArray[t].x, y:ptArray[t].y}; @@ -234,13 +253,15 @@ quadraticCurve = function(x1, y1, x, y) { var curvePts = new Array(); curvePts = q(x1, y1, x, y, 5); for (var i = 0; i < curvePts.length; i++) - doSetTimeout(mapX(curvePts[i].x), mapY(curvePts[i].y), penHeight, delay / curvePts.length); + doSetTimeout(mapX(curvePts[i].x), mapY(curvePts[i].y), penHeight, delay*2 / curvePts.length, "none"); + //return q(x1, y1, x, y, 8); } //Draws a relative quadratic Bezier curve relQuadraticCurve = function(x1, y1, x, y) { var tempX = currentPoint.x; var tempY = currentPoint.y; + //return quadraticCurve(tempX + x1, tempY + y1, tempX + x, tempY + y); quadraticCurve(tempX + x1, tempY + y1, tempX + x, tempY + y); } @@ -260,7 +281,7 @@ arc = function(rx, ry, rotation, largeArc, sweep, x, y) { for (var i = 0; i <= t; i++) { var newI = i / t; var angle = startAngle + sweepAngle * newI; - var newPt = {x: cx + rx * cos(angle), y: cy + ry * sin(angle)}; + var newPt = {x: cx + rx * cos(angle), y: cy + ry * sin(angle), z:penHeight}; ptArray.push(newPt); } @@ -335,11 +356,13 @@ arc = function(rx, ry, rotation, largeArc, sweep, x, y) { sweepAngle %= 360; - var ptArray = a(rx, ry, largeArc, sweep, x, y, 7); + var ptArray = a(rx, ry, largeArc, sweep, x, y, 5); for (var i = 0; i < ptArray.length; i++) { - doSetTimeout(mapX(ptArray[i].x), mapY(ptArray[i].y), penHeight, delay / ptArray.length); + doSetTimeout(mapX(ptArray[i].x), mapY(ptArray[i].y), penHeight, delay*2 / ptArray.length, "none"); } + + //return a(rx, ry, largeArc, sweep, x, y, 10); } //Function for drawing a relative elliptical arc @@ -347,15 +370,10 @@ relArc = function(rx, ry, rotation, largeArc, sweep, x, y) { x = currentPoint.x + x; y = currentPoint.y + y; + //return arc(rx, ry, rotation, largeArc, sweep, x, y); arc(rx, ry, rotation, largeArc, sweep, x, y); } -//A separate setTimeout method so that delays work properly -doSetTimeout = function(x, y, z, timeDelay) { - setTimeout(function() { go(x, y, z) }, timer); - timer = timer + timeDelay; -}; - //A function for setting the penHeight from the command line setPenHeight = function(penHeight) { penHeight = penHeight; @@ -378,19 +396,13 @@ reflect = function(x, y, x1, y1) { //Convert points pixel coordinates to Tapster coordinates //Done in two methods for ease of use mapX = function(x) { - if (transformX) - var newX = x + transformX; - else - var newX = x; + var newX = x; newX = (newX - halfway.x) / widthRatio; //The center of the canvas corresponds to (0, 0) on the Tapster return newX; }; mapY = function(y) { - if (transformY) - var newY = y + transformY; - else - var newY = y; + var newY = y; newY = (halfway.y - newY) / heightRatio; return newY; } @@ -406,6 +418,7 @@ cos = function(degree) { } //Function for converting Inkscape dimensions into Tapster-friendly pixels +//Should switch to switch statements dimensionConversion = function(width, height) { width = String(width); if (width.search("mm") != -1) { @@ -436,10 +449,15 @@ dimensionConversion = function(width, height) { } //Goes through the list of commands an generated by the SVG-Path-Parser and calls the corresponding functions +//Should switch to switch statements SVGReader.prototype.interpretCommands = function(commands) { + delay = this.delay; for (var i = 0; i < commands.length; i++) { var cmdCode = commands[i].code; if (cmdCode == 'M') { + //var temp = move(commands[i].x, commands[i].y); + //for (var i = 0; i < temp.length; i++) + //doSetTimeout(temp[i].x, temp[i].y, temp[i].z, delay); move(commands[i].x, commands[i].y); } @@ -548,17 +566,51 @@ SVGReader.prototype.interpretCommands = function(commands) { } } +SVGReader.prototype.loadFont = function(filePath) { + try { + parseString(fs.readFileSync(filePath, "utf8"), function(err, result) { + parsed = JSON.stringify(result, null, 1); + loaded = true; + }); + } catch (e) { + if (e.code === "ENOENT") + console.log("File not found."); + else + throw e; + + return; //If the file is not found stop execution + } + + //Parse the JSON string into an array + fontFile = JSON.parse(parsed); + + //Extract width and height data from the drawing + var svgDimensions = dimensionConversion(fontFile.svg.$.width, fontFile.svg.$.height); + width = svgDimensions.width; + height = svgDimensions.height; + + fontFile = fontFile.svg.g[0].path; + //svg.loadFont("C:/Projects/Tapsterbot/software/src/fontFile.svg") +} + //Creates a working clock //To-do: Add support for user-specified fonts -SVGReader.prototype.clock = function() { +SVGReader.prototype.clock = function(filePath) { var dimensions = dimensionConversion("80mm", "95mm"); //Since no dimensions are specified, assume the default //To-do: Pull this from a config file width = dimensions.width; height = dimensions.height; + if (filePath) + this.loadFont(filePath); + else { + console.log("Font not specified. Using default font."); + loaded = false; + } + objRef = this; - timer = 0; + resetTimer(); connected = false; //Used to access the erase functions @@ -578,20 +630,28 @@ SVGReader.prototype.clock = function() { halfway = {x:width / 2, y:height / 2}; currentPoint = {x:halfway.x, y:halfway.y}; //Start at the center of the canvas, which corresponds to (0,0) on the Tapster - //Currently hardcoded path data - //To-do: Allow users to specify fonts - var zero = "M 40.890286,241.44557 30.687127,245.57254 23.885021,257.95342 20.483968,278.58823 20.483968,290.96912 23.885021,311.60393 30.687127,323.98482 40.890286,328.11178 47.692392,328.11178 57.895551,323.98482 64.697657,311.60393 68.09871,290.96912 68.09871,278.58823 64.697657,257.95342 57.895551,245.57254 47.692392,241.44557 40.890286,241.44557"; - var one = "M 50.289453,243.07635 50.289453,329.52761"; - var two = "M 27.618803,262.08031 27.618803,257.95271 30.945521,249.69749 34.27224,245.56989 40.925676,241.44228 54.23255,241.44228 60.885986,245.56989 64.212704,249.69749 67.539423,257.95271 67.539423,266.20792 64.212704,274.46313 57.559268,286.84595 24.292085,328.12202 70.866141,328.12202"; - var three = "M 27.042715,241.44643 64.666988,241.44643 44.144657,274.4608 54.405822,274.4608 61.2466,278.58759 64.666988,282.71439 68.087377,295.09478 68.087377,303.34837 64.666988,315.72875 57.826211,323.98234 47.565046,328.10914 37.30388,328.10914 27.042715,323.98234 23.622326,319.85555 20.201938,311.60196"; - var four = "M 54.845427,241.51709 22.803997,300.19076 70.866142,300.19076 M 54.845427,241.51709 54.845427,329.5276"; - var five = "M 61.700462,246.77512 26.654546,246.77512 23.149954,281.56734 26.654546,277.70154 37.168321,273.83574 47.682096,273.83574 58.19587,277.70154 65.205054,285.43315 68.709645,297.03055 68.709645,304.76216 65.205054,316.35956 58.19587,324.09117 47.682096,327.95697 37.168321,327.95697 26.654546,324.09117 23.149954,320.22537 19.645363,312.49376"; - var six = "M 64.294901,253.7887 60.65789,245.53396 49.746856,241.40659 42.472833,241.40659 31.561799,245.53396 24.287776,257.91607 20.650765,278.55291 20.650765,299.18976 24.287776,315.69923 31.561799,323.95397 42.472833,328.08134 46.109844,328.08134 57.020878,323.95397 64.294901,315.69923 67.931912,303.31713 67.931912,299.18976 64.294901,286.80765 57.020878,278.55291 46.109844,274.42554 42.472833,274.42554 31.561799,278.55291 24.287776,286.80765 20.650765,299.18976"; - var seven = "M 68.262659,241.73033 32.158284,328.90301 M 17.716535,241.73033 68.262659,241.73033"; - var eight = "M 37.489233,241.44557 27.286074,245.57254 23.885021,253.82646 23.885021,262.08039 27.286074,270.33431 34.08818,274.46127 47.692392,278.58823 57.895551,282.7152 64.697657,290.96912 68.09871,299.22304 68.09871,311.60393 64.697657,319.85785 61.296604,323.98482 51.093445,328.11178 37.489233,328.11178 27.286074,323.98482 23.885021,319.85785 20.483968,311.60393 20.483968,299.22304 23.885021,290.96912 30.687127,282.7152 40.890286,278.58823 54.494498,274.46127 61.296604,270.33431 64.697657,262.08039 64.697657,253.82646 61.296604,245.57254 51.093445,241.44557 37.489233,241.44557"; - var nine = "M 67.931912,270.29818 64.2949,282.68028 57.020878,290.93502 46.109844,295.06239 42.472833,295.06239 31.561799,290.93502 24.287776,282.68028 20.650765,270.29818 20.650765,266.17081 24.287776,253.7887 31.561799,245.53396 42.472833,241.40659 46.109844,241.40659 57.020878,245.53396 64.2949,253.7887 67.931912,270.29818 67.931912,290.93502 64.2949,311.57186 57.020878,323.95397 46.109844,328.08134 38.835821,328.08134 27.924787,323.95397 24.287776,315.69923"; - - pathData = [zero, one, two, three, four, five, six, seven, eight, nine]; + if (connected) { + for (var i = 0; i < fontFile.length; i++) { + pathData[fontFile[i].$.id] == fontFile[i].$.d; + } + } + + else { + //Currently hardcoded path data + //To-do: Allow users to specify fonts + var zero = "M 40.890286,241.44557 30.687127,245.57254 23.885021,257.95342 20.483968,278.58823 20.483968,290.96912 23.885021,311.60393 30.687127,323.98482 40.890286,328.11178 47.692392,328.11178 57.895551,323.98482 64.697657,311.60393 68.09871,290.96912 68.09871,278.58823 64.697657,257.95342 57.895551,245.57254 47.692392,241.44557 40.890286,241.44557"; + var one = "M 50.289453,243.07635 50.289453,329.52761"; + var two = "M 27.618803,262.08031 27.618803,257.95271 30.945521,249.69749 34.27224,245.56989 40.925676,241.44228 54.23255,241.44228 60.885986,245.56989 64.212704,249.69749 67.539423,257.95271 67.539423,266.20792 64.212704,274.46313 57.559268,286.84595 24.292085,328.12202 70.866141,328.12202"; + var three = "M 33.809127,248.8729 C 45.779077,241.87747 46.887418,243.5556 53.186196,243.05913 61.663816,242.39092 65.286579,246.55715 64.812967,252.72615 64.140416,261.48642 55.207358,274.28536 40.185777,283.01419 L 52.623884,281.94331 59.295192,285.47169 62.630846,289.00007 65.9665,299.58523 65.9665,306.64193 62.630846,317.22713 55.959538,324.28383 45.952576,327.81223 35.945614,327.81223 25.938652,324.28383 22.602998,320.75553 19.267344,313.69873"; + var four = "M 54.845427,241.51709 22.803997,300.19076 70.866142,300.19076 M 54.845427,241.51709 54.845427,329.5276"; + var five = "M 61.700462,246.77512 26.654546,246.77512 23.149954,281.56734 26.654546,277.70154 37.168321,273.83574 47.682096,273.83574 58.19587,277.70154 65.205054,285.43315 68.709645,297.03055 68.709645,304.76216 65.205054,316.35956 58.19587,324.09117 47.682096,327.95697 37.168321,327.95697 26.654546,324.09117 23.149954,320.22537 19.645363,312.49376"; + var six = "M 64.294901,253.7887 60.65789,245.53396 49.746856,241.40659 42.472833,241.40659 31.561799,245.53396 24.287776,257.91607 20.650765,278.55291 20.650765,299.18976 24.287776,315.69923 31.561799,323.95397 42.472833,328.08134 46.109844,328.08134 57.020878,323.95397 64.294901,315.69923 67.931912,303.31713 67.931912,299.18976 64.294901,286.80765 57.020878,278.55291 46.109844,274.42554 42.472833,274.42554 31.561799,278.55291 24.287776,286.80765 20.650765,299.18976"; + var seven = "M 68.262659,241.73033 32.158284,328.90301 M 17.716535,241.73033 68.262659,241.73033"; + var eight = "M 37.489233,241.44557 27.286074,245.57254 23.885021,253.82646 23.885021,262.08039 27.286074,270.33431 34.08818,274.46127 47.692392,278.58823 57.895551,282.7152 64.697657,290.96912 68.09871,299.22304 68.09871,311.60393 64.697657,319.85785 61.296604,323.98482 51.093445,328.11178 37.489233,328.11178 27.286074,323.98482 23.885021,319.85785 20.483968,311.60393 20.483968,299.22304 23.885021,290.96912 30.687127,282.7152 40.890286,278.58823 54.494498,274.46127 61.296604,270.33431 64.697657,262.08039 64.697657,253.82646 61.296604,245.57254 51.093445,241.44557 37.489233,241.44557"; + var nine = "M 67.931912,270.29818 64.2949,282.68028 57.020878,290.93502 46.109844,295.06239 42.472833,295.06239 31.561799,290.93502 24.287776,282.68028 20.650765,270.29818 20.650765,266.17081 24.287776,253.7887 31.561799,245.53396 42.472833,241.40659 46.109844,241.40659 57.020878,245.53396 64.2949,253.7887 67.931912,270.29818 67.931912,290.93502 64.2949,311.57186 57.020878,323.95397 46.109844,328.08134 38.835821,328.08134 27.924787,323.95397 24.287776,315.69923"; + + pathData = [zero, one, two, three, four, five, six, seven, eight, nine]; + } var colon = "M 165.73227,300.21546 Z M 165.73227,253.34648 Z"; var arrayTime = new Array(); @@ -700,7 +760,7 @@ SVGReader.prototype.clock = function() { //Draws a circle to indicate the amount of time left in the minute timeCircle = function() { - timer = 0; + resetTimer(); //Draws a circle, given an array of points and the amount of delay in between each point circle = function(array, timeDelay) { @@ -712,7 +772,7 @@ SVGReader.prototype.clock = function() { //Calculates the amount of time left in the minute, based on the difference between the time the erase function was called and the time the drawTime function ended calcTimeLeft = function() { - difference += 500; //Add half a second of padding just to be safe + difference += 1000; //Add some padding just to be safe return 60000 - difference; //Everything is in milliseconds to keep calculations simple } @@ -722,7 +782,7 @@ SVGReader.prototype.clock = function() { return msPerPt; } - var radius = 54; + var radius = 53.5; var centerX = 0; var centerY = -5; var points = new Array(); @@ -741,7 +801,7 @@ SVGReader.prototype.clock = function() { //Tells the time tellTime = function() { - timer = 0; + resetTimer(); startTime = new Date().getTime(); @@ -749,7 +809,9 @@ SVGReader.prototype.clock = function() { drawTime(convertToPath(getTheTime()), function() { timeCircle(); }); - }); + }); + + //drawTime(convertToPath([0, 2, 4, 2]), console.log); //setTimeout(function() { drawTime(convertToPath(getTheTime())) }, 12500); //setTimeout(function() { drawTime(convertToPath([0, 8, 5, 8]))}, 12500); //setTimeout(function() { timeCircle() }, 12501); @@ -768,7 +830,7 @@ SVGReader.prototype.clearClock = function() { //Says 'hello' in multiple languages SVGReader.prototype.hello = function() { - timer = 0; + resetTimer(); var fileArray = fs.readdirSync("./hello"); var fileNum; objRef = this; @@ -803,6 +865,7 @@ SVGReader.prototype.hello = function() { //Says hello. sayHello = function() { + resetTimer(); var fileChoice = pickFile(); drawing.erase(function() { setTimeout(function() { objRef.drawSVG(fileChoice, connect) }, 10000); @@ -822,7 +885,7 @@ SVGReader.prototype.sayGoodbye = function() { //Cycles through all the languages rather than picking one at random SVGReader.prototype.helloCycle = function() { - timer = 0; + resetTimer(); var fileArray = fs.readdirSync("./hello"); objRef = this; var fileNum = 0; @@ -843,9 +906,9 @@ SVGReader.prototype.helloCycle = function() { else connect = false; - drawing.erase(function() { - setTimeout(function() { objRef.drawSVG(fileChoice, connect) }, 10000); - }); + //drawing.erase(function() { + // setTimeout(function() { objRef.drawSVG(fileChoice, connect) }, 10000); + //}); fileNum++; @@ -858,11 +921,6 @@ SVGReader.prototype.helloCycle = function() { } - - -//Timer variable for use with the doSetTimeout method -var timer = 0; - //Saves the coordinates of the first point drawn var firstPoint; @@ -874,7 +932,7 @@ var heightRatio; var halfway; var currentPoint; var penHeight; - -var transformX, transformY, objRef, connected, startTime, endTime, difference, clockTimer, firstMove, lastNum1, lastNum2, connect, baseWidth, baseHeight; +var delay, loaded, fontFile; +var transformX, transformY, objRef, connected, startTime, endTime, difference, clockTimer, firstMove, lastNum1, lastNum2, connect, baseWidth, baseHeight, defaultEaseType; module.exports.SVGReader = SVGReader; diff --git a/software/src/bot.js b/software/src/bot.js index 6858f18..5615d97 100755 --- a/software/src/bot.js +++ b/software/src/bot.js @@ -1,7 +1,8 @@ five = require("johnny-five"); kinematics = require("./kinematics"); -svg = require("./SVGReader"); +svgRead = require("./SVGReader"); drawing = require("./draw"); +motion = require("./motion"); //If a filepath is specified, load that config //Otherwise, resort to the default config @@ -48,34 +49,67 @@ k = new kinematics.Kinematics({ rf: config.rf }); -svgRead = new svg.SVGReader({ +svg = new svgRead.SVGReader({ baseWidth: config.baseWidth, - baseHeight: config.baseHeight + baseHeight: config.baseHeight, + drawHeight: config.drawHeight, + delay: config.delay, + defaultEaseType: config.defaultEaseType }); draw = new drawing.Draw({ baseWidth: config.baseWidth, - baseHeight: config.baseHeight + baseHeight: config.baseHeight, + drawHeight: config.drawHeight, + defaultEaseType: config.defaultEaseType }); board = new five.Board({ debug: false }); +var steps = 5; +var delay = config.delay / steps; +var defaultEaseType = config.defaultEaseType; + +current = [0, 0, -140]; +timer = 0; + board.on("ready", function() { // Setup - servo1 = five.Servo({ - pin: 9, - range: [0,90] + /*servo1 = five.Servo({ + address: 0x40, + controller: "PCA9685", + pin: 0, + range: [35, 145] //Too high of a minimum input will cause issues with the forward kinematics }); servo2 = five.Servo({ - pin: 10, - range: [0,90] + address: 0x40, + controller: "PCA9685", + pin: 1, + range: [35, 145] }); servo3 = five.Servo({ - pin: 11, - range: [0, 90] - }); + address: 0x40, + controller: "PCA9685", + pin: 2, + range: [35, 145] + }); */ + + servo1 = five.Servo({ + pin: 9, + range: [0, 100] + }); + + servo2 = five.Servo({ + pin: 10, + range: [0, 100] + }); + + servo3 = five.Servo({ + pin: 11, + range: [0, 100] + }); servo1.on("error", function() { console.log(arguments); @@ -100,10 +134,11 @@ board.on("ready", function() { var max = 15; var min = 5; var range = max - min; - servo1.to(min); - servo2.to(min); - servo3.to(min); + servo1.to(30); + servo2.to(30); + servo3.to(30); + /* var dance = function() { servo1.to(parseInt((Math.random() * range) + min, 10)); servo2.to(parseInt((Math.random() * range) + min, 10)); @@ -126,12 +161,11 @@ board.on("ready", function() { board.repl.inject({ dance: start_dance, chill: stop_dance - }); + }); */ }); - Number.prototype.map = function ( in_min , in_max , out_min , out_max ) { return ( this - in_min ) * ( out_max - out_min ) / ( in_max - in_min ) + out_min; } @@ -158,22 +192,56 @@ sin = function(degree) { // A cosine function for working with degrees, not radians cos = function(degree) { - return Math.cos(Math.PI * (degree/180)); + return Math.cos(Math.PI * (degree/180)); } - -// TODO: pull out map values to config file or some other solution. -go = function(x, y, z) { +moveServosTo = function(x, y, z) { + current = [x, y, z] reflected = reflect(x,y); rotated = rotate(reflected[0],reflected[1]); - + angles = k.inverse(rotated[0], rotated[1], z); + servo1.to((angles[1]).map(config.servo1.in_min, config.servo1.in_max, config.servo1.out_min, config.servo1.out_max)); servo2.to((angles[2]).map(config.servo2.in_min, config.servo2.in_max, config.servo2.out_min, config.servo2.out_max)); servo3.to((angles[3]).map(config.servo3.in_min, config.servo3.in_max, config.servo3.out_min, config.servo3.out_max)); console.log(angles); } +go = function(pointB, easeType) { + if (easeType == "none") { + moveServosTo(pointB[0], pointB[1], pointB[2]); + return; //Ensures that it doesn't move twice + } + + else if (!easeType) + easeType = defaultEaseType //If no easeType is specified, go with default (specified in config.js) + + //motion.move(current, pointB, steps, easeType, delay); + var points = motion.getPoints(current, pointB, steps, easeType); + + for (var i = 0; i < points.length; i++) { + setTimeout( function(point) { moveServosTo(point[0], point[1], point[2]) }, i * delay, points[i]); + } +} + +//Returns the coordinates of the end effector, based on the angles +//Using the map function messes up these values +//Simply passing in the original angles will return the correct coordinates position = function() { return k.forward(servo1.last.degrees, servo2.last.degrees, servo3.last.degrees); } + +//A separate setTimeout method so that delays work properly +doSetTimeout = function(x, y, z, timeDelay, easing) { + if (!easing) + easing = defaultEaseType; + + setTimeout(function() { go([x, y, z], easing) }, timer); + timer = timer + timeDelay; +}; + + +resetTimer = function() { + timer = 0; +} \ No newline at end of file diff --git a/software/src/draw.js b/software/src/draw.js index 397bd7a..098c8db 100644 --- a/software/src/draw.js +++ b/software/src/draw.js @@ -7,12 +7,14 @@ //> draw.erase() var fs = require('fs'); -var objRef; +var objRef, defaultEaseType; var calculated, spiralPts; function Draw(args) { - this.baseWidth = 0; - this.baseHeight = 0; + this.baseWidth = 80; + this.baseHeight = 95; + this.drawHeight = -140; + this.defaultEaseType = "linear"; if (args) { var keys = Object.keys(args); @@ -21,6 +23,7 @@ function Draw(args) { }, this) } penHeight = this.drawHeight; + defaultEaseType = this.defaultEaseType; objRef = this; } @@ -38,28 +41,31 @@ mapPoints = function(x, y) { //Adds delays while the Tapster is writing //The specific delay can be changed if the bot has to go slower or faster for a particular segment -doSetTimeout = function(x, y, z, delay) { - setTimeout(function() { go(x, y, z) }, timer); +/*doSetTimeout = function(x, y, z, delay, easing) { + if (!easing) + easing = defaultEaseType; + setTimeout(function() { go([x, y, z], easing) }, timer); currentPoint = {x: x, y: y, z: z}; timer = timer + delay; -}; +}; */ //A go method that iterates over points rather than jumping from a to b //Assumes that z stays the same //Draws points + 1 points //Delay is the total amount of time that it takes //Each point takes delay / points time to draw -Draw.prototype.itGo = function(x, y, z, points, delay) { - var x1 = currentPoint.x; - var y1 = currentPoint.y; - var deltaX = x - x1; - var deltaY = y - y1; - var slope = ((y - y1) / (x - x1)); +Draw.prototype.itGo = function(x, y, x1, y1, points, delay) { + //var x1 = currentPoint.x; + //var y1 = currentPoint.y; + var z = penHeight; + var deltaX = x1 - x; + var deltaY = y1 - y; + var slope = ((y1 - y) / (x1 - x)); var pointArray = new Array(); for (var i = 0; i <= points; i++) { - var newX = x1 + deltaX / points * i; - var newY = y1 + deltaY / points * i; + var newX = x + deltaX / points * i; + var newY = y + deltaY / points * i; var point = {x: newX, y: newY}; pointArray.push(point); } @@ -75,9 +81,6 @@ var baseHeight, baseWidth, canvasHeight, canvasWidth, heightRatio, widthRatio, h var currentPoint = {x: 0, y: 0, z: -140}; var penHeight = this.drawHeight; -//A timer variable for use with doSetTimeout() -var timer = 0; - //Set the penHeight from the command line Draw.prototype.setPenHeight = function(height) { penHeight = height; @@ -85,6 +88,7 @@ Draw.prototype.setPenHeight = function(height) { } //Draws an image from a JSON file of coordinates +//The SVGReader drawSVG method is preferred Draw.prototype.drawFromCoordinates = function() { baseHeight = this.baseHeight; @@ -136,7 +140,10 @@ Draw.prototype.drawFromCoordinates = function() { //Draws every nth point Draw.prototype.drawSquare = function(sideLength, n) { - timer = 0; //Reset the timer so that there isn't unnecessary delay when calling the function multiple times + resetTimer(); //Reset the timer so that there isn't unnecessary delay when calling the function multiple times + + if (!n) + var n = 2; var halfSide = sideLength / 2; var points = sideLength / n; @@ -165,18 +172,21 @@ Draw.prototype.drawSquare = function(sideLength, n) { }; //Draws a star to test that the Tapster bot is working properly -Draw.prototype.drawStar = function(x, y, x1, y1, x2, y2) { - doSetTimeout(x, y, penHeight, 1000); - doSetTimeout(x1, y1, penHeight, 1000); - doSetTimeout(x2, y2, penHeight, 1000); - doSetTimeout(x, y1 * 3 / 4, penHeight, 1000); - doSetTimeout(x2, y1 * 3 / 4, penHeight, 1000); - doSetTimeout(x, y, penHeight, 1000); +Draw.prototype.drawStar = function() { + resetTimer(); + doSetTimeout(-20, -20, penHeight, 1000); + doSetTimeout(0, 30, penHeight, 1000); + doSetTimeout(20, -20, penHeight, 1000); + doSetTimeout(-30, 10, penHeight, 1000); + doSetTimeout(30, 10, penHeight, 1000); + doSetTimeout(-20, -20, penHeight, 1000); + + //-20, -20, 0, 30, 20, -20 }; Draw.prototype.drawTriangle = function(x, y, x1, y1, x2, y2) { - timer = 0; + resetTimer(); doSetTimeout(x, y, penHeight, 1000); doSetTimeout(x1, y1, penHeight, 1000); doSetTimeout(x2, y2, penHeight, 1000); @@ -184,30 +194,39 @@ Draw.prototype.drawTriangle = function(x, y, x1, y1, x2, y2) { } Draw.prototype.drawCircle = function(radius, x, y) { - timer = 0; - var centerX=x; - var centerY=y; + //40 + resetTimer(); + if (x && y) { + var centerX = x; + var centerY = y; + } + + else { + var centerX = 0; + var centerY = 0; + } + var radius=radius; // an array to save your points var points=[]; - + // populate array with points along a circle //Goes slightly over so that the circle is actually completed - for (var degree=0; degree < 365; degree++) { + for (var degree=0; degree < 395; degree++) { var radians = (degree + 90) * Math.PI/180; var x = centerX + radius * Math.cos(radians); var y = centerY + radius * Math.sin(radians); points.push({x:x,y:y}); } - - doSetTimeout(points[0].x, points[0].y, penHeight + 10, 20); - doSetTimeout(points[0].x, points[0].y, penHeight, 20); circle = function() { + doSetTimeout(0, 0, -120, 50, "none"); + doSetTimeout(points[0].x, points[0].y, penHeight + 10, 150, "none"); + doSetTimeout(points[0].x, points[0].y, penHeight, 150, "none"); for (var i=0; i + + + + + + + + + image/svg+xml + + + + + + + + + + + + + + + diff --git a/software/src/example/textTutorial.svg b/software/src/example/textTutorial.svg new file mode 100644 index 0000000..d816475 --- /dev/null +++ b/software/src/example/textTutorial.svg @@ -0,0 +1,74 @@ + + + + + + + + + + image/svg+xml + + + + + + + + + + + + diff --git a/software/src/example/tutorial.svg b/software/src/example/tutorial.svg new file mode 100644 index 0000000..70ed47d --- /dev/null +++ b/software/src/example/tutorial.svg @@ -0,0 +1,77 @@ + + + + + + + + + + image/svg+xml + + + + + + + + + + + + diff --git a/software/src/hello/helloChF.svg b/software/src/hello/helloChF.svg index 56c4a7b..2fe5d96 100644 --- a/software/src/hello/helloChF.svg +++ b/software/src/hello/helloChF.svg @@ -15,7 +15,7 @@ id="svg3658" version="1.1" inkscape:version="0.91 r13725" - sodipodi:docname="helloCh.svg"> + sodipodi:docname="helloChF.svg"> + inkscape:window-maximized="1" + showguides="false"> + + @@ -44,106 +50,101 @@ image/svg+xml - + - - - - - - - - - - - - - - - - - + id="g3544"> + + + + + + + + + + + + + + + diff --git a/software/src/hello/helloRuF.svg b/software/src/hello/helloRuF.svg index 3205637..d3d732e 100644 --- a/software/src/hello/helloRuF.svg +++ b/software/src/hello/helloRuF.svg @@ -25,9 +25,9 @@ showgrid="false" inkscape:current-layer="svg4336" inkscape:document-units="mm" - inkscape:cy="168.30709" - inkscape:cx="141.73228" - inkscape:zoom="1.6517427" + inkscape:cy="123.97494" + inkscape:cx="187.85961" + inkscape:zoom="6.3964574" inkscape:pageshadow="2" inkscape:pageopacity="0.0" borderopacity="1.0" @@ -49,42 +49,42 @@ + id="g4818"> + sodipodi:nodetypes="ccccccccccccccccc" /> + sodipodi:nodetypes="ccccccccccccc" /> + sodipodi:nodetypes="cccccccccccccccccccccc" /> + sodipodi:nodetypes="cccccccccccccccccc" /> + sodipodi:nodetypes="ccccccccc" /> diff --git a/software/src/kinematics.js b/software/src/kinematics.js index b4d6476..400eeb1 100644 --- a/software/src/kinematics.js +++ b/software/src/kinematics.js @@ -30,7 +30,7 @@ function Kinematics(args) { } // Forward kinematics: (theta1, theta2, theta3) -> (x0, y0, z0) -// Returned {error code,theta1,theta2,theta3} +// Returned {error code,theta1,theta2,theta3} Kinematics.prototype.forward = function(theta1, theta2, theta3) { var x0 = 0.0; var y0 = 0.0; diff --git a/software/src/motion.js b/software/src/motion.js new file mode 100644 index 0000000..1f62398 --- /dev/null +++ b/software/src/motion.js @@ -0,0 +1,90 @@ +/* + +// Usage: +// motion.move(pointA, pointB, numberOfSteps, easingType, timeDeltaInMilliseconds) +// Example +$ node bot.js +>> motion = require('./motion') +>> motion.move([0,0,-140],[-20,20,-140], 20, 'easeInOutCubic', 500) +>> motion.move([-20,60,-165],[-20,-60,-165], 30, 'easeInOutCubic', 20) +>> motion.move([-20,-60,-165],[-20,60,-165], 30, 'easeInOutCubic', 20) + + +Usage: +> motion = require('./motion') +> motion.directionVector([0,0,-100], [5,10,-150]) +[ 5, 10, -50 ] + +> var A = [1,6,3]; +> var B = [8,2,7]; +> motion.directionVector(A, B); +[ 7, -4, 4 ] + + +Reference: +Line between two points in 3D space: http://mathcentral.uregina.ca/QQ/database/QQ.09.01/murray2.html +Easing functions: https://gist.github.com/gre/1650294 + +*/ +var EasingFunctions = { + linear: function (t) { return t }, + easeInQuad: function (t) { return t*t }, + easeOutQuad: function (t) { return t*(2-t) }, + easeInOutQuad: function (t) { return t<.5 ? 2*t*t : -1+(4-2*t)*t }, + easeInCubic: function (t) { return t*t*t }, + easeOutCubic: function (t) { return (--t)*t*t+1 }, + easeInOutCubic: function (t) { return t<.5 ? 4*t*t*t : (t-1)*(2*t-2)*(2*t-2)+1 }, + easeInQuart: function (t) { return t*t*t*t }, + easeOutQuart: function (t) { return 1-(--t)*t*t*t }, + easeInOutQuart: function (t) { return t<.5 ? 8*t*t*t*t : 1-8*(--t)*t*t*t }, + easeInQuint: function (t) { return t*t*t*t*t }, + easeOutQuint: function (t) { return 1+(--t)*t*t*t*t }, + easeInOutQuint: function (t) { return t<.5 ? 16*t*t*t*t*t : 1+16*(--t)*t*t*t*t } +}; + +(function(exports) { + var directionVector = function(pointA, pointB) { + var vector = [ pointB[0] - pointA[0], + pointB[1] - pointA[1], + pointB[2] - pointA[2] ]; + return vector; + } + + // (x,y,z) = (1,6,3) + t(7,-4,4) = (1 + 7t, 6 - 4t, 3 + 4t). + var parametricEquation = function(pointA, pointB) { + var dv = directionVector(pointA, pointB); + var equation = function(t) { + return [ pointA[0] + dv[0]*t, + pointA[1] + dv[1]*t, + pointA[2] + dv[2]*t ]; + } + return equation; + } + + // get an array of points between (and including) two end points + // numberOfSteps and easingType are required + var getPoints = function(pointA, pointB, numberOfSteps, easingType) { + var points = []; + var point = parametricEquation(pointA, pointB); + var easingFunction = EasingFunctions[easingType]; + + for (var i = 0; i <= numberOfSteps; i++) { + t = easingFunction(i/numberOfSteps); + points.push(point(t)) + } + return points; + } + + var move = function(pointA, pointB, numberOfSteps, easingType, timeDelta) { + var points = getPoints(pointA, pointB, numberOfSteps, easingType); + for (var i=0; i + + + + + + + + + + + image/svg+xml + + + + + + + + + + + + + + + + + + + + + + diff --git a/software/src/tutorial.svg b/software/src/tutorial.svg new file mode 100644 index 0000000..31896fe --- /dev/null +++ b/software/src/tutorial.svg @@ -0,0 +1,60 @@ + + + + + + + + + + image/svg+xml + + + + + + + + + From 2474553dd3202d3ddeb8cc43965532254eb8a4f6 Mon Sep 17 00:00:00 2001 From: Jack Skalitzky Date: Thu, 16 Jul 2015 22:35:22 -0500 Subject: [PATCH 37/43] Deleted redundant file --- software/src/tutorial.svg | 60 --------------------------------------- 1 file changed, 60 deletions(-) delete mode 100644 software/src/tutorial.svg diff --git a/software/src/tutorial.svg b/software/src/tutorial.svg deleted file mode 100644 index 31896fe..0000000 --- a/software/src/tutorial.svg +++ /dev/null @@ -1,60 +0,0 @@ - - - - - - - - - - image/svg+xml - - - - - - - - - From 3c23214b869670e14cbc970fff90fa3430e00344 Mon Sep 17 00:00:00 2001 From: Jack Skalitzky Date: Fri, 17 Jul 2015 19:31:31 -0500 Subject: [PATCH 38/43] Angry Birds demo --- software/src/bot.js | 14 ++- software/src/demo/angry-birds/angrybirds.js | 121 +++++++++++++++++--- 2 files changed, 113 insertions(+), 22 deletions(-) diff --git a/software/src/bot.js b/software/src/bot.js index 5615d97..cfe4344 100755 --- a/software/src/bot.js +++ b/software/src/bot.js @@ -3,6 +3,7 @@ kinematics = require("./kinematics"); svgRead = require("./SVGReader"); drawing = require("./draw"); motion = require("./motion"); +ab = require("./demo/angry-birds/angrybirds") //If a filepath is specified, load that config //Otherwise, resort to the default config @@ -64,6 +65,10 @@ draw = new drawing.Draw({ defaultEaseType: config.defaultEaseType }); +birds = new ab.AngryBirds({ + drawHeight: config.drawHeight +}); + board = new five.Board({ debug: false }); @@ -134,9 +139,9 @@ board.on("ready", function() { var max = 15; var min = 5; var range = max - min; - servo1.to(30); - servo2.to(30); - servo3.to(30); + servo1.to(15); + servo2.to(15); + servo3.to(15); /* var dance = function() { @@ -208,7 +213,8 @@ moveServosTo = function(x, y, z) { console.log(angles); } -go = function(pointB, easeType) { +go = function(x, y, z, easeType) { + var pointB = [x, y, z]; if (easeType == "none") { moveServosTo(pointB[0], pointB[1], pointB[2]); return; //Ensures that it doesn't move twice diff --git a/software/src/demo/angry-birds/angrybirds.js b/software/src/demo/angry-birds/angrybirds.js index 697d9ac..d78d507 100644 --- a/software/src/demo/angry-birds/angrybirds.js +++ b/software/src/demo/angry-birds/angrybirds.js @@ -1,24 +1,92 @@ +function AngryBirds(args) { + this.drawHeight = -151; + if (args) { + var keys = Object.keys(args) + keys.forEach(function(key){ + this[key] = args[key] + }, this) + } + + drawHeight = this.drawHeight; + +}; + move = function(x,y,z, when) { - setTimeout(function(){ go(x,y,z);}, when); + setTimeout(function(){ go(x,y,z) }, moveTimer); + moveTimer += when; +} + +resetMoveTimer = function() { + moveTimer = 0; +} + +var moveTimer = 0; + +AngryBirds.prototype.playLevelOne = function(){ + resetMoveTimer(); + move(-20, -3, drawHeight + 10, 0); + move(-20, -3, drawHeight, 500); + move(-35,-6, drawHeight,300); + move(-35,-6, drawHeight + 10,400); + move(-30,0, drawHeight + 10,400); + + move(42,-18, drawHeight + 10, 2800); + move(42,-18, drawHeight - 3, 500); + move(42, -18, drawHeight + 10, 500); + + move(9,-13, drawHeight + 10, 3000); + move(9,-13, drawHeight - 2, 300); + move(9, -13, drawHeight + 10,300); + move(0,0, drawHeight + 10,300); } -play = function(){ - move(-10,-20,-140,0); - move(-10,-20,-149,200); - move(-25,-28,-148,500); - move(-25,-28,-140,800); - move(0,0,-120,1200); +AngryBirds.prototype.playLevelTwo = function() { + resetMoveTimer(); + move(-20, -3, drawHeight + 10, 500); + move(-20, -3, drawHeight, 200); + move(-35, -2, drawHeight, 300); + move(-35, -2, drawHeight + 10, 300); + move(-20, -3, drawHeight + 10, 15000); + move(-20, -3, drawHeight - 1, 300); + move(-35, -8, drawHeight, 400); + move(-35, -8, drawHeight + 10, 300); - move(34,-36,-140, 4000); - move(34,-36,-150, 4500); - move(34,-36,-140, 5000); + move(42, -18, drawHeight + 10, 6000); + move(42, -18, drawHeight - 3, 500); + move(42, -18, drawHeight + 10, 500); - move(4,-32,-140,0+8000); - move(4,-32,-149,300+8000); - move(4,-32,-140,600+8000); - move(0,0,-120,900+8000); + move(9, -13, drawHeight + 10, 3000); + move(9, -13, drawHeight - 2, 300); + move(9, -13, drawHeight + 10, 300); + move(0, 0, drawHeight + 10, 400); } +AngryBirds.prototype.playLevelThree = function() { + resetMoveTimer(); + move(-20, -3, drawHeight + 10, 1000); + move(-20, -3, drawHeight, 200); + move(-25, -10, drawHeight, 350); + move(-23, -6.5, drawHeight, 350); + move(-23, -6.5, drawHeight + 10, 350); + + move(42, -18, drawHeight + 10, 8000); + move(42, -18, drawHeight - 3, 500); + move(42, -18, drawHeight + 10, 500); +}; + +goToLevelOne = function() { + resetMoveTimer(); + move(-14, -12, drawHeight + 10, 250); + move(-14, -12, drawHeight - 2, 250); + move(-14, -12, drawHeight + 10, 250); + move(-48, 23, drawHeight + 10, 1000); + move(-46, 23, drawHeight - 2, 1000); + move(-48, 23, drawHeight + 10, 500); + move(45, -18, drawHeight + 10, 500); + move(41, -18, drawHeight - 4, 500); + move(45, -18, drawHeight + 10, 500); + move(0, 0, drawHeight + 10, 250); +} repeat = function(){ move(4,-32,-140,0); @@ -27,10 +95,27 @@ repeat = function(){ move(0,0,-120,900); } +AngryBirds.prototype.playLevels = function() { + resetMoveTimer(); + var objRef = this; + setTimeout(objRef.playLevelOne, 0); + setTimeout(objRef.playLevelTwo, 15000); + setTimeout(objRef.playLevelThree, 47500); + setTimeout(goToLevelOne, 65000); +} + -play_forever = function(){ +AngryBirds.prototype.play_forever = function(){ + var objRef = this; console.log("Now playing forever...") - play(); - interval = setInterval(play,13000); + this.playLevels(); + interval = setInterval(objRef.playLevels, 70000); return interval; -} \ No newline at end of file +} + +AngryBirds.prototype.stop_playing = function() { + clearInterval(interval); + console.log("No longer playing forever."); +} + +module.exports.AngryBirds = AngryBirds; \ No newline at end of file From 619a04557da58ca7e1a1c1941ae9e40ef5a0ea84 Mon Sep 17 00:00:00 2001 From: Jack Skalitzky Date: Tue, 28 Jul 2015 13:07:47 -0500 Subject: [PATCH 39/43] Moved angrybirds outside of bot.js --- software/src/bot.js | 7 +- software/src/demo/angry-birds/angrybirds.js | 163 +++++++++++++++++--- 2 files changed, 144 insertions(+), 26 deletions(-) diff --git a/software/src/bot.js b/software/src/bot.js index cfe4344..4c0cb8e 100755 --- a/software/src/bot.js +++ b/software/src/bot.js @@ -3,7 +3,6 @@ kinematics = require("./kinematics"); svgRead = require("./SVGReader"); drawing = require("./draw"); motion = require("./motion"); -ab = require("./demo/angry-birds/angrybirds") //If a filepath is specified, load that config //Otherwise, resort to the default config @@ -65,10 +64,6 @@ draw = new drawing.Draw({ defaultEaseType: config.defaultEaseType }); -birds = new ab.AngryBirds({ - drawHeight: config.drawHeight -}); - board = new five.Board({ debug: false }); @@ -243,7 +238,7 @@ doSetTimeout = function(x, y, z, timeDelay, easing) { if (!easing) easing = defaultEaseType; - setTimeout(function() { go([x, y, z], easing) }, timer); + setTimeout(function() { go(x, y, z, easing) }, timer); timer = timer + timeDelay; }; diff --git a/software/src/demo/angry-birds/angrybirds.js b/software/src/demo/angry-birds/angrybirds.js index d78d507..86643ec 100644 --- a/software/src/demo/angry-birds/angrybirds.js +++ b/software/src/demo/angry-birds/angrybirds.js @@ -1,18 +1,143 @@ -function AngryBirds(args) { - this.drawHeight = -151; - if (args) { - var keys = Object.keys(args) - keys.forEach(function(key){ - this[key] = args[key] - }, this) - } +var five = require("johnny-five") +var kin = require("./../../kinematics.js"); +var config = require("./../../../config.js"); + +drawHeight = config.drawHeight; + +board = new five.Board({ + debug: false +}); + +k = new kin.Kinematics({ + e: config.e, + f: config.f, + re: config.re, + rf: config.rf +}) + +board.on("ready", function() { + // Setup + /*servo1 = five.Servo({ + address: 0x40, + controller: "PCA9685", + pin: 0, + range: [35, 145] //Too high of a minimum input will cause issues with the forward kinematics + }); + servo2 = five.Servo({ + address: 0x40, + controller: "PCA9685", + pin: 1, + range: [35, 145] + }); + servo3 = five.Servo({ + address: 0x40, + controller: "PCA9685", + pin: 2, + range: [35, 145] + }); */ + + servo1 = five.Servo({ + pin: 9, + range: [0, 100] + }); + + servo2 = five.Servo({ + pin: 10, + range: [0, 100] + }); + + servo3 = five.Servo({ + pin: 11, + range: [0, 100] + }); + + servo1.on("error", function() { + console.log(arguments); + }) + servo2.on("error", function() { + console.log(arguments); + }) + servo3.on("error", function() { + console.log(arguments); + }) + + board.repl.inject({ + servo1: servo1, + s1: servo1, + servo2: servo2, + s2: servo2, + servo3: servo3, + s3: servo3, + }); + + // Move to starting point + var max = 15; + var min = 5; + var range = max - min; + servo1.to(15); + servo2.to(15); + servo3.to(15); + + /* + var dance = function() { + servo1.to(parseInt((Math.random() * range) + min, 10)); + servo2.to(parseInt((Math.random() * range) + min, 10)); + servo3.to(parseInt((Math.random() * range) + min, 10)); + }; + + var dancer; + + start_dance = function() { + if (!dancer) dancer = setInterval(dance, 250); + } + + stop_dance = function() { + if (dancer) { + clearInterval(dancer); + dancer = null; + } + } + + board.repl.inject({ + dance: start_dance, + chill: stop_dance + }); */ + + +}); + +rotate = function(x,y) { + var theta = -60 * Math.PI / 180; + x1 = x * Math.cos(theta) - y * Math.sin(theta); + y1 = y * Math.cos(theta) + x * Math.sin(theta); + return [x1,y1] +} - drawHeight = this.drawHeight; +reflect = function(x,y) { + var theta = 0; + x1 = x; + y1 = x * Math.sin(2*theta) - y * Math.cos(2*theta); + return [x1,y1] +} -}; +Number.prototype.map = function ( in_min , in_max , out_min , out_max ) { + return ( this - in_min ) * ( out_max - out_min ) / ( in_max - in_min ) + out_min; +} + +moveServosTo = function(x, y, z) { + reflected = reflect(x,y); + rotated = rotate(reflected[0],reflected[1]); + + angles = k.inverse(rotated[0], rotated[1], z); + + servo1.to((angles[1]).map(config.servo1.in_min, config.servo1.in_max, config.servo1.out_min, config.servo1.out_max)); + servo2.to((angles[2]).map(config.servo2.in_min, config.servo2.in_max, config.servo2.out_min, config.servo2.out_max)); + servo3.to((angles[3]).map(config.servo3.in_min, config.servo3.in_max, config.servo3.out_min, config.servo3.out_max)); + console.log(angles); +} move = function(x,y,z, when) { - setTimeout(function(){ go(x,y,z) }, moveTimer); + setTimeout(function(){ moveServosTo(x,y,z) }, moveTimer); moveTimer += when; } @@ -22,7 +147,7 @@ resetMoveTimer = function() { var moveTimer = 0; -AngryBirds.prototype.playLevelOne = function(){ +playLevelOne = function(){ resetMoveTimer(); move(-20, -3, drawHeight + 10, 0); move(-20, -3, drawHeight, 500); @@ -40,7 +165,7 @@ AngryBirds.prototype.playLevelOne = function(){ move(0,0, drawHeight + 10,300); } -AngryBirds.prototype.playLevelTwo = function() { +playLevelTwo = function() { resetMoveTimer(); move(-20, -3, drawHeight + 10, 500); move(-20, -3, drawHeight, 200); @@ -61,7 +186,7 @@ AngryBirds.prototype.playLevelTwo = function() { move(0, 0, drawHeight + 10, 400); } -AngryBirds.prototype.playLevelThree = function() { +playLevelThree = function() { resetMoveTimer(); move(-20, -3, drawHeight + 10, 1000); move(-20, -3, drawHeight, 200); @@ -95,7 +220,7 @@ repeat = function(){ move(0,0,-120,900); } -AngryBirds.prototype.playLevels = function() { +playLevels = function() { resetMoveTimer(); var objRef = this; setTimeout(objRef.playLevelOne, 0); @@ -105,7 +230,7 @@ AngryBirds.prototype.playLevels = function() { } -AngryBirds.prototype.play_forever = function(){ +play_forever = function(){ var objRef = this; console.log("Now playing forever...") this.playLevels(); @@ -113,9 +238,7 @@ AngryBirds.prototype.play_forever = function(){ return interval; } -AngryBirds.prototype.stop_playing = function() { +stop_playing = function() { clearInterval(interval); console.log("No longer playing forever."); -} - -module.exports.AngryBirds = AngryBirds; \ No newline at end of file +} \ No newline at end of file From 55cdb3c0b4bbced41eb7955bf3f1f97aa48a2de2 Mon Sep 17 00:00:00 2001 From: Jack Skalitzky Date: Tue, 28 Jul 2015 13:08:18 -0500 Subject: [PATCH 40/43] Updated draw method arguments --- software/src/draw.js | 161 +++++++++++++++++++++---------------------- 1 file changed, 78 insertions(+), 83 deletions(-) diff --git a/software/src/draw.js b/software/src/draw.js index 098c8db..8b6e025 100644 --- a/software/src/draw.js +++ b/software/src/draw.js @@ -1,10 +1,4 @@ //Draws lines and shapes. -//> To test: -//> draw.drawSquare(sideLength, n) -//> draw.drawStar() -//> draw.drawCircle() -//> draw.drawFromCoordinates() -//> draw.erase() var fs = require('fs'); var objRef, defaultEaseType; @@ -44,37 +38,11 @@ mapPoints = function(x, y) { /*doSetTimeout = function(x, y, z, delay, easing) { if (!easing) easing = defaultEaseType; - setTimeout(function() { go([x, y, z], easing) }, timer); + setTimeout(function() { go(x, y, z, easing) }, timer); currentPoint = {x: x, y: y, z: z}; timer = timer + delay; }; */ -//A go method that iterates over points rather than jumping from a to b -//Assumes that z stays the same -//Draws points + 1 points -//Delay is the total amount of time that it takes -//Each point takes delay / points time to draw -Draw.prototype.itGo = function(x, y, x1, y1, points, delay) { - //var x1 = currentPoint.x; - //var y1 = currentPoint.y; - var z = penHeight; - var deltaX = x1 - x; - var deltaY = y1 - y; - var slope = ((y1 - y) / (x1 - x)); - var pointArray = new Array(); - - for (var i = 0; i <= points; i++) { - var newX = x + deltaX / points * i; - var newY = y + deltaY / points * i; - var point = {x: newX, y: newY}; - pointArray.push(point); - } - - for (var i = 0; i < pointArray.length; i++) { - doSetTimeout(pointArray[i].x, pointArray[i].y, z, delay / points); - } -} - //Initialized here so that they are accessible from the mapPoints function var baseHeight, baseWidth, canvasHeight, canvasWidth, heightRatio, widthRatio, halfway; @@ -136,35 +104,44 @@ Draw.prototype.drawFromCoordinates = function() { }; //Draws a square in order to ensure that everything is working properly -//sideLength is the length of the sides -//Draws every nth point -Draw.prototype.drawSquare = function(sideLength, n) { +//Optional args: +//sideLength: the length of each side (default: 20) +//n: draws every nth point (default: 2) +Draw.prototype.drawSquare = function(args) { - resetTimer(); //Reset the timer so that there isn't unnecessary delay when calling the function multiple times + //Default values + this.sideLength = 20; + this.n = 2; - if (!n) - var n = 2; + if (args) { + var keys = Object.keys(args) + keys.forEach(function(key){ + this[key] = args[key] + }, this) + } + + resetTimer(); //Reset the timer so that there isn't unnecessary delay when calling the function multiple times - var halfSide = sideLength / 2; - var points = sideLength / n; + var halfSide = this.sideLength / 2; + var points = this.sideLength / this.n; doSetTimeout(-halfSide, halfSide, penHeight + 10, 0) doSetTimeout(-halfSide, halfSide, penHeight, 500); //Top left corner for (var i = 0; i < points; i++) { //To bottom left - doSetTimeout(-halfSide, halfSide - (n * i), penHeight, i * 5); + doSetTimeout(-halfSide, halfSide - (this.n * i), penHeight, i * 5); } for (var i = 0; i < points; i++) { //To bottom right - doSetTimeout(-halfSide + (n * i), -halfSide, penHeight, i * 5); + doSetTimeout(-halfSide + (this.n * i), -halfSide, penHeight, i * 5); } for (var i = 0; i < points; i++) { //To top right - doSetTimeout(halfSide, -halfSide + (n * i), penHeight, i * 5); + doSetTimeout(halfSide, -halfSide + (this.n * i), penHeight, i * 5); } for (var i = 0; i < points; i++) { //To top left - doSetTimeout(halfSide - (n * i), halfSide, penHeight, i * 5); + doSetTimeout(halfSide - (this.n * i), halfSide, penHeight, i * 5); } doSetTimeout(0, 0, -140, timer + 100); @@ -193,20 +170,24 @@ Draw.prototype.drawTriangle = function(x, y, x1, y1, x2, y2) { doSetTimeout(x, y, penHeight, 1000); } -Draw.prototype.drawCircle = function(radius, x, y) { - //40 +//Draws a circle +//Optional args: +//centerX: the x coordinate of the center (default: 0) +//centerY: the y coordinate of the center (default: 0) +//radius: the radius of the center (default: 20) +Draw.prototype.drawCircle = function(args) { resetTimer(); - if (x && y) { - var centerX = x; - var centerY = y; - } - else { - var centerX = 0; - var centerY = 0; - } + this.centerX = 0; + this.centerY = 0; + this.radius = 20; - var radius=radius; + if (args) { + var keys = Object.keys(args) + keys.forEach(function(key){ + this[key] = args[key] + }, this) + } // an array to save your points var points=[]; @@ -215,8 +196,8 @@ Draw.prototype.drawCircle = function(radius, x, y) { //Goes slightly over so that the circle is actually completed for (var degree=0; degree < 395; degree++) { var radians = (degree + 90) * Math.PI/180; - var x = centerX + radius * Math.cos(radians); - var y = centerY + radius * Math.sin(radians); + var x = this.centerX + this.radius * Math.cos(radians); + var y = this.centerY + this.radius * Math.sin(radians); points.push({x:x,y:y}); } @@ -234,34 +215,48 @@ Draw.prototype.drawCircle = function(radius, x, y) { }; //Draws an arbitrary amount of spirals to test that the Tapster bot is working properly -//Spirals is the amount of spirals to draw -//Radius is the diameter(?) of the largest spiral -//zLevel is optional -- it is the penHeight to draw the spiral at (mainly used for the erase function) -Draw.prototype.drawSpiral = function(spirals, radius, zLevel, ptArray) { +//Optional args: +//startX: the x coordinate of the spiral's starting point (default: 0) +//startY: the y coordinate of the spiral's starting point (default: 0) +//spirals: amount of spirals to draw (default: 3) +//radius: radius of largest spiral (default: 30) +//zLevel: height at which to draw the spiral (default: penHeight) +//ptArray: an array of points to draw instead of calculating the spiral again (default: null) +Draw.prototype.drawSpiral = function(args) { //spirals, radius, zLevel, ptArray) { + + this.startX = 0; + this.startY = 0; + this.spirals = 3; + this.radius = 30; + this.zLevel = penHeight; + this.ptArray = null; + + if (args) { + var keys = Object.keys(args) + keys.forEach(function(key){ + this[key] = args[key] + }, this) + } resetTimer(); - var centerX = 0; - var centerY = 0; + var x1 = 0; var y1 = 0; var points = []; - if (zLevel) //If a zLevel is specified set the penHeight at that level - penHeight = zLevel; - - if (ptArray) { - points = ptArray; + if (this.ptArray) { + points = this.ptArray; } //Draws additional points, mainly for use with the erase function - //The extra points allow the eraser to end up in its resting positio + //The extra points allow the eraser to end up in its resting position else { - for (var degree = 0; degree < spirals * 360 + 95; degree++) { - x1 = x1 + radius/(spirals * 360); - y1 = y1 + radius/(spirals * 360); + for (var degree = 0; degree < this.spirals * 360 + 95; degree++) { + x1 = x1 + this.radius/(this.spirals * 360); + y1 = y1 + this.radius/(this.spirals * 360); var radians = degree * Math.PI/180; - var x = centerX + x1 * Math.cos(radians); - var y = centerY + y1 * Math.sin(radians); + var x = this.startX + x1 * Math.cos(radians); + var y = this.startY + y1 * Math.sin(radians); points.push({x:x, y:y}); } //doSetTimeout(points[0].x, points[0].y, penHeight + 10, 150); @@ -271,7 +266,7 @@ Draw.prototype.drawSpiral = function(spirals, radius, zLevel, ptArray) { spiral = function() { for (var z = 0; z < points.length; z++) { point = points[z]; - doSetTimeout(point.x, point.y, penHeight, 5, "none"); + doSetTimeout(point.x, point.y, objRef.zLevel, 5, "none"); } } @@ -281,14 +276,14 @@ Draw.prototype.drawSpiral = function(spirals, radius, zLevel, ptArray) { Draw.prototype.test = function() { var objRef = this; - setTimeout(function() { objRef.drawCircle(15, 20, 23.75) }, 1000); - setTimeout(function() { go([-30, 10, penHeight + 20]) }, 6500); + setTimeout(function() { objRef.drawCircle({centerX: 15, centerY: 20, radius: 23.75}) }, 1000); + setTimeout(function() { go(-30, 10, penHeight + 20) }, 6500); setTimeout(function() { star(-30, 10, -20, 30, -15, 10, 5) }, 7000); - setTimeout(function() { go([-10, -10, penHeight + 20]) }, 16000); + setTimeout(function() { go(-10, -10, penHeight + 20) }, 16000); setTimeout(function() { square(-10, -10, -30, -10, -30, -30, -10, -30) }, 18000); - setTimeout(function() { go([10, -30, penHeight + 20]) }, 25000); + setTimeout(function() { go(10, -30, penHeight + 20) }, 25000); setTimeout(function() { objRef.drawTriangle(10, -30, 20, -10, 30, -30) }, 27000); - setTimeout(function() { go([0, 0, -140]) }, 31000); + setTimeout(function() { go(0, 0, -140) }, 31000); square = function(x, y, x1, y1, x2, y2, x3, y3) { resetTimer(); @@ -322,11 +317,11 @@ Draw.prototype.eraseBoard = function() { eraseHeight = this.drawHeight + 5.25; if (!calculated) { - spiralPts = this.drawSpiral(4, 55, eraseHeight); + spiralPts = this.drawSpiral({spirals: 4, radius: 55, zLevel: eraseHeight}); calculated = true; } else { - this.drawSpiral(4, 55, eraseHeight, spiralPts); + this.drawSpiral({ptArray: spiralPts}); } } From 51741d8111f8f2875a63b9da39ba05d6f060800b Mon Sep 17 00:00:00 2001 From: Jack Skalitzky Date: Tue, 28 Jul 2015 13:09:42 -0500 Subject: [PATCH 41/43] Support for relative filepaths --- software/src/SVGReader.js | 1 + 1 file changed, 1 insertion(+) diff --git a/software/src/SVGReader.js b/software/src/SVGReader.js index 9060605..21c85d1 100644 --- a/software/src/SVGReader.js +++ b/software/src/SVGReader.js @@ -29,6 +29,7 @@ function SVGReader(args) { //Draws from an SVG image specified by filepath //> Usage: //> drawSVG("C:/Projects/Tapsterbot/software/src/drawing.svg"); +//Note: filePath can be relative //connect is a special flag that indicates that each path should be drawn connected to each other //It is really only used for drawing in cursive and does not need to be specified otherwise SVGReader.prototype.drawSVG = function(filePath, connect) { From 0c773377fb7465b3428e77cbafa1972dd6fecf91 Mon Sep 17 00:00:00 2001 From: Jack Skalitzky Date: Tue, 28 Jul 2015 14:18:23 -0500 Subject: [PATCH 42/43] *Switch* to switch statements heh Also some misc. cleanup --- software/src/SVGReader.js | 278 ++++++++++++++++---------------------- 1 file changed, 120 insertions(+), 158 deletions(-) diff --git a/software/src/SVGReader.js b/software/src/SVGReader.js index 21c85d1..52e2fdc 100644 --- a/software/src/SVGReader.js +++ b/software/src/SVGReader.js @@ -455,160 +455,131 @@ SVGReader.prototype.interpretCommands = function(commands) { delay = this.delay; for (var i = 0; i < commands.length; i++) { var cmdCode = commands[i].code; - if (cmdCode == 'M') { - //var temp = move(commands[i].x, commands[i].y); - //for (var i = 0; i < temp.length; i++) - //doSetTimeout(temp[i].x, temp[i].y, temp[i].z, delay); - move(commands[i].x, commands[i].y); - } - - else if (cmdCode == 'm') { - relMove(commands[i].x, commands[i].y); - } - - else if (cmdCode == 'L') { - line(commands[i].x, commands[i].y); - } - - else if (cmdCode == 'l') { - relLine(commands[i].x, commands[i].y); - } - - else if (cmdCode == 'V') { - line(currentPoint.x, commands[i].y); - } - - else if (cmdCode == 'v') { - relLine(currentPoint.x, commands[i].y); - } - - else if (cmdCode == 'H') { - line(commands[i].x, currentPoint.y); - } - - else if (cmdCode == 'h') { - relLine(commands[i].x, currentPoint.y); - } - - else if (cmdCode == 'C') { - cubicCurve(commands[i].x1, commands[i].y1, commands[i].x2, commands[i].y2, commands[i].x, commands[i].y); - } + switch (cmdCode) { + case 'M': + move(commands[i].x, commands[i].y); + break; + + case 'm': + relMove(commands[i].x, commands[i].y); + break; + + case 'L': + line(commands[i].x, commands[i].y); + break; + + case 'l': + relLine(commands[i].x, commands[i].y); + break; + + case 'V': + line(currentPoint.x, commands[i].y); + break; + + case 'v': + relLine(currentPoint.x, commands[i].y); + break; + + case 'H': + line(commands[i].x, currentPoint.y); + break; + + case 'h': + relLine(commands[i].x, currentPoint.y); + break; + + case 'C': + cubicCurve(commands[i].x1, commands[i].y1, commands[i].x2, commands[i].y2, commands[i].x, commands[i].y); + break; + + case 'c': + relCubicCurve(commands[i].x1, commands[i].y1, commands[i].x2, commands[i].y2, commands[i].x, commands[i].y); + break; + + //Smooth cubic curve + case 'S': + if (i > 1 && (commands[i-1].code == 's' || commands[i-1].code == 'c' || commands[i-1].code == 'C' || commands[i-1].code == 'S')) { + var reflected = reflect(commands[i].x, commands[i].y, commands[i-1].x, commands[i-1].y); + var ctrl = {x:reflected.x, y:reflected.y}; + } + else + var ctrl = {x:currentPoint.x, y:currentPoint.y}; - else if (cmdCode == 'c') { - relCubicCurve(commands[i].x1, commands[i].y1, commands[i].x2, commands[i].y2, commands[i].x, commands[i].y); - } + cubicCurve(ctrl.x, ctrl.y, commands[i].x2, commands[i].y2, commands[i].x, commands[i].y); + break; - //Smooth cubic curve - else if (cmdCode == 'S') { - if (i > 1 && (commands[i-1].code == 's' || commands[i-1].code == 'c' || commands[i-1].code == 'C' || commands[i-1].code == 'S')) { - var reflected = reflect(commands[i].x, commands[i].y, commands[i-1].x, commands[i-1].y); - var ctrl = {x:reflected.x, y:reflected.y}; - } - else - var ctrl = {x:currentPoint.x, y:currentPoint.y}; - - cubicCurve(ctrl.x, ctrl.y, commands[i].x2, commands[i].y2, commands[i].x, commands[i].y); - } + //Smooth relative cubic curve + case 's': + if (i > 1 && (commands[i-1].code == 's' || commands[i-1].code == 'c' || commands[i-1].code == 'C' || commands[i-1].code == 'S')) { + var reflected = reflect(commands[i].x, commands[i].y, commands[i-1].x, commands[i-1].y); + var ctrl = {x:reflect(commands[i-1].x2).x, y:reflect(commands[i-1].y2).y}; + } + else + var ctrl = {x:currentPoint.x, y:currentPoint.y}; - //Smooth relative cubic curve - else if (cmdCode == 's') { - if (i > 1 && (commands[i-1].code == 's' || commands[i-1].code == 'c' || commands[i-1].code == 'C' || commands[i-1].code == 'S')) { - var reflected = reflect(commands[i].x, commands[i].y, commands[i-1].x, commands[i-1].y); - var ctrl = {x:reflect(commands[i-1].x2).x, y:reflect(commands[i-1].y2).y}; - } - else - var ctrl = {x:currentPoint.x, y:currentPoint.y}; + relCubicCurve(ctrl.x, ctrl.y, commands[i].x2, commands[i].y2, commands[i].x, commands[i].y); + break; - relCubicCurve(ctrl.x, ctrl.y, commands[i].x2, commands[i].y2, commands[i].x, commands[i].y); - } + case 'Q': + quadraticCurve(commands[i].x1, commands[i].y1, commands[i].x, commands[i].y); + break; - else if (cmdCode == 'Q') { - quadraticCurve(commands[i].x1, commands[i].y1, commands[i].x, commands[i].y); - } + case 'q': + relQuadraticCurve(commands[i].x1, commands[i].y1, commands[i].x, commands[i].y); + break; - else if (cmdCode == 'q') { - relQuadraticCurve(commands[i].x1, commands[i].y1, commands[i].x, commands[i].y); - } - - //Smooth quadratic curve - else if (cmdCode == 'T') { - if (i > 1 && (commands[i-1].code == 't' || commands[i-1].code == 'q' || commands[i-1].code == 'Q' || commands[i-1].code == 'T')) { - var reflected = reflect(commands[i].x, commands[i].y, commands[i-1].x, commands[i-1].y); - var ctrl = {x:reflect(commands[i-1].x1).x, y:reflect(commands[i-1].y1).y}; - } - else - var ctrl = {x:currentPoint.x, y:currentPoint.y}; + //Smooth quadratic curve + case 'T': + if (i > 1 && (commands[i-1].code == 't' || commands[i-1].code == 'q' || commands[i-1].code == 'Q' || commands[i-1].code == 'T')) { + var reflected = reflect(commands[i].x, commands[i].y, commands[i-1].x, commands[i-1].y); + var ctrl = {x:reflect(commands[i-1].x1).x, y:reflect(commands[i-1].y1).y}; + } + else + var ctrl = {x:currentPoint.x, y:currentPoint.y}; - quadraticCurve(ctrl.x, ctrl.y, commands[i].x, commands[i].y); - } + quadraticCurve(ctrl.x, ctrl.y, commands[i].x, commands[i].y); + break; - //Smooth relative quadratic curve - else if (cmdCode == 't') { - if (i > 1 && (commands[i-1].code == 't' || commands[i-1].code == 'q' || commands[i-1].code == 'Q' || commands[i-1].code == 'T')) { - var reflected = reflect(commands[i].x, commands[i].y, commands[i-1].x, commands[i-1].y); - var ctrl = {x:reflect(commands[i-1].x1).x, y:reflect(commands[i-1].y1).y}; + //Smooth relative quadratic curve + case 't': + if (i > 1 && (commands[i-1].code == 't' || commands[i-1].code == 'q' || commands[i-1].code == 'Q' || commands[i-1].code == 'T')) { + var reflected = reflect(commands[i].x, commands[i].y, commands[i-1].x, commands[i-1].y); + var ctrl = {x:reflect(commands[i-1].x1).x, y:reflect(commands[i-1].y1).y}; + } + else + var ctrl = {x:currentPoint.x, y:currentPoint.y}; + + relQuadraticCurve(ctrl.x, ctrl.y, commands[i].x, commands[i].y); + break; + + case 'A': + arc(commands[i].rx, commands[i].ry, commands[i].xAxisRotation, commands[i].largeArc, commands[i].sweep, commands[i].x, commands[i].y); + break; + + case 'a': + relArc(commands[i].rx, commands[i].ry, commands[i].xAxisRotation, commands[i].largeArc, commands[i].sweep, commands[i].x, commands[i].y); + break; + + case 'Z': + line(firstPoint.x, firstPoint.y); + firstPoint = null; + break; + + case 'z': + line(firstPoint.x, firstPoint.y); + firstPoint = null; + break; } - else - var ctrl = {x:currentPoint.x, y:currentPoint.y}; - - relQuadraticCurve(ctrl.x, ctrl.y, commands[i].x, commands[i].y); - } - - else if (cmdCode == 'A') - arc(commands[i].rx, commands[i].ry, commands[i].xAxisRotation, commands[i].largeArc, commands[i].sweep, commands[i].x, commands[i].y); - - else if (cmdCode == 'a') - relArc(commands[i].rx, commands[i].ry, commands[i].xAxisRotation, commands[i].largeArc, commands[i].sweep, commands[i].x, commands[i].y); - - else if (cmdCode == 'Z' || cmdCode == 'z') { - line(firstPoint.x, firstPoint.y); - firstPoint = null; } } -} - -SVGReader.prototype.loadFont = function(filePath) { - try { - parseString(fs.readFileSync(filePath, "utf8"), function(err, result) { - parsed = JSON.stringify(result, null, 1); - loaded = true; - }); - } catch (e) { - if (e.code === "ENOENT") - console.log("File not found."); - else - throw e; - - return; //If the file is not found stop execution - } - - //Parse the JSON string into an array - fontFile = JSON.parse(parsed); - - //Extract width and height data from the drawing - var svgDimensions = dimensionConversion(fontFile.svg.$.width, fontFile.svg.$.height); - width = svgDimensions.width; - height = svgDimensions.height; - - fontFile = fontFile.svg.g[0].path; - //svg.loadFont("C:/Projects/Tapsterbot/software/src/fontFile.svg") -} //Creates a working clock -//To-do: Add support for user-specified fonts -SVGReader.prototype.clock = function(filePath) { +SVGReader.prototype.clock = function() { var dimensions = dimensionConversion("80mm", "95mm"); //Since no dimensions are specified, assume the default //To-do: Pull this from a config file width = dimensions.width; height = dimensions.height; - if (filePath) - this.loadFont(filePath); - else { - console.log("Font not specified. Using default font."); - loaded = false; - } - objRef = this; resetTimer(); @@ -631,28 +602,19 @@ SVGReader.prototype.clock = function(filePath) { halfway = {x:width / 2, y:height / 2}; currentPoint = {x:halfway.x, y:halfway.y}; //Start at the center of the canvas, which corresponds to (0,0) on the Tapster - if (connected) { - for (var i = 0; i < fontFile.length; i++) { - pathData[fontFile[i].$.id] == fontFile[i].$.d; - } - } - - else { - //Currently hardcoded path data - //To-do: Allow users to specify fonts - var zero = "M 40.890286,241.44557 30.687127,245.57254 23.885021,257.95342 20.483968,278.58823 20.483968,290.96912 23.885021,311.60393 30.687127,323.98482 40.890286,328.11178 47.692392,328.11178 57.895551,323.98482 64.697657,311.60393 68.09871,290.96912 68.09871,278.58823 64.697657,257.95342 57.895551,245.57254 47.692392,241.44557 40.890286,241.44557"; - var one = "M 50.289453,243.07635 50.289453,329.52761"; - var two = "M 27.618803,262.08031 27.618803,257.95271 30.945521,249.69749 34.27224,245.56989 40.925676,241.44228 54.23255,241.44228 60.885986,245.56989 64.212704,249.69749 67.539423,257.95271 67.539423,266.20792 64.212704,274.46313 57.559268,286.84595 24.292085,328.12202 70.866141,328.12202"; - var three = "M 33.809127,248.8729 C 45.779077,241.87747 46.887418,243.5556 53.186196,243.05913 61.663816,242.39092 65.286579,246.55715 64.812967,252.72615 64.140416,261.48642 55.207358,274.28536 40.185777,283.01419 L 52.623884,281.94331 59.295192,285.47169 62.630846,289.00007 65.9665,299.58523 65.9665,306.64193 62.630846,317.22713 55.959538,324.28383 45.952576,327.81223 35.945614,327.81223 25.938652,324.28383 22.602998,320.75553 19.267344,313.69873"; - var four = "M 54.845427,241.51709 22.803997,300.19076 70.866142,300.19076 M 54.845427,241.51709 54.845427,329.5276"; - var five = "M 61.700462,246.77512 26.654546,246.77512 23.149954,281.56734 26.654546,277.70154 37.168321,273.83574 47.682096,273.83574 58.19587,277.70154 65.205054,285.43315 68.709645,297.03055 68.709645,304.76216 65.205054,316.35956 58.19587,324.09117 47.682096,327.95697 37.168321,327.95697 26.654546,324.09117 23.149954,320.22537 19.645363,312.49376"; - var six = "M 64.294901,253.7887 60.65789,245.53396 49.746856,241.40659 42.472833,241.40659 31.561799,245.53396 24.287776,257.91607 20.650765,278.55291 20.650765,299.18976 24.287776,315.69923 31.561799,323.95397 42.472833,328.08134 46.109844,328.08134 57.020878,323.95397 64.294901,315.69923 67.931912,303.31713 67.931912,299.18976 64.294901,286.80765 57.020878,278.55291 46.109844,274.42554 42.472833,274.42554 31.561799,278.55291 24.287776,286.80765 20.650765,299.18976"; - var seven = "M 68.262659,241.73033 32.158284,328.90301 M 17.716535,241.73033 68.262659,241.73033"; - var eight = "M 37.489233,241.44557 27.286074,245.57254 23.885021,253.82646 23.885021,262.08039 27.286074,270.33431 34.08818,274.46127 47.692392,278.58823 57.895551,282.7152 64.697657,290.96912 68.09871,299.22304 68.09871,311.60393 64.697657,319.85785 61.296604,323.98482 51.093445,328.11178 37.489233,328.11178 27.286074,323.98482 23.885021,319.85785 20.483968,311.60393 20.483968,299.22304 23.885021,290.96912 30.687127,282.7152 40.890286,278.58823 54.494498,274.46127 61.296604,270.33431 64.697657,262.08039 64.697657,253.82646 61.296604,245.57254 51.093445,241.44557 37.489233,241.44557"; - var nine = "M 67.931912,270.29818 64.2949,282.68028 57.020878,290.93502 46.109844,295.06239 42.472833,295.06239 31.561799,290.93502 24.287776,282.68028 20.650765,270.29818 20.650765,266.17081 24.287776,253.7887 31.561799,245.53396 42.472833,241.40659 46.109844,241.40659 57.020878,245.53396 64.2949,253.7887 67.931912,270.29818 67.931912,290.93502 64.2949,311.57186 57.020878,323.95397 46.109844,328.08134 38.835821,328.08134 27.924787,323.95397 24.287776,315.69923"; - - pathData = [zero, one, two, three, four, five, six, seven, eight, nine]; - } + //Currently hardcoded path data + var zero = "M 40.890286,241.44557 30.687127,245.57254 23.885021,257.95342 20.483968,278.58823 20.483968,290.96912 23.885021,311.60393 30.687127,323.98482 40.890286,328.11178 47.692392,328.11178 57.895551,323.98482 64.697657,311.60393 68.09871,290.96912 68.09871,278.58823 64.697657,257.95342 57.895551,245.57254 47.692392,241.44557 40.890286,241.44557"; + var one = "M 50.289453,243.07635 50.289453,329.52761"; + var two = "M 27.618803,262.08031 27.618803,257.95271 30.945521,249.69749 34.27224,245.56989 40.925676,241.44228 54.23255,241.44228 60.885986,245.56989 64.212704,249.69749 67.539423,257.95271 67.539423,266.20792 64.212704,274.46313 57.559268,286.84595 24.292085,328.12202 70.866141,328.12202"; + var three = "M 33.809127,248.8729 C 45.779077,241.87747 46.887418,243.5556 53.186196,243.05913 61.663816,242.39092 65.286579,246.55715 64.812967,252.72615 64.140416,261.48642 55.207358,274.28536 40.185777,283.01419 L 52.623884,281.94331 59.295192,285.47169 62.630846,289.00007 65.9665,299.58523 65.9665,306.64193 62.630846,317.22713 55.959538,324.28383 45.952576,327.81223 35.945614,327.81223 25.938652,324.28383 22.602998,320.75553 19.267344,313.69873"; + var four = "M 54.845427,241.51709 22.803997,300.19076 70.866142,300.19076 M 54.845427,241.51709 54.845427,329.5276"; + var five = "M 61.700462,246.77512 26.654546,246.77512 23.149954,281.56734 26.654546,277.70154 37.168321,273.83574 47.682096,273.83574 58.19587,277.70154 65.205054,285.43315 68.709645,297.03055 68.709645,304.76216 65.205054,316.35956 58.19587,324.09117 47.682096,327.95697 37.168321,327.95697 26.654546,324.09117 23.149954,320.22537 19.645363,312.49376"; + var six = "M 64.294901,253.7887 60.65789,245.53396 49.746856,241.40659 42.472833,241.40659 31.561799,245.53396 24.287776,257.91607 20.650765,278.55291 20.650765,299.18976 24.287776,315.69923 31.561799,323.95397 42.472833,328.08134 46.109844,328.08134 57.020878,323.95397 64.294901,315.69923 67.931912,303.31713 67.931912,299.18976 64.294901,286.80765 57.020878,278.55291 46.109844,274.42554 42.472833,274.42554 31.561799,278.55291 24.287776,286.80765 20.650765,299.18976"; + var seven = "M 68.262659,241.73033 32.158284,328.90301 M 17.716535,241.73033 68.262659,241.73033"; + var eight = "M 37.489233,241.44557 27.286074,245.57254 23.885021,253.82646 23.885021,262.08039 27.286074,270.33431 34.08818,274.46127 47.692392,278.58823 57.895551,282.7152 64.697657,290.96912 68.09871,299.22304 68.09871,311.60393 64.697657,319.85785 61.296604,323.98482 51.093445,328.11178 37.489233,328.11178 27.286074,323.98482 23.885021,319.85785 20.483968,311.60393 20.483968,299.22304 23.885021,290.96912 30.687127,282.7152 40.890286,278.58823 54.494498,274.46127 61.296604,270.33431 64.697657,262.08039 64.697657,253.82646 61.296604,245.57254 51.093445,241.44557 37.489233,241.44557"; + var nine = "M 67.931912,270.29818 64.2949,282.68028 57.020878,290.93502 46.109844,295.06239 42.472833,295.06239 31.561799,290.93502 24.287776,282.68028 20.650765,270.29818 20.650765,266.17081 24.287776,253.7887 31.561799,245.53396 42.472833,241.40659 46.109844,241.40659 57.020878,245.53396 64.2949,253.7887 67.931912,270.29818 67.931912,290.93502 64.2949,311.57186 57.020878,323.95397 46.109844,328.08134 38.835821,328.08134 27.924787,323.95397 24.287776,315.69923"; + + pathData = [zero, one, two, three, four, five, six, seven, eight, nine]; var colon = "M 165.73227,300.21546 Z M 165.73227,253.34648 Z"; var arrayTime = new Array(); From c57b4276891dfedbc794b352cd3824297a228f0a Mon Sep 17 00:00:00 2001 From: Jack Skalitzky Date: Tue, 28 Jul 2015 14:19:40 -0500 Subject: [PATCH 43/43] Misc. clean up --- software/src/draw.js | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/software/src/draw.js b/software/src/draw.js index 8b6e025..066eb05 100644 --- a/software/src/draw.js +++ b/software/src/draw.js @@ -222,7 +222,7 @@ Draw.prototype.drawCircle = function(args) { //radius: radius of largest spiral (default: 30) //zLevel: height at which to draw the spiral (default: penHeight) //ptArray: an array of points to draw instead of calculating the spiral again (default: null) -Draw.prototype.drawSpiral = function(args) { //spirals, radius, zLevel, ptArray) { +Draw.prototype.drawSpiral = function(args) { this.startX = 0; this.startY = 0;