Polar Software

I wanted to toss out for ideas for software.

What I'm currently working on for software consists of microcontrol and host software.

The microcontrol software basis uses cubic polynomial spline curves for each motor and control target position.
- this code uses integer addition only.
- uses a simplified generic 'controller' bytecode, for example, bytecodes for:
* set control #A, parameter #B, to value #Z.
* advance all controls #N steps forward in time, one step at a time.
- current implementation includes just a spline control with 6 registers: stepwise position MSB, stepwise position LSB, stepwise velocity MSB, stepwise velocity LSB, stepwise accelleration, and stepwise jerk.
* targets a stepper with logic, step up if position MSB > current position, step down if position MSB < current position.
- additional logic implements full window / crc to give reliable communications over unreliable channel (I found loss of data on USB to Serial link I was using to arduino -- probably related to interrupt handling on arduino side).
* keeping the reliable comms worthwhile even if solution for interrupts is discovered, since overhead is small and guarantees confidence that either the right numbers will be delivered, or an error reported on the host if too much error occurs.

The host software basis converts linear paths into cubic polynomial paths for the specific robot topology.
- I expect to try and leverage any existing gcode interpreter to produce the linear paths for input into this code.
- The engine would also support any function as input, not just linear. This might be useful, for example, to drive an exponential decay on extruder rate to reduce or eliminate dribble without loss of print accuracy, and still get crisp starts and stops on extrusion. (Nophead indicates that he uses a fast reverse to accomplish this, with good results. He also indicated that perhaps modeling using an exponential decay may also improve print quality (modeling extrusion rate using a first/second order differential equation involving plastic pressure within the extruder nozzle.)

I think with simple modication, both host and client software should be easy to map to any robot platform. It would have most utility on a non-linear (E.G, polar) robot platform, because of the non-trivial computations for X,Y,Z location, but would also be usable on a cartesian system, and may have advantage in that it could create instructions for microcontroller that produces optimized builds -- the more closely one can tune robot limits, the faster the prints will happen.

After thinking about this, I'm thinking a 'configuration' wizard, that gives different choices for robot topology for mapping the linear paths to a set of 'curvy' paths, which the spline engine then takes, optimizes, and produces a set of spline bytecodes the microcontroller can consume to create the model. (I'll start a different thread that discusses the different polar topologies.)

- Best/easiest topologies are those that are analytic. Not all linkage systems have closed form though, so some could be numerical/iterative. I don't see any reason why these could not also be consumed, as my engine wants to have a point and the partial derivatives of each axis, which also exist for the numerical/iterative algorithms -- as long as they converge to the appropriate values.

Any thoughts or ideas?

That sounds great. How will the step and direction be output to the microcontroller? Are you going to code a seperate program for that too or simply make a new gcode to run in Kcam? Because it might be easier to make a modified gcode. All you would have to do is take a piece of gcode, convert it to polar, then output a new gcode with the step and directions for the motors. It might be more complicated than this, but I think this would be a good approximation.

I use a bytecode interpreter on the firmware side; it consists of the following registers:

motionRegister - 32 bit
controlID - 8 bit
stepCount - 16 bit

Bytecode instructions are coded as:

1xxxxxxx - shift motionRegister - motionRegister <<= 7; motionRegister |= xxxxxxx
01xxxxxx - set controlID = xxxxxx
001xxxxx - load motionRegister - motionRegister = xxxxx (sign extended)
0001xxyy - fast set control xx, register yy <- motionRegister
00001xxx - set control controlID, register xxx <- motionRegister
00000001 - set stepCount <- motionRegister
00000000 - nop

The other 6 bytecode instructions are reserved for future use.

Each motor in the robot would be attached to a specific controlID. The ones with the most activity should be assigned to id 0, 1, 2, and 3, as they can use the fast set control instruction and reduce bandwidth (1-5 bytes per register assign instead of 4-8 bytes per register assign.)

Comms uses a packet structure with two modes - one to send a block of opcodes to the bytecode interpreter, and one to execute out of band requests. Normal operation would be the host sending as many blocks of opcodes as fit into memory, and wait for responses indicating room has opened up for additional blocks. The out of band requests allow the host to request current information for the state of any control, the state of the bytecode machine, etc.

