Difference between revisions of "Software"

From FabLabGenovaWiki
Jump to: navigation, search
(→‎Processing code)
Line 251: Line 251:
 
#        if clockStart - clock() > m2Interval * intMot2:
 
#        if clockStart - clock() > m2Interval * intMot2:
 
#            #setp motor 2
 
#            #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"
 +
 +
# this really move the motor operating on GPIO (note: doing only once)
 +
def stepMotor(motorId,direction):
 +
print "stepping motor"+str(motorId)
 +
 +
#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
 +
        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>
 
</source>

Revision as of 00:50, 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:

  1. Install Processing
  2. Download and Install toxiclib
  3. 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

  1. import RPi.GPIO as GPIO

from numpy import loadtxt, pi, sqrt, subtract

  1. load path file

path = loadtxt("path.txt")

  1. print path
  1. 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]

  1. drawing parameters

s = 2.0 #.................. drawing scale [-]

  1. 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
  1. ___TODO___ debug from here
  2. i = 0 #........................................ init while loop
  3. m1Interval = ?? #.............................. compute time interval for stepping motor 1
  4. m2Interval = ?? #.............................. compute time interval for stepping motor 2
  5. intMot1 = 0 #.................................. motor 1 step counter
  6. intMot2 = 0 #.................................. motor 2 step counter
  7. clockStart = clock() #......................... initialize clock for stepping the motors
  8. while 1:
  9. i = i + 1
  10. if clockStart - clock() > m1Interval * intMot1:
  11. #setp motor 1
  12. if clockStart - clock() > m2Interval * intMot2:
  13. #setp motor 2

</source>

nightly Python code

DO NOT RUN if you care your cpu -> is an infinite loop <source lang="python">

  1. 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

  1. end of system parameters.
              1. DO NOT EDIT BELOW THIS LINE ##########
  1. packages import

from time import sleep, clock

  1. import RPi.GPIO as GPIO

from numpy import loadtxt, pi, sqrt, subtract, median

  1. function definitions #
  1. 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)]
  1. enable motors

def enableMotors(): print "enabling motors"

  1. this really move the motor operating on GPIO (note: doing only once)

def stepMotor(motorId,direction): print "stepping motor"+str(motorId)

  1. 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
       M1TravelTime = M1requestedStep*defaultStepInterval
       M2TravelTime = M2requestedStep*defaultStepInterval
       RealTravelInterval = median([M1TravelTime,M2TravelTime])

#initialize then...

  1. 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:

  1. 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

  1. if(i > 100000): run = False # this is a timeout
  1. 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('.')

  1. print str(i)+" give: "+ RequestedStepMotor1+" "+RequestedStepMotor2

moveMotor(RequestedStepMotor1,RequestedStepMotor2) # _TODO_ double check that this for cicle wait moveMotor()

except (KeyboardInterrupt, SystemExit): 

       print " exiting..."
  1. GPIO.cleanup()
  2. print "\ncleaning GPIO..."
       raise

except: 

       print "\n errori nel codice!!!"

</source>

Retrieved from "https://fablabgenova.it/index.php?title=Software&oldid=92"