Elegant multispline motion controller

Elegant MultiSpline Motion Controller

a rough draft by Brian Korsedal

This is a project to redo the main controller board and motion controller. I am designing a new board based on a Xilinx Spartan-3AN chip.[1] This is a very low cost FPGA with internal flash to store the FPGA program. It should be easy to source for under $9 and comes in a tqfp144 package which is reasonable easy to solder (compared to a bga). This design will include a high speed USB 1.1 interface running at the full 12 MBPS. There will be a small amount of preprocessing which occurs on the desktop PC.

This device will not use G-code. It will use a custom language based on cubic beziers. This allows for much better description of arcs and will result in much higher quality prints with a much lower data throughput requirements. It is easy to write an application to translate from G-code to the custom bezier language. This will be the flow to support backwards compatibility.

Algorithm Explanation

The basis of the motion profile is cubic Bezier curves. These curves are easy to specify and can accurately describe toolpaths which would be a nightmare to describe in Gcode.

(insert example here)

Above is an example of a cubic Bezier. They are specified by four points. Point 1 is the start point. Point 2 is a control point. It is synonymous with the velocity vector at the start of the curve Point 1. Control point 3 is synonymous with the inverse of the velocity vector at the end of the curve. Point 4 is the end point of the curve. The magnitude of the velocity vectors is specified in terms of the curve parameter.

Bezier curves are defined by a curve parameter. This parameter ranges from 0 to 1. It describes where you are located along the curve. This parameter is usually called u. There is no simple relationship between u and S (the position in the x,y,z). This causes a big problem when plotting motion along this curve.

A key advantage of cubic beziers is that they describe jerk limited motion. The first derivative (velocity) of a cubic Bezier curve (dS/du) is a quadratic Bezier. The second derivative (acceleration) is a straight line. The third derivative (jerk) is a constant. This naturally creates the desired motion profiles while still being easy to compute. It might be possible to further simplify the computations using quadratic Beziers instead of cubic Beziers. However, I am unsure if the quadratic Beziers will describe all the required toolpaths accurately. There are also interesting papers on Biarc specifications or higher orders. This can be determined by whomever continues my work. “A good plan violently executed now is better than a perfect plan executed next week.” General George S. Patton.

As was noted earlier there is no simple relationship between u and S (position in x,y,z). This creates a large problem. The way I solve it is to use numerical integration and parallel computing. FPGA's are masters of parallel computing. Even low cost FPGA's will crush most DSP and processors in raw power. In the past FPGA's have been difficult to program. Fortunately there are new tools on the market which dramatically reduce the effort needed to program FPGA's. These new tools are based on Matlab and Simulink. They allow an exponential leap in productivity and allow engineers to keep pace with Moore's law. This design was implemented using Xilinx System Generator. This design was created from scratch in under three weeks. It is a good example of the exponential leap in productivity that these new tools create.

The curves are subdivided into straight line segments. They are subdivided by 2^16 subsections. This motion controller can process about 96e6/21 subsections per second. The approximate maximum velocity of a reprap is 40mm per second. It would require 69.7 arcs with a average arc length of 0.57344 mm at maximum tool travel speed to overpower the computational limits of this motion controller. These settings will be considered user error. Most arcs should be over a mm in length. The maximum arc length possible on a reprap would be on the order of 300 mm. These large arcs may cause a slight fluctuation in the velocity profile but it should be unnoticeable to the user.

The motion controller produces points using two passes of the bezier engine. The first pass creates a rough speed profile. This profile is then filtered and interpolated to create a very smooth speed profile. This smooth speed profile is passed through the bezier engine a second time to produce the actual points. These points are translated into pulses which go into the stepper driver IC's.

The details of the algorithm are tightly intertwined with the FPGA implementation. They will be described in the section detailing the FPGA implementation.

Cubic Bezier Motion Language (*.cbml)

The input into this design is not G code. The input is a series of points which describe cubic Bezier curves and other control information. This file should be computer generated. The prepossessing requirements for this file are described after the commands are detailed. This language is not meant for human programming. The language has the following commands:

Set = [tempa,tempb];

This sets two variables associated with PDM temperature controls.

Pause = [x,y,z,t,exta,extb,minv,adst,maxv];