With this, the specification limits on my firmware control are:
64 individual controls with up to 8 32 bit registers to define control motion for 1 to 65535 steps.

The controls themselves are implemented via an abstract C++ class:

class Controller
{
public:
Controller() {};

virtual bool get_IsReady() const = 0;
virtual void Step() = 0;
virtual void SetProperty( uint8_t propertyId, int32_t propertyValue ) = 0;

virtual void Display( bool details ) = 0;

virtual void Init() = 0;
virtual void Reset() = 0;
virtual void TimeSlice( uint32_t dtime ) = 0;
};

get_IsReady() returns true when the motor is ready to advance to the next state. The idea is that all controls have to report back as ready before advancing to the next step.

Step() is called once for every control each time the stepCount is decremented.

SetProperty(..) is called by the bytecode interpreter.

Display(..) is called out of band; the host can query individual controls and this is called by the comms controller to report the short state of the control, or detailed state of the control.

Init() is called during board initialization.

Reset() is called out of band; the idea here is to home the control and set it's state to a know position. May do nothing for some controls.

TimeSlice( .. ) is called by the main loop. This has real time behavior; under most situations, the control can expect an invocation to timeslice with set delta time minimum expected before the next call, and it must guarantee it will not use more than some minimum of time executing it before returning.

I've created one derived class so far, the SplineStepControl, that implements the following registers:

5: positionMSB - 16 bits
4: positionLSB - 32 bits (representing a base 2 'decimal' for position)
3: velocityMSB - 16 bits
2: velocityLSB - 32 bits (again, representing a base 2 'decimal' for velocity)
1: accelleration - 32 bits (represeting a number from -0.5 to 0.5 fixed point)
0: jerk - 32 bits (representing a number from -0.5 to 0.5 fixed point)

I plan on implementing two more derived class, the SplineLoopStepControl, similar to the above but with expectation that it will wrap around at a specific position, and the SplinePidControl, that will be used for temperature control.

I'll be allocating the two polar angles for the RepOlaRap to use control ID 0 and 1. I expect the extruder to use control 3. Everything else will probably use 4 and higher since they should change very rarely. For a robot configuration where the third spacial motor must continually be refined, it would probably want to use control ID 2. Since I don't yet have host software yet, all of these have not been implemented in fixed form. Ideally, what I'd like is for the host to send an out of band configuration stream that defines a control type, and a set of IO ports specific to each type, so reconfiguring firmware to a different robot would not require a firmware change, just a configuration change on the host side. It may be easier to hold off doing this right now though; recompiling and redownloading firmware isn't a big deal right now.

Updated: I guess that didn't really answer your question. To output step and dir signals, the SplineStepController implements behavior in the TimeSlice() method. It executes the following logic:

  if( current < positionMSB && dir != LOW )
  {
     dir = LOW;
     DIGITAL_OUT( dirPin, LOW );

     // return immediately to give pin a chance to change before stepping.
     return;
  }

  if( current > positionMSB && dirPin != HIGH )
  {
    dir = HIGH;
    DIGITAL_OUT( dirPin, HIGH );

    // return immediately to give pin a chance to change before stepping.
    return;
  }

  if( current < positionMSB )
  {
    ++current;
    step = !step;
    DIGITAL_OUT( stepPin, step );
  }

  if( current > positionMSB )
  {
    --current;
    step = !step;
    DIGITAL_OUT( stepPin, step );
  }

The stepper get_IsReady() returns true if current == positionMSB.

Edited 1 time(s). Last edit at 03/16/2010 09:46AM by BeagleFury.

Ok good, that is all I needed to know. How to do you want the microcontroller to communicate back to the computer? Obviously you can't use a parallel port, so what do you think we should use?

My current firmware assumes Arduino, using USB over Serial. I used standard Arduino libraries, which is part of the reason I use windowing and crc checking (Arduino serial comms drops packets under high throughput situations drops data, presumably because TX data does not use a ring buffer.)

In terms of data format, I may eventually wrap transmitted packets with CRC and length markers too, to ensure the host recieves correct data or knows it is garbage (and can request state information again.)


For example, I believe I coded short form 'display' on a spline stepper to output (assuming control ID 0) as:

