Fixed backlash layer shift bug

[tombrazier]

Description

For full details of the problem, see the long conversation in schmtcc's branch linked below.

In summary, when backlash steps are added to a segment block->millimeters is not updated. This causes errors in later acceleration calculations in Planner::_populate_block() which depend on steps_per_mm which, ultimately, depends on block->millimeters. If the segment is long enough the errors are small and insignificant but for a short segment this can result in accelerations several times the configured maximum.

This PR fixes the bug by updating block->millimeters with a 1st order (i.e. linear) approximation. This works for cartesian and core kinematics and, to varying degrees, also for other kinematics. The approximation is least accurate if the backlash distance is large with respect to the segment length so applied backlash is limited to no more than the movement distance for any given axis.

Note: the behaviour limiting the applied backlash to <= segment distance can make backlash towers and patterns inaccurate if they use very small movements to introduce backlash and also require backlash to be enabled whilst they are printed. The solution is to use such towers/patterns that run with backlash disabled or always use movements larger than the currently configured backlash.

Mathematical details

By "1st order approximation" I mean the following. We assume that block->millimeters is calculated as

D = sqrt(X^2 + Y^2 + Z^2)

where X = X(a, b, c), Y = Y(a, b, c) and Z = Z(a, b, c) are linear functions (or linear enough for the segment being printed - which will always be the case because even with non-linear kinematics like delta or SCARA movements are broken down into short linear segments) and a, b and c are step counts.

The change ΔD in block->millimeters for changes Δa, Δb and Δc in the step counts can be approximated as

ΔD = dD/da * Δa + dD/db * Δb + dD/dc * Δc.

If we analyse the contribution from the a steps we can then apply the same reasoning to the other axes. Differentiating, we find

dD/da * Δa = 0.5/sqrt(X^2 + Y^2 + Z^2) * 2 * (X * dX/da + Y * dY/da + Z * dZ/da) * Δa
= 0.5/D * 2 * (X * dX/da + Y * dY/da + Z * dZ/da) * Δa
= (X * dX/da + Y * dY/da + Z * dZ/da) * Δa / D
= (X * dX/da + Y * dY/da + Z * dZ/da) * Δa / block->millimeters

Cartesian

For cartesian kinematics, X = dist[A_AXIS] * planner.mm_per_step[A_AXIS], dX/da = planner.mm_per_step[A_AXIS], dY/da = 0 and dZ/da = 0. So the net effect of adding Δa steps to the movement of the a stepper is:

dD/da * Δa = dist[A_AXIS] * sq(planner.mm_per_step[A_AXIS]) * Δa / block->millimeters

and the same logic applies to the other two axes giving

ΔD = (dist[A_AXIS] * sq(planner.mm_per_step[A_AXIS]) * Δa + dist[B_AXIS] * sq(planner.mm_per_step[B_AXIS]) * Δb + dist[C_AXIS] * sq(planner.mm_per_step[C_AXIS]) * Δc) / block->millimeters

Core

For core kinematics one of the three axes is identical to the cartesian case and for the other two axes (let's say a and b)

X = sqrt(0.5) * (dist[A_AXIS] * planner.mm_per_step[A_AXIS] + dist[B_AXIS] * planner.mm_per_step[B_AXIS])
dX/da = sqrt(0.5) * planner.mm_per_step[A_AXIS]
Y = sqrt(0.5) * (dist[A_AXIS] * planner.mm_per_step[A_AXIS] - dist[B_AXIS] * planner.mm_per_step[B_AXIS])
dY/da = sqrt(0.5) * planner.mm_per_step[A_AXIS]

so

X * dX/da = sqrt(0.5) * (dist[A_AXIS] * planner.mm_per_step[A_AXIS] + dist[B_AXIS] * planner.mm_per_step[B_AXIS]) * sqrt(0.5) * planner.mm_per_step[A_AXIS]
Y * dY/da = sqrt(0.5) * (dist[A_AXIS] * planner.mm_per_step[A_AXIS] - dist[B_AXIS] * planner.mm_per_step[B_AXIS]) * sqrt(0.5) * planner.mm_per_step[A_AXIS]
dZ/da = 0

Adding it all up you get the same as for the cartesian case.

dD/da * Δa = dist[A_AXIS] * sq(planner.mm_per_step[A_AXIS]) * Δa / block->millimeters

and so the ultimate formula for ΔD is the same.

This make sense when you consider that core kinematics are just a matter of rotation around one of the axes by 45° and rotation operations do not change distances.

Non-linear

For non-linear kinematics we assume that the kinematics are a further generalisation in which there is rotation and also scaling from stepper space to cartesian space and no other transformations. This must be the case to an acceptable degree because Marlin implements core kinematics by breaking the resultant curved segments down into multiple short linear segments in which it is assumed that any linear scaling of the travel in a, b and c is close enough to a linear scaling of the desired movement in cartesian space. We have already seen that rotation has no effect on distance so we will ignore it. We will assume that all three axes have the same scaling factor from mm in stepper space to mm in cartesian space. This will vary in accuracy being, probably, worst for SCARA. A better solution would calculate dD/da, dD/db and dD/dc in the kinematics logic and pass it down to the backlash logic.

Under these assumptions

D = sqrt(A^2 + B^2 + C^2) * α

where α is the uniform scaling factor and A(a) = a * planner.mm_per_step[A_AXIS], B(b) = b * planner.mm_per_step[B_AXIS] and C(c) = c * planner.mm_per_step[B_AXIS]. Following similar logic to above we find each axis has the pattern

dD/da * Δa = 0.5 / sqrt(A^2 + B^2 + C^2) * α * 2 * A * dA/da * Δa
= 0.5 * D / (A^2 + B^2 + C^2) * 2 * A * dA/da * Δa
= A * dA/da * Δa * D / (A^2 + B^2 + C^2)
= A * dA/da * Δa * block->millimeters / (A^2 + B^2 + C^2)

Substitute in A = dist[A_AXIS] * planner.mm_per_step[A_AXIS] and dA/da = planner.mm_per_step[A_AXIS] and apply the same logic to all axes and the resultant value for ΔD is just the value for the cartesian case multiplied by sq(block->millimeters) / (A^2 + B^2 + C^2), i.e.

ΔD = (dist[A_AXIS] * sq(planner.mm_per_step[A_AXIS]) * Δa + dist[B_AXIS] * sq(planner.mm_per_step[B_AXIS]) * Δb + dist[C_AXIS] * sq(planner.mm_per_step[C_AXIS]) * Δc) * block->millimeters / (sq(dist[A_AXIS] * planner.mm_per_step[A_AXIS]) + sq(dist[B_AXIS] * planner.mm_per_step[B_AXIS]) + sq(dist[C_AXIS] * planner.mm_per_step[C_AXIS]))

Requirements

Backlash compensation enabled.

Benefits

Fewer layer shifts.

Related Issues

schmttc/EasyThreeD-K7-STM32#2

[tombrazier]

I have tested the logic on my printer and it seems to work fir @schmttc as well. I have confirmed that it at least compiles on at least one delta printer but cannot test non-cartesian kinematics.