Penta Axis (PAX) support

[xyzdims]

Some debugging infos (M111 S1 P4 + P6), more useful for you @dc42 :
G1 B20 (fails to perform mechanically):

pr 1 ts=43851297 DDA: start=[0.000000 0.000000 0.000000 0.000000 20.000000] end=[0.000000 0.000000 0.000000 0.000000 20.000000] s=0.000000 vec=[0.000000 0.000000 0.000000 0.000000 20.000000]
a=40.000000 d=nan reqv=4.166667 startv=0.000000 topv=0.000000 endv=0.000000
cks=0 sstcda=0 tstcddpdsc=0 exac=0
DMB: dir=F steps=45 next=1 rev=46 interval=0 2dtstc2diva=nan
accelStopStep=1 decelStartStep=1 2c2mmsda=0.00 2c2mmsdd=nan
mmPerStepTimesCdivtopSpeed=nan fmsdmtstdca2=0.00 cc=0 acc=0
DMZ: dir=B steps=1085 next=1 rev=1086 interval=0 2dtstc2diva=nan
accelStopStep=1 decelStartStep=1 2c2mmsda=0.00 2c2mmsdd=nan
mmPerStepTimesCdivtopSpeed=nan fmsdmtstdca2=0.00 cc=0 acc=0
DMY: dir=B steps=1539 next=1 rev=1540 interval=0 2dtstc2diva=nan
accelStopStep=1 decelStartStep=1 2c2mmsda=0.00 2c2mmsdd=nan
mmPerStepTimesCdivtopSpeed=nan fmsdmtstdca2=0.00 cc=0 acc=0

[ At the first glance it's the first 'd=nan'which raises my attention ]

and debugging info from my kinematics (appears before the above debugging output), whereas ma machinePos mt motorPos:

; CartesianToMotorSteps: zoff=45.000 xm=0.000 ym=0.000 zm=0.000 am=0.000 bm=20.000
;tool->machine(1): x=0.000 y=15.391 z=-42.286
;tool->machine(2): x=0.000 y=15.391 z=2.714
;=== ma.x=0.000 y=-15.391 z=-2.714
;    mt.x=0 y=-1539 z=-1085
; 5 numVisibleAxes
; axis #0: ma.pos 0.00 (100.00stp/mm) mt.pos 0
; axis #1: ma.pos 0.00 (100.00stp/mm) mt.pos -1539
; axis #2: ma.pos 0.00 (400.00stp/mm) mt.pos -1085
; axis #3: ma.pos 0.00 (2.25stp/mm) mt.pos 0
; axis #4: ma.pos 20.00 (2.25stp/mm) mt.pos 45

As comparison, performing G1 X0.1 B20 (succeeds to perform mechanically):

pr 1 ts=323555603 DDA: start=[0.000000 0.000000 0.000000 0.000000 -0.000000] end=[0.100000 0.000000 0.000000 0.000000 20.000000] s=0.100000 vec=[1.000000 0.000000 0.000000 0.000000 200.000000]
a=4.000000 d=4.000000 reqv=0.416667 startv=0.000000 topv=0.416667 endv=0.000000
cks=258125 sstcda=0 tstcddpdsc=258125 exac=39062
DMY: dir=B steps=1539 next=1 rev=1540 interval=4274 2dtstc2diva=28124999680.00
accelStopStep=334 decelStartStep=1206 2c2mmsda=18274854.00 2c2mmsdd=18274854.00
mmPerStepTimesCdivtopSpeed=116.96 fmsdmtstdca2=0.00 cc=0 acc=0
DMZ: dir=B steps=1085 next=1 rev=1086 interval=5091 2dtstc2diva=28124999680.00
accelStopStep=236 decelStartStep=850 2c2mmsda=25921658.00 2c2mmsdd=25921658.00
mmPerStepTimesCdivtopSpeed=165.90 fmsdmtstdca2=0.00 cc=0 acc=0
DMB: dir=F steps=45 next=1 rev=46 interval=25000 2dtstc2diva=28124999680.00
accelStopStep=10 decelStartStep=36 2c2mmsda=625000000.00 2c2mmsdd=625000000.00
mmPerStepTimesCdivtopSpeed=4000.00 fmsdmtstdca2=0.00 cc=0 acc=0
DMX: dir=F steps=10 next=1 rev=11 interval=53033 2dtstc2diva=28124999680.00
accelStopStep=3 decelStartStep=8 2c2mmsda=2812499968.00 2c2mmsdd=2812499968.00
mmPerStepTimesCdivtopSpeed=18000.00 fmsdmtstdca2=0.00 cc=0 acc=0

and my debugging (also comes before):

; CartesianToMotorSteps: zoff=45.000 xm=0.100 ym=0.000 zm=0.000 am=0.000 bm=20.000
;tool->machine(1): x=0.000 y=15.391 z=-42.286
;tool->machine(2): x=0.000 y=15.391 z=2.714
;=== ma.x=0.100 y=-15.391 z=-2.714
;    mt.x=10 y=-1539 z=-1085
; 5 numVisibleAxes
; axis #0: ma.pos 0.10 (100.00stp/mm) mt.pos 10
; axis #1: ma.pos 0.00 (100.00stp/mm) mt.pos -1539
; axis #2: ma.pos 0.00 (400.00stp/mm) mt.pos -1085
; axis #3: ma.pos 0.00 (2.25stp/mm) mt.pos 0
; axis #4: ma.pos 20.00 (2.25stp/mm) mt.pos 45

[xyzdims]

@joergs5 thanks for participating

M203 B20000 is 333 degree/second, which is quite high*)