Pause is a generic command used to configure parameters. It creates a discontinuity at [x,y,z] with a minv of 0. When the toolhead is very close to x,y,z and moving very slow it will be paused for t sample times. Then the minv is put back to normal and the toolhead accelerates along the path. This command can be used to change maxv. Performing a maxv command followed by a Pause command will change the maxv. It is also useful for extruding. This will allow the machine to move fast while not extruding. Come to a stop and start extruding while moving at a slower rate. It is valid to set t=0. It is also valid to set t = 'tempa', 'tempb'. This will pause the machine until the A/B/C PDM loops achieve a steady state. The parameters exta and extb are boolean variables which turn extruders on or off. Extruder rates are determined inside the FPGA by the distance the toolhead travels. Current devices only have exta. Extb is included for future expansion. The parameter minv is the minimum velocity at which the machine will step. The machine never stops. It will slow down to the minv as it approaches the discontinuity. It will never go below the minimum velocity. If this is set too low the machine might get stuck around discontinuities. If this is too high the machine will jerk at discontinuities. This is the maximum velocity at which the toolhead will travel. This velocity calculation takes into effect the velocity in the x,y and z directions. Adst is the distance required to transition from maxv to minv or minv to maxv. The speed profile is generated using a quadratic bezier curve. Hence the Multispline name or this architecture. The variable which determines our current position along the speed curve is determined by the distance to the next discontinuity/adst. This creates a natural slowing down as the toolhead approaches a discontinuity and a natural acceleration away from a discontinuity. This controller uses the straight line distance to the next discontinuity. This results in some error in the speed profile but dramatically simplifies the architecture. The error is acceptable for our application. This error can be eliminated using a look ahead buffer and other more complex systems. They are not implemented in this design due to the size constraints of our low cost FPGA.

Cbez = [ x1,x2,x3,x4, y1,y2,y3,y4, z1,z2,z3,z4, e1,e2,e3,e4, sDsctEn,sDsctX,sDsctY,sDsctZ,sDsctAdst,sDsctMinV,sDsctMaxV, eDsctEn,eDsctX,eDsctY,eDsctZ,eDsctAdst,eDsctMinV,eDsctMaxV];

This specifies a Cubic Bezier curve in the X,Y,Z space. The first 12 numbers specify the control points for this arc. The next four parameters specify the extrusion profile. This is the thickness of the extruded line with respect to S the curve parameter. The bezier engine records the distance the toolhead travels. The is multilied by the Extrusion profile to generate the control signals for the extruder. 'sDsct' stands for start discontinuity. It is used to generate the speed profile in the beginning of the curve due to a discontinuity in curvature or velocity at the transition from one bezier to the next. If there is a discontinuity then sDsctEn will equal 1 otherwise this discontinuity will be ignored. Small discontinuities are acceptable. Large discontinuities require the toolhead to reduce speed along the curve before or after the discontinuity. The x,y,z values is the position of this discontinuity. The Adst, MinV and MaxV describe the speed profile for this discontinuity. 'eDsct' stands for end discontinuity. It specifies a potential discontinuity at the end of the curve.

There is a further section defining the specification these cubic bezier curves must follow for error free motion and examples of some situations which will cause problems for the machine.

Preprocessing requirements

The cubic Bezier curves must be C2 continuous at the transition points. This means that the curvature at the transition points is continuous. Any discontinuities in the curvature will cause discontinuities in the speed profile. Small discontinuities are acceptable. The speed at which the toolhead can traverse the curve is related to the maximum acceptable acceleration on the machine (Amax) and the mass of the toolhead. The force generated by the curvature of the curve is the instantanious curvature multiplied by the current toolhead velocity ^ 2. To find the maximum velocity we need to set these two equations equal to each other.

Mass*Amax = curvature * VmaxCurve^2 VmaxCurve = sqrt(Mass*Amax/curvature)

As long as the VmaxCurve is not less than the Vmax already defined in the motion profile we do not need to slow down for the discontinuity. Our speed profile filtering will smooth out the discontinuity.

FPGA implimentation and theory

The FPGA will be split into several parts.

AWGN Pulse Density Modulator

The MOSFETs for the heaters are driven by a Pulse Density Modulator. The density of my modulator is driven by probability. There is a psuedo-random number generator in the design. It compares a this random number with the setting for the DC voltage level. This creates a random high or low at the gate of the mosfet. The probability of this output is driven by the DC voltage setting. The higher the setting inside the FPGA, the higher the FPGA will drive the gate high.

The psuedo-random number generator is a linear shift register. Theory can be found on Wikipedia. In this configuration it will generate a repeating sequence of 2^16-1 numbers. None of the numbers will be repeated in the sequence.

The update rate of the linear shift register is driven by a speed parameter. It is configured so that the update rate is equal to the FPGA clock rate divided by (128 - speed). This parameter will be determined based upon the switching speed of the MOSFET. Higher update rates on the gate will create less noise. If the setting is too high it might cause issues with heat or problems with duty cycle on the MOSFET.

The advantage of this type of modulation is a decrease in spurious noise. PWM is a square wave. This creates very high frequencies at the harmonics of the square wave.

[[2]]