host sends>
C0?

firmware responds>
C0 566

where C0 means "control #0", and 566 is the current position.

The 'details' flag allows a full enumeration of hard configured controls. If the host, for example requested details for all controls, it would get text along the lines of:

Host sends (out of band channel)>
C0?+
C1?+
C2?+
C3?+
...
C63?+

Firmware responds>
C0 SplineStep 566,24291392,0,2499032,249,1
C1 SplineStep 2349,64582,0,0,0,0
C2 NC
C3 SplineLoopStep 65535,852,258,0,0,0,0
...
C63 NC

One important out of band request that I feel should always be implemented is the firmware version request:

Host>
V?

Firmware Responds>
V2010-03-16T13:44 RepOlaRap Firmware V0.1 alpha

This allows the host client to know immediately whether it has any chance of working with the currently downloaded firmware version, and can also include logic to switch features on and off based on the firmware capabilities.

Edited 1 time(s). Last edit at 03/16/2010 02:04PM by BeagleFury.

Ahh, I was planning on using a cheaper microcontroller, the SX from parallax. I was just going to do simple step and direction proccessing through a parallel port, handling all the calculations on the computer, but checking the position of the motors with the microcontroller. There would be no feedback to the computer from the microcontroller. If the steps went to fast for the motors to catch up, I would put an emergency stop in the microcontroller and display a red light.

Your way sounds much more advanced than I am used to, so you can handle the microcontroller stuff. I will design the mechanics and send you the stl file to build your own. Do you have a reprap already so you can print one out?

galaxyman7 Wrote:
> Do you have a reprap already so you can print one out?

I do not. Part of my own goals include a simple and easy to build polar repstrap (I.E, RepOlaRap is primarily a repstrap, and secondarily, a reprap if things look advantageous when compared to Mendel.)

In terms of your idea for firmware, you can certainly try something along the lines of an EMC2 RepRap. The biggest problem others have had running this kind of configuration has been real time control gotchas. The real time requirements mean that you may need effectively run a specialized real time operating system instead of a normal multi-tasking OS, removing all possibility of using the drive computer for anything else for the hours or days when it is printing. In contrast, a simple SD card interface allows a standalone printer with Arduino or similar microcontrol cards where you can disconnect the computer without losing print bandwidth.

You trade a $50-$150 electronic board for a $300+ computer for your electronics 'controller'... Although, one could probably buy or recycle an inexpensive, older less powerful PC, and run DOS or MINUX or something similar -- this could result in a simple and inexpensive option.

On the positive, there is no need for the complexity I introduce with spline curves, etc. Just drive it using the direct calculations, and just ensure they run fast enough to achieve the required real time constraints.

If you are going this route, you might want to start by reading the wiki article on EMC on RTLinux....

Edited 1 time(s). Last edit at 03/16/2010 04:52PM by BeagleFury.

Isn't Mach 3 a real time program? I have a cnc controlled by parallel port and it doesn't have any problems with bandwidth. Why would this be any different?

Also, couldn't you set the priority of the program to "real time" in windows task manager?

galaxyman7 Wrote:
-------------------------------------------------------
> Isn't Mach 3 a real time program? I have a cnc
> controlled by parallel port and it doesn't have
> any problems with bandwidth. Why would this be any
> different?

I don't recall if Mach 3 has realtime extensions or not. Several multi-tasking operating systems do have real time extensions; however, some users don't always understand the limitations that come with running other stuff while realtime stuff is running, and in addition, there have been issues when real time programs start interfering with other hardware that also needs real time responses.

> Also, couldn't you set the priority of the program
> to "real time" in windows task manager?

Yes. 'Realtime' in a multi-tasking operating system often has caveats because of system complexity.

For your CNC over parallel port, do you have a dedicated computer for it, or do you use that computer for other tasks while it is printing, I.E, do you use it for email, browsing the web, playing movies / music, etc. while using it to drive the CNC? -- the idea that you use a $300+ electronics platform by using a real time OS and a computer instead of a microcontroller has no drawback if you already have the system sitting idle doing nothing, but incurs greater cost if you must go out and buy the dedicated system to use if you do not.