RRF all speeds are mm or degrees per minutes. Also, G1 A20 performs ok (doesn't not involve X/Y/Z if B=0).

I have lowered it earlier to M203 A10000 B10000 (the info on my blog isn't up-to-date as I change it a lot), but as you perhaps read on my above experience, it's not so much tuning min/max speed the problem as such, the click/blocked motor indicates way out of range speed or acel/decel, there is miscalculation somewhere on DDA/DDARing level, which my debug info I posted today confirms or hints to. I currently try to comprehend Move/DDA/DDARing functionality better, perhaps I'm missing an important config which leads to the d=nan (deceleration) line (see my other post on this thread).

[dc42]

@xyzdims unfortunately I won't have time to look at this in detail for the next couple of weeks. But the following may be useful:

• Acceleration and speed limits set by M201 and M203 apply to the movements of axes as specified on the command line. So if e.g. you command X to move and your kinematics causes B to move as well, the acceleration and speed B won't be limited by the M201/M203 B values, only by the M201/203 X values and the ration of B movement to X movement.
• If you get NaNs in the calculations, this normally means that somewhere you have taken the square root of a negative number, or taken an arcsin or arccos of a number outside the range -1.0..1.0.
• This bit of your debug looks odd:

pr 1 ts=43851297 DDA: start=[0.000000 0.000000 0.000000 0.000000 20.000000] end=[0.000000 0.000000 0.000000 0.000000 20.000000] s=0.000000 vec=[0.000000 0.000000 0.000000 0.000000 20.000000]
a=40.000000 d=nan reqv=4.166667 startv=0.000000 topv=0.000000 endv=0.000000
cks=0 sstcda=0 tstcddpdsc=0 exac=0

Assuming you created extra axes A B only in that order, this means that your kinematics command a move of total distance NaN after segmentation, and B was moving 20 times as much as the distance specified in the G1 command. That's not right, only axes that are moving implicitly should have have a 'vec' component greater than 1. You need to find out where that NaN is coming from. Check that you haven't accidentally set the segmentation distance to zero.

[dc42]

PS - you might want to try the same move using regulator Cartesian kinematics + your A and B axes, to check that the code outside your kinematics is behaving properly in the same configuration.

[xyzdims]

The "pr" comes from void DDA::Prepare() in DDA.cpp, there d or decelaration is NAN.

G1 B20: DDA "pr": a=40.0 (aceleration), d=NAN (deceleration), s=0.0 (distance)

G1 X0.1 B20: DDA "pr": a=4.0, d=4.0, s=0.1

I need to dive deeper into the DDA.cpp to find out, I suspect the main problem could be s=0.0; just after if(delta!=0) in DDA::Prepare() the debug is printed out with tag "pr", it determined there is change, but distance is 0 (=> contradiction), and if any value is divided by s (=0.0) before we get NAN.

[xyzdims]

@dc42 thanks for the hints, I assumed you must be super busy I try to proceed by myself and with @JoergS5 input and share my findings.

[xyzdims]

So, DDA.cpp: DDA::Prepare() calls https://github.com/Duet3D/RepRapFirmware/blob/3.3-dev/src/Movement/DriveMovement.cpp#L59 PrepareCartesianAxis() just before the debug, and there

const float stepsPerMm = (float)totalSteps/dda.totalDistance;

and dda.totalDistance is 0.0 . . . and other calculations follow about acceleration / deceleration, there the NAN is created.

The problem starts somewhere in DDA.cpp DDA::InitStandardMove(), there totalDistance is calculated, which should be non-zero and it looks it is 0.0 there too.

        if (linearAxesMoving)
	{
		// There is some linear axis movement, so normalise the direction vector so that the total linear movement has unit length and 'totalDistance' is the linear distance moved.
		// This means that the user gets the feed rate that he asked for. It also makes the delta calculations simpler.
		// First do the bed tilt compensation for deltas.
		directionVector[Z_AXIS] += (directionVector[X_AXIS] * k.GetTiltCorrection(X_AXIS)) + (directionVector[Y_AXIS] * k.GetTiltCorrection(Y_AXIS));
		totalDistance = NormaliseLinearMotion(reprap.GetPlatform().GetLinearAxes());
		debugPrintf("DDA::InitStandardMove.1: %f totalDistance\n",totalDistance);
	}
	else if (rotationalAxesMoving)
	{
		// Some axes are moving, but not axes that X or Y are mapped to. Normalise the movement to the vector sum of the axes that are moving.
		totalDistance = Normalise(directionVector, reprap.GetPlatform().GetRotationalAxes());
	       debugPrintf("DDA::InitStandardMove.2: %f totalDistance\n",totalDistance);
	}
        else
	{
		// Extruder-only movement. Normalise so that the magnitude is the total absolute movement. This gives the correct feed rate for mixing extruders.
		totalDistance = 0.0;
		for (size_t d = 0; d < MaxAxesPlusExtruders; d++)
		{
			totalDistance += fabsf(directionVector[d]);
		}
		if (totalDistance > 0.0)		// should always be true
		{
			Scale(directionVector, 1.0/totalDistance);
		}
	}
	debugPrintf("DDA::InitStandardMove.3: %f totalDistance\n",totalDistance);

linearAxesMoving and rotationalAxesMoving are both true when I do G1 B20, so which gives strange debug results:

DDA::InitStandardMove.1: 0.000000 totalDistance
DDA::InitStandardMove.3: 0.000000 totalDistance

As reminder: G1 B20, causing Y, Z and B motors to move by request ofPAXKinematics::CartesianToMotorSteps().

That both linearAxesMoving and rotationalAxesMoving are true makes sense, given they mean motor axes.

I think there is a bug in the logic of the code I quoted above:

• if(linearAxesMoving) code block needs to consider rotationalAxesMoving case too, it's both, not "either or" as with else if(rotationalAxesMoving)

I disabled if(0 && linearAxesMoving) case for G1 B20, and I had correct mechanical movement! [Update: https://www.youtube.com/watch?v=2x_rOThE_Ns doing G1 B45 and G1 B0 again, the nozzle tip is supposed to stay in place in relation to bed (hence Y, Z and B movement) ].

@dc42 Let me know if there is someone else I should address this in case you don't have the time/capacity. I think we are close to resolve this.

[dc42]

@xyzdims thanks for looking into this. I think the bug is that if you have declared B to be a rotational axis (the default), then linearAxesMoving should not be set for a G1 B20 move. It should be set if any only if there is motion of the tool head in Cartesian space. The fact that your kinematics has to move X, Y or Z when moving B should not cause linearAxesMoving to be set.

Looking into this some more, I think the linear or rotational axis moving flag should be set as moving if positionDelta is nonzero, not if delta is nonzero as at present.

[xyzdims]

@dc42 doMotorMapping is true in DDA::InitStandardMove(); not sure what the flag actually means, I inserted some more debugPrintf in the relevant loop:

			if (doMotorMapping)
			{
				delta = endPoint[drive] - positionNow[drive];
				debugPrintf("axis #%d: posN %d -> endP %d => delta=%d\n",drive,positionNow[drive],endPoint[drive],delta);
				const float positionDelta = endCoordinates[drive] - prev->GetEndCoordinate(drive, false);
				debugPrintf("   positionDelta=%f\n",positionDelta);
				directionVector[drive] = positionDelta;
				if (positionDelta != 0.0 && (Tool::GetXAxes(nextMove.tool).IsBitSet(drive) || Tool::GetYAxes(nextMove.tool).IsBitSet(drive)))
				{
					flags.xyMoving = true;				// this move has XY movement in user space, before axis were mapped
				}
			}

G1 B20 requested:

doMotorMapping=1, numVisibleAxes=5
axis #0: posN 0 -> endP 0 => delta=0
   positionDelta=0.000000
axis #1: posN 0 -> endP -1539 => delta=-1539
   positionDelta=0.000000
axis #2: posN 0 -> endP -1085 => delta=-1085
   positionDelta=0.000000
axis #3: posN 0 -> endP 0 => delta=0
   positionDelta=0.000000
axis #4: posN 0 -> endP 45 => delta=45
   positionDelta=20.000000
linearAxesMoving=1, rotationAxesMoving=1
DDA::InitStandardMove.1: totalDistance=0.000000
DDA::InitStandardMove.3: totalDistance=0.000000

Hope this helps.

[dc42]

@xyzdims this is the current code (I am looking at 3.4beta1 so it might be slightly different in 3.3):

	for (size_t drive = 0; drive < MaxAxesPlusExtruders; drive++)
	{
		accelerations[drive] = normalAccelerations[drive];
		endCoordinates[drive] = nextMove.coords[drive];

		if (drive < numVisibleAxes)
		{
			int32_t delta;
			if (doMotorMapping)
			{
				delta = endPoint[drive] - positionNow[drive];
				const float positionDelta = endCoordinates[drive] - prev->GetEndCoordinate(drive, false);
				directionVector[drive] = positionDelta;
				if (positionDelta != 0.0 && (Tool::GetXAxes(nextMove.tool).IsBitSet(drive) || Tool::GetYAxes(nextMove.tool).IsBitSet(drive)))
				{
					flags.xyMoving = true;				// this move has XY movement in user space, before axis were mapped
				}
			}
			else
			{
				// Raw motor move on a visible axis
				endPoint[drive] = Move::MotorMovementToSteps(drive, nextMove.coords[drive]);
				delta = endPoint[drive] - positionNow[drive];
				directionVector[drive] = (float)delta/reprap.GetPlatform().DriveStepsPerUnit(drive);
			}

			if (delta != 0)
			{
#if 0	// debug only
				stepsRequested[drive] += labs(delta);
#endif
				if (reprap.GetPlatform().IsAxisRotational(drive))
				{
					rotationalAxesMoving = true;
				}
				else
				{
					linearAxesMoving = true;
				}
			}
		}
		else if (LogicalDriveToExtruder(drive) < reprap.GetGCodes().GetNumExtruders())

This is what I think it should be:

	for (size_t drive = 0; drive < MaxAxesPlusExtruders; drive++)
	{
		accelerations[drive] = normalAccelerations[drive];
		endCoordinates[drive] = nextMove.coords[drive];

		if (drive < numVisibleAxes)
		{
			const float positionDelta = endCoordinates[drive] - prev->GetEndCoordinate(drive, false);
                        if (positionDelta != 0.0)
                        {
				if (reprap.GetPlatform().IsAxisRotational(drive))
				{
					rotationalAxesMoving = true;
				}
				else
				{
					linearAxesMoving = true;
				}
                        }
			int32_t delta;
			if (doMotorMapping)
			{
				delta = endPoint[drive] - positionNow[drive];
				directionVector[drive] = positionDelta;
				if (positionDelta != 0.0 && (Tool::GetXAxes(nextMove.tool).IsBitSet(drive) || Tool::GetYAxes(nextMove.tool).IsBitSet(drive)))
				{
					flags.xyMoving = true;				// this move has XY movement in user space, before axis were mapped
				}
			}
			else
			{
				// Raw motor move on a visible axis
				endPoint[drive] = Move::MotorMovementToSteps(drive, nextMove.coords[drive]);
				delta = endPoint[drive] - positionNow[drive];
				directionVector[drive] = (float)delta/reprap.GetPlatform().DriveStepsPerUnit(drive);
			}

#if 0	// debug only
			if (delta != 0)
			{
				stepsRequested[drive] += labs(delta);
			}
#endif
		}
		else if (LogicalDriveToExtruder(drive) < reprap.GetGCodes().GetNumExtruders())

[xyzdims]

@dc42 I copied it, thanks! It works but give me a day or so to confirm some edge cases as well.