Above you can see a block diagram for the AWGN Pulse Density Modulator. These next two plots show the output of the block. The top signal is a square wave with a 25% duty cycle. The second signal is the output of the AWGN PDM with a duty cycle of 25%. The next two signals are the results of a low pass filter. The low pass filter in the real Reprap is the nicrome wire or whatever is being heated. This is a very low pass filter.

[[3]]

[[4]]

Next is a comparison of the spectrum of the two waves. They both have a sharp peak at DC showing the 25% duty cycle. The AWGN PWM drops off to a noise floor about 20 dB down from the peak. The square wave drops down sharply but then spikes dramatically at the harmonics of the square wave.

[[5]]

[[6]]

I2C Audio ADC interface

USB 1.1 interface

AB FIFO

MultiSpline Engine

Solving problems in FPGA fabric is a much different approach then doing the same problem in a DSP or microprocessor. For many applications it can dramatically speed up the solution but it requires a lot of skill to figure out a good algorithm. I use numeric integration and brute force power of the FPGA to accelerate the evaluation of the Bezier curve.

The first step is to look at the bezier equation. The cubic Bezier equation comes in the form:

P0*(1-t)^3 + 3*P1*(1-t)^2*t + 3*P2*(1-t)*t^2 + P3*t^3

This equation requires 9 multiplies in an FPGA. We can multiply out this equation and factor to create a form:

P0 + V0*t + 1/2 A0*t^2 + 1/6 J0*t^3

This form is much easier to evaluate. It requires 5 multiplies. However, we can also come at this equation from a numerical integration perspective. Lets first figure out how we get the acceleration term. The acceleration will be the current acceleration plus J0 divided by the number of time steps. This is the form for finding the acceleration.

A = A + J0 * timeStep

Any multiplication or division by a power of 2 is free inside an FPGA. Therefore we choose a timeStep which is a power of 2. This form is much easier than multiplication. We can do the same for the other parts.

A = A + J0 * timeStep V = V + A * timeStep P = P + V * timeStep

This form involves three accumulators. It requires that we accumulate all the timeSteps in the curve. We cannot jump around from point to point on the curve. We have to evaluate the entire curve at all the timeSteps. The main advantage of this approach is that it is easy to run on an FPGA and we need to evaluate the entire curve anyway to get the length. This form requires three accumulators per axis. We need to do this for each axis (including the extrude axis) which results in 9 accumulators for the first pass and 12 accumulators for the second pass.

However, there is another optimization which can be performed. It is possible to overclock an accumulator and map multiple physical accumulators into one running at a higher clock speed. An accumulator is an addition followed by one register. If we want to map four accumulators into one then we need to clock the multiplier at 4x, use four delay registers before the value is feed back into the adder and time division multiplex the data into the accumulator.

In this design we take this to the extreme. We map 21 accumulators into one physical accumulator. This creates an ultra tiny hardware implementation which can operate at a very high speed.

Now lets look at the order of equations that this accumulator performs. We will use a specific notation for these equations. There are two splines which will be evaluated. One is called the A Spline and one is the B Spline. The A Spline evaluates the X,Y and Z position of a Cubic Bezier Spline. These values are used to determine the rough speed profile. The second spline is called the B Spline and it is used to generate the exact points in space which the toolhead will travel to. Terms such as 'dXa' will refer to the first derivative of the X variable for the A Spline. 'd2Zb' is the second derivative of the Z variable for the B Spline. The order of operations performed by the accumulator is:

d2Xa = d2Xa + Jx0 * timeStep

dXa =  dXa + d2Xa * timeStep
 Xa =   Xa +  dXa * timeStep

d2Ya = d2Ya + Jy0 * timeStep

dYa =  dYa + d2Ya * timeStep
 Ya =   Ya +  dYa * timeStep

d2Za = d2Za + JZ0 * timeStep

dZa =  dZa + d2Za * timeStep
 Za =   Za +  dZa * timeStep

d2Xb = d2Xb + Jx0 * timeStep

dXb =  dXb + d2Xb * timeStep
 Xb =   Xb +  dXb * timeStep

d2Yb = d2Yb + Jy0 * timeStep

dYb =  dYb + d2Yb * timeStep
 Yb =   Yb +  dYb * timeStep

d2Zb = d2Zb + JZ0 * timeStep

dZb =  dZb + d2Zb * timeStep
 Zb =   Zb +  dZb * timeStep

d2Eb = d2Eb + JE0 * timeStep

dEb =  dEb + d2Eb * timeStep
 Eb =   Eb +  dEb * timeStep

Notice anything? The accumulate term is the bit shift of the previous accumulation. This can be accomplished by adding another delay in the feeback path. It makes the implementation extremely simple. Below you can find a link to a picture showing this implementation. There are some ports to load the accumulators with new values at the beginning of a curve. There are some registers to store the jerk values (third derivative with respect to space).

3D Cordic Speed Profiler

CIC Speed Interpolator

A4983 Interface