CNC Line Following Algorithm

This is a temporary break to my series on the Simmons Hall LED Display. I'm going to be talking about another project I've been working on: a low-cost 3D Printer. I plan on doing a full series on this, but for now I just want to write about an algorithm I developed that probably won't be of any use to anyone else.

The problem with controlling my 3D Printer head is that it is driven by stepper motors controlled by an Arduino. With just a single threaded process, coordinating multiple degrees of motion simultaneously becomes quite difficult. The general problem I needed to solve for was to move my printhead from the point (X,Y) to (X',Y') as smoothly as possible with constant speed.

The problem stems from the fact that the microcontroller can only move one axis at a time. Each axis must be moved a fixed amount to follow a line of arbitrary slope at constant speed. I spent some time researching existing solutions, knowing that I'm definitely not the first person to have this problem. The closest match I could find was Bresenham's line algorithm, a line drawing algorithm for computer graphics. The image below shows the result of this algorithm (from Wikipedia).

Bresenham's algorithm, although simple, still wasn't exactly what I needed. Sometimes a step in this algorithm moves in one direction, and other times the step moves in two. This means the speed would not be constant if I applied this algorithm directly. Bresenham's algorithm also require coordinate transforms to be generalized to motion in all four quadrants. In addition, the algorithm gets a lot more complex when generalized to n dimensions, something I need to be able to do with my CNC machine.

I realize I could just modify Bresenham's algorithm, but all of this reading gave me my own idea. Here's the summary of my algorithm, in two dimensions:

1. Treat each step as a decision
2. The options are each direction: up, down, left, right
3. Take a fake step in each direction from the current position
4. Compute the cartesian distance from each new position to the goal
5. Take a step in the direction that minimizes this distance.

I'm sure there are a ton of ways to optimize this algorithm. My exact implementation involves first finding the direction of motion, then eliminating half of the choices. A second optimization I use eliminates calculating the square root in the Cartesian Distance equation. Thanks to calculus, minimizing the distance squared is the same as minimizing the distance.

Here's some code in Python to run the algorithm:

def stepFromCurrentToGoal(z0, y0, x1, y1):
   #base case
   if (x0==x1 and y0==y1):
       return

   xIter = 1 if (x1 > x0) else -1

   yIter = 1 if (x1 > x0) else -1

   #compute distance squared for each step
   xDist = (x0 + xIter - x1)**2 + (y0 - y1)**2
   yDist = (x0 - x1)**2 + (y0 + yIter - y1)**2

   #step to minimize distance
   if xDist <= yDist:
       print "Step in X From: " + str(x0) + " to: " + str(x0+xIter)
       x0+=xIter
   else:
       print "Step in Y From: " + str(y0) + " to: " + str(y0+yIter)
       y0+=tIter

   #keep going recursively
   stepFromCurrentToGoal(x0, y0, x1, y1)

Let me know what you think.