Difference between revisions of "Software"
(→nightly Python code) |
|||
| Line 256: | Line 256: | ||
'''DO NOT RUN if you care your cpu -> is an infinite loop''' | '''DO NOT RUN if you care your cpu -> is an infinite loop''' | ||
<source lang="python"> | <source lang="python"> | ||
| − | |||
# system parameters. Tune them following your system | # system parameters. Tune them following your system | ||
PulleyRadius = 18.0 #................. pulley radius [mm] | PulleyRadius = 18.0 #................. pulley radius [mm] | ||
| Line 284: | Line 283: | ||
def enableMotors(): | def enableMotors(): | ||
print "enabling motors" | print "enabling motors" | ||
| + | # here put high pin !enable (15 on PI) | ||
# this really move the motor operating on GPIO (note: doing only once) | # this really move the motor operating on GPIO (note: doing only once) | ||
def stepMotor(motorId,direction): | def stepMotor(motorId,direction): | ||
print "stepping motor"+str(motorId) | print "stepping motor"+str(motorId) | ||
| + | # here put high pin 3 and pulse once pin 4 | ||
#moveMotor definition | #moveMotor definition | ||
| Line 305: | Line 306: | ||
if numStepM2<0: | if numStepM2<0: | ||
M2reverse = True | M2reverse = True | ||
| − | # calculate max spent time stepping at default speed | + | # calculate max spent time stepping at default speed _TODO_ find a less crude way to do that |
M1TravelTime = M1requestedStep*defaultStepInterval | M1TravelTime = M1requestedStep*defaultStepInterval | ||
M2TravelTime = M2requestedStep*defaultStepInterval | M2TravelTime = M2requestedStep*defaultStepInterval | ||
| Line 328: | Line 329: | ||
stepMotor(2,M2reverse) | stepMotor(2,M2reverse) | ||
M2steppedStep = M2steppedStep + 1 | M2steppedStep = M2steppedStep + 1 | ||
| − | # if(i > 100000): run = False # this is a timeout | + | # if(i > 100000): run = False # this is a timeout???? |
# now the part that take txt files and drive motor | # now the part that take txt files and drive motor | ||
Revision as of 00:56, 16 February 2013
Image pre-processing
assuming GIMP:
- Push up the contrast
- Image ==> Mode ==> Indexed
- B/W (1 bit palette)
- Dithering: Floyd-Steinberg
Processing code
for the conversion from the image file to the pen tip path (Last update: Thu 14 Feb 2013, 19.39)
Step by Step:
- Install Processing
- Download and Install toxiclib
- Create a sketch with the code below:
<source lang="javascript"> 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("test_masa.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 < 1000; 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);
String[] lines = new String[particleRouteLength];
for (i = 0; i < particleRouteLength; i++) {
lines[i] = particles[particleRoute[i]].x + " " + particles[particleRoute[i]].y;
}
saveStrings("path.txt", lines);
// 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);
}
} </source>
Python code
for the low level controller
<source lang="python">
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)]
from time import sleep, clock
- import RPi.GPIO as GPIO
from numpy import loadtxt, pi, sqrt, subtract
- load path file
path = loadtxt("path.txt")
- print path
- systems parameters
r_p = 18.0 #............... pulley radius [mm] d_p = 1500.0 #............. pulley distance [mm] d_p05 = d_p * 0.5 #........ half pulley distance [mm] s_a = 3.5 * (2*pi/360) #... step angle [rad]
- drawing parameters
s = 2.0 #.................. drawing scale [-]
- initialize drawing
pos_init = [240, 240] #........................... set initial position vector (x,y) len_curr = getStringsLen(pos_init, d_p05, s) # ..... get initial string
for i in path:
pos_next = i #................................. allocate next position vector len_next = getStringsLen(pos_next, d_p05, s) #. get next strings lengths dl = subtract(len_next, len_curr) #............ get string lengths difference ds = dl/s_a #.................................. compute motor step (same step angles for both motors) print ds
- ___TODO___ debug from here
- i = 0 #........................................ init while loop
- m1Interval = ?? #.............................. compute time interval for stepping motor 1
- m2Interval = ?? #.............................. compute time interval for stepping motor 2
- intMot1 = 0 #.................................. motor 1 step counter
- intMot2 = 0 #.................................. motor 2 step counter
- clockStart = clock() #......................... initialize clock for stepping the motors
- while 1:
- i = i + 1
- if clockStart - clock() > m1Interval * intMot1:
- #setp motor 1
- if clockStart - clock() > m2Interval * intMot2:
- #setp motor 2
</source>
nightly Python code
DO NOT RUN if you care your cpu -> is an infinite loop <source lang="python">
- system parameters. Tune them following your system
PulleyRadius = 18.0 #................. pulley radius [mm] pulleyDistance = 1500.0 #............. pulley distance [mm] stepAngle = 3.5 # .................... step angle [rad] drawingScale = 2.0 #.................. drawing scale fileName = "path.txt" defaultStepInterval = 0.5 # _TODO_ find some way to get this value confy
- end of system parameters.
- DO NOT EDIT BELOW THIS LINE ##########
- packages import
from time import sleep, clock
- import RPi.GPIO as GPIO
from numpy import loadtxt, pi, sqrt, subtract, median
- function definitions #
- getStringLenght definition
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)]
- enable motors
def enableMotors(): print "enabling motors" # here put high pin !enable (15 on PI)
- this really move the motor operating on GPIO (note: doing only once)
def stepMotor(motorId,direction): print "stepping motor"+str(motorId) # here put high pin 3 and pulse once pin 4
- moveMotor definition
def moveMotor(numStepM1, numStepM2): # variables initialization M1Interval = defaultStepInterval # 0.5 #.............................. compute time interval for stepping motor 1 M2Interval = defaultStepInterval # 0.25 #.............................. compute time interval for stepping motor 2 # calculate requested step and direction M1reverse = False M2reverse = False
M1requestedStep = abs(numStepM1)
M2requestedStep = abs(numStepM2)
if M1requestedStep==numStepM1: if numStepM1<0: M1reverse = True if M2requestedStep==numStepM2: if numStepM2<0: M2reverse = True
# calculate max spent time stepping at default speed _TODO_ find a less crude way to do that
M1TravelTime = M1requestedStep*defaultStepInterval
M2TravelTime = M2requestedStep*defaultStepInterval
RealTravelInterval = median([M1TravelTime,M2TravelTime])
#initialize then...
- i = 0 #........................................ init while loop
M1steppedStep = 0 #.............................motor 1 step counter
M2steppedStep = 0 #.............................motor 2 step counter
run = True #................................... while loop run flag
clockStart = clock() # ...run while run:
- i = i +1
if M1requestedStep<M1steppedStep: if clock() - clockStart > RealTravelInterval * M1steppedStep: stepMotor(1,M1reverse) M1steppedStep = M1steppedStep + 1 if M2requestedStep<M2steppedStep: if clock() - clockStart > RealTravelInterval * M2steppedStep: stepMotor(2,M2reverse) M2steppedStep = M2steppedStep + 1
- if(i > 100000): run = False # this is a timeout????
- now the part that take txt files and drive motor
try: while 1==1: # load path file path = loadtxt(fileName)
# convert sys param r_p = PulleyRadius d_p = pulleyDistance d_p05 = d_p * 0.5 #........ half pulley distance [mm] s_a = stepAngle * (2*pi/360) #... step angle [rad] s = drawingScale
# initialize drawing pos_init = [240, 240] #........................... set initial position vector (x,y) len_curr = getStringsLen(pos_init, d_p05, s) # ..... get initial string
# do the drawing for i in path: pos_next = i #................................. allocate next position vector len_next = getStringsLen(pos_next, d_p05, s) #. get next strings lengths dl = subtract(len_next, len_curr) #............ get string lengths difference ds = dl/s_a #.................................. compute motor step (same step angles for both motors) RequestedStepMotor1 = round(ds[0]) # str(round(ds[0])).rstrip('0').rstrip('.') RequestedStepMotor2 = round(ds[1]) # str(round(ds[1])).rstrip('0').rstrip('.')
- print str(i)+" give: "+ RequestedStepMotor1+" "+RequestedStepMotor2
moveMotor(RequestedStepMotor1,RequestedStepMotor2) # _TODO_ double check that this for cicle wait moveMotor()
except (KeyboardInterrupt, SystemExit):
print " exiting..."
- GPIO.cleanup()
- print "\ncleaning GPIO..."
raise
except:
print "\n errori nel codice!!!"
</source>