I do use a laptop to control the cnc. How is this different than using an Arduino to control it? Won't the program you are proposing also take up bandwidth, needing real time priority? I know the microcontroller gives feedback to the computer, but even then, bandwidth will affect how the machine performs by slowing down calculation times.

Also, I was thinking about using the SD card interface, so that all you need to do is convert the g-code to polar, put it on the card, and stick it into the slot on the machine. However, reading an SD card with a microcontroller would probably be complicated and increase processing time.

> I do use a laptop to control the cnc. How is this different than using an Arduino to control it? Won't the program you are proposing also take up bandwidth, needing real time priority? I know the microcontroller gives feedback to the computer, but even then, bandwidth will affect how the machine performs by slowing down calculation times.

Yes. It still requires real time responses, but the timing requirements are expanded several orders of magnitude. Instead of responsiveness on the order of 10s of microseconds, you have responsiveness requirements on the order of a few seconds -- depending on how much buffer space you have on the microcontroller. Unless you really hammer on a system, or try to run any microsoft products, this should not pose any problems.

> Also, I was thinking about using the SD card interface, so that all you need to do is convert the g-code to polar, put it on the card, and stick it into the slot on the machine. However, reading an SD card with a microcontroller would probably be complicated and increase processing time.

I believe there already exist the code to read the SD card, so someone else already solved the complicated part relating to this. This too would produce a possible solution for a printer, polar or not.

I do not intend to take this approach on my repstrap. However, I can see no great difficulty swapping in or including a flash read solution once a working system has been tested and works.

Funny enough: is it possible the easiest way to program a robotic arm is to strap a clone of it with sensors in place of the actuators/motors to a CNC mill and use the output to drive the arm's tool path?

In industry: I understand that they normally program the robotic arms using a "training" process where either the arm goes limp or each joint is remote controlled, and then the computer just copies what has already been done. They can tweak it as necessary at that point if desired.

Edited 1 time(s). Last edit at 03/25/2010 05:03PM by JohnnyCooper.

That is interesting. You could have the computer sample the angle for each joint, then reproduce the motion with another bot. However, this would mean that every toolpath you wanted to do, you would need to first do on the linear machine, in order to program the robotic arm. I could see how this programming method would be very useful in mass production however, since you could have an army of robotic arms all doing the same toolpath over and over. It could also be useful for finding a function for the angles that would give straight line motion or circular motion. Then use that to do all the lines or arcs in your program. I think this can be done without an actual robotic arm and milling machine though, just using a computer simulation. The problem is taking the information and applying it to any line in any direction, or an arc with any radius. This would require a lot of different trials. I hope we can just come up with a formula for this instead of introducing experimental errors.

Here is a webpage explaining conversion of rectangular to polar coordinates.
[www.mecca.org]
Here is the equation of a straight line in polar
(3) 2x – y = 4
2r cos q - r sin q = 4
r = 4 / (2 cos q - sin q)
If we could get two points on the endpoints of a line, create an equation for them, and convert it to polar, then we could find out what the function of the angles must be to create that line.

Here is the general equation for a line in polar, using two points
r = (y1-m*x1)/(sin(theta)-m*cos(theta)) where m = (y2-y1)/(x2-x1)

now we need to convert r into a function of angles a and b.

Edited 1 time(s). Last edit at 03/25/2010 08:14PM by galaxyman7.

Unfortunately this is only for 2d lines, but since it is a 3d printer that only prints 2d layers, it is enough.
Now we combine this equation with the previous equation for r
r= L1*cos(a) + L2*cos(a+b)

to get a combined equation with theta, a, and B in terms of x1, x2, y1, and y2.

L1*cos(a) + L2*cos(a+b) = (y1-m*x1)/(sin(theta)-m*cos(theta))

m = (y2-y1)/(x2-x1)

Edited 1 time(s). Last edit at 03/25/2010 08:31PM by galaxyman7.

Since the motor will only step, rather than actually run according to a function, all the lines will be a series of small arcs. But as these arcs get very small, they should be a very good approximation of a line. All the program really needs to do is take in lines or arcs, and smaple points on them. Then convert those points into angles, and make a new modified g code. The robotic arm can then be run on any simple step- direction program such as EMC or Mach 3.

