The distance function is to find the carestian distance from the last point to this point. This is used solely for scaling the feed rate in the gus coordinate system. The x1,y1,z1,x2,y2,z2 values don't escape that function. If you look at the top of the code you will see the global values used for those.

- Code: Select all
`x1=-SIZE/2.0`

y1=-SIZE*math.sqrt(3)/2.0/3.0

z1=-SHOULDER_Z1

x2=+SIZE/2.0

y2=-SIZE*math.sqrt(3)/2.0/3.0

z2=-SHOULDER_Z2

x3=0

y3=2*SIZE*math.sqrt(3)/2.0/3.0

z3=-SHOULDER_Z3

Annotated code:

- Code: Select all
`d=distance(coord,position) #get cartesian distance from last position`

xs,ys,zs=coord["X"],coord["Y"],coord["Z"] #get last position coords

x,y,z,f=position["X"],position["Y"],position["Z"],position["F"] #get this position coords

a1=MAX_LENGTH_1-math.sqrt((xs-x1)**2+(ys-y1)**2+(zs-z1)**2) #get last machine coords

b1=MAX_LENGTH_2-math.sqrt((xs-x2)**2+(ys-y2)**2+(zs-z2)**2)

c1=MAX_LENGTH_3-math.sqrt((xs-x3)**2+(ys-y3)**2+(zs-z3)**2)

a2=MAX_LENGTH_1-math.sqrt((x-x1)**2+(y-y1)**2+(z-z1)**2) #get this machine coords

b2=MAX_LENGTH_2-math.sqrt((x-x2)**2+(y-y2)**2+(z-z2)**2)

c2=MAX_LENGTH_3-math.sqrt((x-x3)**2+(y-y3)**2+(z-z3)**2)

virtual_d=math.sqrt((a1-a2)**2+(b1-b2)**2+(c1-c2)**2) #get gus/machine coordinate system distance from last position

fnew=f #get cartesian feed rate

if d!=0:

fnew=f*virtual_d/d #scale the cartesian feed rate to get the machine feed rate

The only thing that is really important is. "Good" firmware should only need this an maybe the forward kinematics.

- Code: Select all
`a=MAX_LENGTH_1-math.sqrt((x-x1)**2+(y-y1)**2+(z-z1)**2)`

b=MAX_LENGTH_2-math.sqrt((x-x2)**2+(y-y2)**2+(z-z2)**2)

c=MAX_LENGTH_3-math.sqrt((x-x3)**2+(y-y3)**2+(z-z3)**2)

x1,y1,z1 is the location of the first shoulder pivot.

x2,y2,z2 ...

x,y,z is the location of the effector.

a,b,c are the positions of the steppers.

Here is a start for the forward kinematics...

- Code: Select all
`def getxyz(r1,r2,r3):`

d=SIZE*1.0

i=SIZE/2.0

j=SIZE*math.sqrt(3)/2.0

x=(r1*r1-r2*r2+d*d)/(2*d)

y=(r1*r1-r3*r3-x*x+(x-i)**2+j*j)/(2*j)

z=math.sqrt(r1*r1-x*x-y*y)

return x,y,z

I say start because the x and y coordinates or possibly translated and rotated. However, the z coordinate is right and that is what I need that function for. (z leveling) The function uses

trilateration. Trilateration is the process of taking 3 distances (laterals/stepper positions) from know locations (shoulder positions) to find a position (effector position). This math is hard unless you put one shoulder at 0,0 and another on the x axis. The typical approach is to translate the shoulder to align with this simplifications and then after you get a position translate it back. I didn't do that last step because it wouldn't change the z position and that is all I needed.

What I am saying is that Smoothieware should only require a inverse kinematic function and possibly a forward kinematic function (typically needed for end stop checking). Marlin on the other hand may require much more work. However, The Simpson math is very similar to delta math so it may not be that hard to port.