Software
processing code for the conversion from the image file to the pen tip path
import toxi.geom.*;
// tsp variables
int particleRouteLength;
Vec2D[] particles;
int[] particleRoute;
int maxParticles;
// image variable
PImage img;
float millisLastFrame = 0;
float frameTime = 0;
// scale of the drawing
float s = 2.0;
void setup() {
maxParticles = 15000;
//img = loadImage("lenna-lg_BW_loRes.png");
//img = loadImage("test.png");
img = loadImage("lenna_BW_loRes2.png");
size(img.width*(int)s, img.height*(int)s);
//size(400, 600);
// count black pixels
int i;
maxParticles = 0;
for ( int x = 0; x < img.width; x++ ) {
for ( int y = 0; y < img.height; y++ ) {
i = ( ( y * img.width ) + x ); // getting pixel index
if ( img.pixels[i] == color( 0, 0, 0 ) ) {
maxParticles++;
}
}
}
println("black dots: " + maxParticles);
// allocate and fill points vector
particles = new Vec2D[maxParticles];
i = 0;
int j = 0;
for ( int x = 0; x < img.width; x++ ) {
for ( int y = 0; y < img.height; y++ ) {
i = ( ( y * img.width ) + x );
if ( img.pixels[i] == color( 0, 0, 0 ) ) {
Vec2D p1 = new Vec2D(x, y);
particles[j] = p1;
j++;
}
}
}
millisLastFrame = millis();
initPath(); // initialize path (NN heuristic)
for (int l = 0; l < 5; l++ ) {
// optimize path with 2-opt heuristic
for (int k = 0; k < 5000; k++ ) optimizePath();
// profiling ...
frameTime = (millis() - millisLastFrame)/1000;
millisLastFrame = millis();
println("Frame time: " + millisLastFrame);
}
noLoop();
}
void initPath()
{
int temp;
println("initializing path (NN)");
Vec2D p1, p2;
particleRouteLength = maxParticles;
// array of free ramaining particles to be queried
boolean freeParticles[] = new boolean[maxParticles];
particleRoute = new int[particleRouteLength];
int closestParticle;
float distMin;
p1 = particles[0];
freeParticles[0] = true;
particleRoute[0] = 0;
// Nearest neighbor ("Simple, Greedy") algorithm path optimization:
int i = 0, j;
float dx, dy, distance;
while (i < particleRouteLength) {
distMin = Float.MAX_VALUE; // re-initialize mimimun distance value
closestParticle = 0; // re-initialize closest particle
for (j = 0; j < particleRouteLength; j++) {
if (freeParticles[j] == false) {
p2 = particles[j]; // get next particle to calculate distance
dx = p1.x - p2.x;
dy = p1.y - p2.y;
distance = (float) (dx*dx+dy*dy); // Only looking for closest; do not need sqrt factor!
if (distance < distMin) {
closestParticle = j; // update the closest particle index
distMin = distance; // update the minimum distance value
}
}
}
freeParticles[closestParticle] = true; // remove the particle from the ones to be queried
particleRoute[i] = closestParticle; //set the next particle in the path
i++; // increment while counter
}
// Initial routing is complete
frameTime = (millis() - millisLastFrame)/1000;
millisLastFrame = millis();
println("Frame time: " + millisLastFrame);
}
void optimizePath() {
// 2-opt heuristic optimization:
// Identify a pair of edges that would become shorter by reversing part of the tour.
int temp;
//println("optimizing path (2-opt) " );
for (int i = 0; i < 5000; ++i) { // 1000 tests per frame; you can edit this number.
int indexA = floor(random(particleRouteLength - 1));
int indexB = floor(random(particleRouteLength - 1));
if (Math.abs(indexA - indexB) < 2)
continue;
if (indexB < indexA) { // swap A, B.
temp = indexB;
indexB = indexA;
indexA = temp;
}
Vec2D a0 = particles[particleRoute[indexA]];
Vec2D a1 = particles[particleRoute[indexA + 1]];
Vec2D b0 = particles[particleRoute[indexB]];
Vec2D b1 = particles[particleRoute[indexB + 1]];
// Original distance:
float dx = a0.x - a1.x;
float dy = a0.y - a1.y;
float distance = (float) (dx*dx+dy*dy); // only a comparison; do not need sqrt factor!
dx = b0.x - b1.x;
dy = b0.y - b1.y;
distance += (float) (dx*dx+dy*dy); // only a comparison; do not need sqrt factor!
// Possible shorter distance?
dx = a0.x - b0.x;
dy = a0.y - b0.y;
float distance2 = (float) (dx*dx+dy*dy); // only a comparison; do not need sqrt factor!
dx = a1.x - b1.x;
dy = a1.y - b1.y;
distance2 += (float) (dx*dx+dy*dy); // only a comparison; do not need sqrt factor!
if (distance2 < distance) { // Reverse tour between a1 and b0.
int indexhigh = indexB;
int indexlow = indexA + 1;
while (indexhigh > indexlow) {
temp = particleRoute[indexlow];
particleRoute[indexlow] = particleRoute[indexhigh];
particleRoute[indexhigh] = temp;
indexhigh--;
indexlow++;
}
}
}
}
void draw() {
//image(img, 0, 0);
image(img, width*s, height*s);
int i = 0;
stroke(128, 128, 255); // Stroke color (blue)
strokeWeight (.5); // stroke weight
println("in draw, n.part : " + particleRouteLength);
// loop the particles drawing a line between successive points
for ( i = 0; i < (particleRouteLength - 1); ++i) {
Vec2D p1 = particles[particleRoute[i]];
Vec2D p2 = particles[particleRoute[i + 1]];
line(p1.x*s, p1.y*s, p2.x*s, p2.y*s);
}
}
preliminary python code for the low level controller
from time import sleep
from math import pi
import RPi.GPIO as GPIO
- systems parameters
r_p = 18.0 #............... pulley radius [mm]
d_p = 1500.0 #............. pulley distance [mm]
d_p05 = dp * 0.5 #......... half pulley distance [mm]
s_a = 3.5 * (2*pi/360) #... step angle [rad]
- drawing parameters
s = 2.0 #.................. drawing scale [-]
- initialize output pins
GPIO.setup(13, GPIO.OUT)
- GPIO.setup(15, GPIO.OUT)
- GPIO.setup(16, GPIO.OUT)
- GPIO.setup(15, GPIO.OUT)
- GPIO.setup(16, GPIO.OUT)
- GPIO.setup(15, GPIO.OUT)
- GPIO.setup(16, GPIO.OUT)
- GPIO.setup(15, GPIO.OUT)
pos = [240, 240] #.... set initial position vector (x,Y)
len_curr = getStringsLen(pos_init, d_p05, s) # get initial string
for i in list
pos_next = path[i]
len_next = getStringsLen(pos_next, d_p05, s)
dl = len_next - len_curr
ds = dl/s_a
def getStringsLen(pos_xy, halfPullDist, scale)
x2 = (pos_xy[0] * scale)**2
x2b2 = (halfPullDist - pos_xy[0] * scale)**2
y2 = (pos_xy[1] * scale)**2
return [sqrt(x2+y2) , sqrt(x2b2+y2)]