The solution I took was to use the closed form equations for the angles, get 4 positions an a target line for every motor involved in the equation (X, Y, Z, Extruder, possibly even Temperature and fan speed), and generate a parametric spline curve for each axis using the corresponding position from those 4 points.

Then, sample the error on a set of points falling between the 4 points.. if error is greater than some threshold, cut the line in half, and try again, resulting in twice the number of spline curves.

I found that for the majority of print area in the repolarap configuration, 1 to 4 spline curves was enough to get accuracy to 0.01% of the target line, which should be adequate for a .5m x .5m print area (0.05mm accuracy)

Once you have it in spline curve format, you just run your time variable from 0 to total print time in a step small enough to be able to get the individual steps, I.E

for( double t = 0; t < maxT; t += smallestTimeIncrement )
{
exactAngle1 = splineAngle1(t);
exactAngle2 = splineAngle2(t);
exactAngle3 = splineAngle3(t);
exactExtruderPosition = splineExtruder(t);

getStepPos1 for exactAngle1
getStepPos2 for exactAngle2
getStepPos3 for exactAngle3
etc.
}

What I found is that for the degenerate areas, you can not use a 'smallest time increment' because as you approach the degenerate area, you get a number closer and closer to zero (it becomes zero at the degenerate.) What I've been struggling with is to find an appropriate function h(x) such that you can do a simple mapping from the above, and use instead:

for( double ht = 0; ht < maxHT; ht += 1 )
{
// map from linear space to 'step' space.
t = h(ht);

exactAngle1 = splineAngle1(t);
exactAngle2 = splineAngle2(t);
exactAngle3 = splineAngle3(t);
exactExtruderPosition = splineExtruder(t);
etc...
}

Further, I want h(x) to map to a time space that would result in every step resulting in an overall velocity, acceleration, and jerk limited curve profile. Such a configuration could be tuned for maximum print speed vs acceptable mechanical vibration.

So I don't really know much about math or programming and the idea of "spline curves" is a little bit over my head. [en.wikipedia.org]

Would it be possible for you guess on a scale from 1 to 10 how close are you to having a functional G&M code converter to control 2 DoF? 3? 4? 6? I'm not even 100% sure that's the objective? I see code and math and my eye's just kind of gloss over.(I once wrote a couple lines of C to control 2 hobby servo's and I practically threw myself a party in excitement when it worked)

In terms of compensating for oscillation: could some PID code be recycled from the OpenServo? [openservo.com]

Edited 1 time(s). Last edit at 03/26/2010 04:43PM by JohnnyCooper.

galaxyman7 Wrote:
-------------------------------------------------------
> Unfortunately this is only for 2d lines, but since
> it is a 3d printer that only prints 2d layers, it
> is enough.
> Now we combine this equation with the previous
> equation for r
> r= L1*cos(a) + L2*cos(a+b)
>
> to get a combined equation with theta, a, and B in
> terms of x1, x2, y1, and y2.
>
> L1*cos(a) + L2*cos(a+b) =
> (y1-m*x1)/(sin(theta)-m*cos(theta))
>
> m = (y2-y1)/(x2-x1)

Could all of these equations be converted in to a library?

Edited 1 time(s). Last edit at 03/26/2010 04:47PM by JohnnyCooper.

To tell the truth, I have no idea how to program (very well) in C either. As funny as it may seem, Game Maker is what I use for most things. It has an extremely easy programming language, but extemely powerful. All the commands pop up on the bottom as you type them in, and you can scroll over them to see what they do. Also, there are no special characters and combinations to learn. It is great for beginning programmers and advanced programmers; for games or for programs. Everything is seperated and easy to see, rather than having one long code in C. Plus it is easy to incorporate a nice GUI interface. Also, people add libraries for it constantly, so it just gets more and more easy to use.

JohnnyCooper Wrote:
> Would it be possible for you guess on a scale from
> 1 to 10 how close are you to having a functional
> G&M code converter to control 2 DoF? 3? 4? 6? I'm
> not even 100% sure that's the objective? I see
> code and math and my eye's just kind of gloss
> over.

It's hard for me to judge. Not close for what I want, but very close if I didn't compensate for jerk too (I have compensated for both velocity and acceleration limits on any number of axis controls) -- Note - I'm not even sure I need to limit jerk. See third order motion profile on Wikipedia's article on Jerk for what I'm trying to accomplish for the curves..

Perhaps a simple option, if you are just trying to generate steps, is to use a modified binary search to find step transistions of at most one step on the set of axes you are working on.

In pseudocode, it would work along the lines of:

FindSteps( fromPoint, toPoint )
1. get polar form of fromPoint.
2. get polar form of toPoint.
3. Convert polar forms into integer motor positions.
4. For each axis in the integer positions, do they differ by at most 1?
4.a. Yes -> include the steps to take motors from the first polar form point to the second polar form point. This may be zero steps to include if all the positions do not change for this particular motion.
4.b. No.
4.b.i compute midpoint between fromPoint and toPoint.
4.b.ii recursively call FindSteps( fromPoint, midPoint ) to find the steps to the middle of the line.
4.b.iii recursively call FindSteps( midPoint, toPoint ) to find steps to the end of the line.

You end up with the steps for each motor that must occur, in the exact order that they must be executed, to get straight line motion for whatever configuration your robot may use.

With the steps, it is relatively easy to limit the velocity (Just make sure you wait some minimum of time between each step.) It is a bit trickier, but also possible to limit acceleration on the set of steps as well. I'm not sure how to limit jerk using this technique.

The only reason to use splines would be the possibility that you could do processing along an entire set of steps, as well as my bias for wanting to send the motion as a sequence of splines rather than a sequence of steps to compress bandwidth to the firmware, as well as reduce the realtime aspect from the host.

When you say "splines", what do you mean? Are you going to use some kind of polynomial equation for each motor in order to simulate straight line motion? Because you could do a derivative of the angle equations with respect to time. Then you set the derivative of y and x to the slope of the line required, finding your velocity for each angle. You could do the same thing with acceleration. If you use this approximation for each line segment, you would get closer and closer to a straight line.

> When you say "splines", what do you mean?

Usually, a cubic spline, but more generally, any polynomial spline to approximate the motion.

> Are you going to use some kind of polynomial equation for each motor in order to simulate straight line motion?

Sort of. Take a look at the curves I generated that trace out a particular 'build' on my builders blog for repola computations

Those curves are only constrained by position. I have advanced beyond that and constraint the curves by velocity, and accelleration, but not jerk. What you see in the first graph of curves consist exactly of the angles needed at each moment in time to trace out the complex shape introducing the post.

If you look at the corresponding 'velocity' curve, you can very easily make out exactly the distinct cubic polynomials making the spline since I did 4 point matching rather than two point position and velocity matching between distinct spline curves (Each 'jag' is a new polynomial in the spline.)

Also, the red lines you see in the 'shape' image are actually a time simulated motion of the splines. The snapshot I use traced only the last 1000 or 2000 steps (I don't recall the exact number), but trust that it traces out the entire curve with the accuracy that I enter as a criteria for acceptance in the spline. If I were to try and simulate this same graph with straight lines, I'm not sure how many more I would need.. definitely an order of magnitude more when near the degenerate areas, but probably only a factor of 2 or 3 more in ideal areas for short lines.

7 DOF open source platform and software:
[www.willowgarage.com]

Some libraries that perhaps IK tidbits can be tapped?
Big list:
[www.ros.org]

IK relevant/spline/motion planning relevant:
[www.ros.org]
[www.ros.org]
[www.ros.org]
[www.ros.org]
[www.ros.org]
[www.ros.org]
[www.ros.org]
[www.ros.org]
[www.ros.org]
[www.ros.org]
[www.ros.org]
[www.ros.org]
[www.ros.org]
[www.ros.org]
[www.ros.org]
[www.ros.org]
[www.ros.org]
[www.ros.org]
[www.ros.org]

Lots of very interesting stuff there. Thanks.

[www.mathworks.com]

It occurs to me that a 3rd joint could be added by simply calculating the trig relationship between Adjacent(needed) and hypotenuse(adjusted) for a given opposite.(Z)


Even less complicated with a scara setup. I've been questioning my goal of a Backhoe format when I consider that a backhoe's design is specialized for the wrong plane. (vertical)

Seems to me: the most basic polar bot would resemble a record player.