voron5x with RTCP

OttoJiang · 5 Jun 2024, 17:25

M669 K13 B"CoreXY5BC"
M669 A"C=0"
M669 A"B=-90:90:0"
M669 A"Z=0:120:0"
M669 A"X=0:120:0"
M669 A"Y=0:120:0"
M669 C"C=0:0:1:0:0:0"
M669 C"B=1:0:0:0:0:0"
M669 C"Z=0:0:1:0:0:0"
M669 C"X=1:0:0:0:0:0"
M669 C"Y=0:1:0:0:0:0"
M669 C"Mnoap=1:0:0:0:1:0:0:0:1:0:0:0"
M669 C"Mreference=0:0:0:0:0"
M669 S400
M669 T0.05

T3P3Tony · 5 Jun 2024, 18:58

@OttoJiang thanks! other than those settings are there anything else that is "out of the ordinary" for a voron V0 with two Z motors?

OttoJiang · 6 Jun 2024, 01:24

@T3P3Tony no, two Z motors share one interface and driver.

OttoJiang · 24 Oct 2024, 10:32

b860ca50-40b5-49c6-a01a-6eee3e6353da-373f17267ac28bce0491e00515ed19fd.mp4

Fixed a bug where, after giving a G1 command that includes only BC values without XYZ, that is, when the nozzle's XYZ coordinates in the workpiece coordinate system remain unchanged and only the BC axis angles are changed, the nozzle moves in a straight line in the machine coordinate system. It should actually move in a curve to keep the nozzle's XYZ coordinates in the workpiece coordinate system constant during the movement.

droftarts · 24 Oct 2024, 11:14

@OttoJiang Would segmenting the moves help? I don't know if @JoergS5's firmware allows segmentation of moves.

Ian

OttoJiang · 24 Oct 2024, 11:32

@droftarts Segmenting the moves does help. There are some small errors in @JoergS5's firmware, and I plan to discuss them with him in the coming days. However, the main issue lies in the GCodes.cpp file in the RepRapFirmware source code. I don't mean that it's a bug in RepRapFirmware itself, but rather that certain parts of the source code need to be modified to adapt to this special kinematics. The code in the DoStraightMove function in GCodes.cpp needs to be changed.

// Apply segmentation if necessary
		// As soon as we set segmentsLeft nonzero, the Move process will assume that the move is ready to take, so this must be the last thing we do.
#if SUPPORT_LASER
		if (machineType == MachineType::laser && isCoordinated && ms.laserPixelData.numPixels > 1)
		{
			ms.totalSegments = ms.laserPixelData.numPixels;			// we must use one segment per pixel
		}
		else
#endif
		{
			const Kinematics& kin = reprap.GetMove().GetKinematics();
			const SegmentationType st = kin.GetSegmentationType();
			// To speed up simulation on SCARA printers, we don't apply kinematics segmentation when simulating.
			if (st.useSegmentation && simulationMode != SimulationMode::normal && (ms.hasPositiveExtrusion || ms.isCoordinated || st.useG0Segmentation))
			{
				// This kinematics approximates linear motion by means of segmentation
				float moveLengthSquared = fsquare(ms.currentUserPosition[X_AXIS] - initialUserPosition[X_AXIS]) + fsquare(ms.currentUserPosition[Y_AXIS] - initialUserPosition[Y_AXIS]);
				if (st.useZSegmentation)
				{
					moveLengthSquared += fsquare(ms.currentUserPosition[Z_AXIS] - initialUserPosition[Z_AXIS]);
				}
				const float moveLength = fastSqrtf(moveLengthSquared);
				const float moveTime = moveLength/(ms.feedRate * StepClockRate);		// this is a best-case time, often the move will take longer
				ms.totalSegments = (unsigned int)max<long>(1, lrintf(min<float>(moveLength * kin.GetReciprocalMinSegmentLength(), moveTime * kin.GetSegmentsPerSecond())));
			}
			else
			{
				ms.totalSegments = 1;
			}
		...
		}

Obviously, when the XYZ coordinates don't change, moveLength=0, and at the same time, totalSegments=1, which causes the segments to have no effect at all. As a result, the nozzle can only move along a straight line.
Although my current code runs fine, the motion accuracy doesn't seem to be very precise, and my code is also too dirty, so it requires further modifications.

OttoJiang · 25 Oct 2024, 10:30

Currently, when I calculate the moveLength, I use linear interpolation, which means maintaining a constant velocity in the machine coordinate system. However, my actual requirement is for the nozzle to move at a constant speed in the workpiece coordinate system. My previous method leads to discrepancies between the calculated moveLength and the actual value, which is one of the issues. The more significant problem now is that I want the nozzle to move at a given feedrate in the workpiece coordinate system, not at a constant speed in the machine coordinate system. I want to figure out which parts of the code I should modify to address this issue.@JoergS5 @dc42

OttoJiang · 25 Oct 2024, 10:39

GCodes.cpp

if (st.useSegmentation && simulationMode != SimulationMode::normal && (ms.hasPositiveExtrusion || ms.isCoordinated || st.useG0Segmentation))
{
    if (!(strcmp(kin.GetName(), "robot")) && st.useZSegmentation)
    {
        const MessageType mt = (MessageType)(gb.GetResponseMessageType() | PushFlag);
        // Initialize the starting and ending XYZ coordinates
        float startPos[3] = {initialUserPosition[X_AXIS], initialUserPosition[Y_AXIS], initialUserPosition[Z_AXIS]}; // Example start point
        float endPos[3] = {ms.currentUserPosition[X_AXIS], ms.currentUserPosition[Y_AXIS], ms.currentUserPosition[Z_AXIS]};   // Example end point

        // Initialize the starting and ending BC rotation angles
        float startBeta = initialUserPosition[3];  // Example start Beta angle (around the Y-axis)
        float endBeta = ms.currentUserPosition[3];    // Example end Beta angle
        float startGamma = initialUserPosition[4]; // Example start Gamma angle (around the Z-axis)
        float endGamma = ms.currentUserPosition[4];   // Example end Gamma angle

        // Set the number of steps, the more steps, the more accurate the calculation
        int steps = 1000; // Example number of steps, 1000 steps

        // Call the function to calculate the total path length
        float moveLength = calculateTotalPathLength(startPos, endPos, startBeta, endBeta, startGamma, endGamma, steps);
        const float moveTime = moveLength/(ms.feedRate * StepClockRate);    // this is a best-case time, often the move will take longer
        ms.totalSegments = (unsigned int)max<long>(1, lrintf(min<float>(moveLength * kin.GetReciprocalMinSegmentLength(), moveTime * kin.GetSegmentsPerSecond())));
        reprap.GetPlatform().MessageF(mt,"1; moveLength:%.4f; totalSegments:%d\n", moveLength, ms.totalSegments);
    }
    else
    {
        // This kinematics approximates linear motion by means of segmentation
        float moveLengthSquared = fsquare(ms.currentUserPosition[X_AXIS] - initialUserPosition[X_AXIS]) + fsquare(ms.currentUserPosition[Y_AXIS] - initialUserPosition[Y_AXIS]);
        if (st.useZSegmentation)
        {
            moveLengthSquared += fsquare(ms.currentUserPosition[Z_AXIS] - initialUserPosition[Z_AXIS]);
        }
        const float moveLength = fastSqrtf(moveLengthSquared);
        const float moveTime = moveLength/(ms.feedRate * StepClockRate);    // this is a best-case time, often the move will take longer
        ms.totalSegments = (unsigned int)max<long>(1, lrintf(min<float>(moveLength * kin.GetReciprocalMinSegmentLength(), moveTime * kin.GetSegmentsPerSecond())));
        const MessageType mt = (MessageType)(gb.GetResponseMessageType() | PushFlag);
        reprap.GetPlatform().MessageF(mt,"2; moveLength:%.4f; totalSegments:%d\n", moveLength, ms.totalSegments);
    }
}
else
{
    ms.totalSegments = 1;
}

GCodes.h

const float radiansToDegrees = 57.295784f;

// Rotation matrix R_B (rotation around the Y-axis)
void rotationMatrixB(float beta, float matrix[3][3]) {
    float rad_beta = beta/radiansToDegrees;
    matrix[0][0] = cos(rad_beta); matrix[0][1] = 0; matrix[0][2] = sin(rad_beta);
    matrix[1][0] = 0; matrix[1][1] = 1; matrix[1][2] = 0;
    matrix[2][0] = -sin(rad_beta); matrix[2][1] = 0; matrix[2][2] = cos(rad_beta);
}

// Rotation matrix R_C (rotation around the Z-axis)
void rotationMatrixC(float gamma, float matrix[3][3]) {
    float rad_gamma = gamma/radiansToDegrees;
    matrix[0][0] = cos(rad_gamma); matrix[0][1] = -sin(rad_gamma); matrix[0][2] = 0;
    matrix[1][0] = sin(rad_gamma); matrix[1][1] = cos(rad_gamma); matrix[1][2] = 0;
    matrix[2][0] = 0; matrix[2][1] = 0; matrix[2][2] = 1;
}

// Matrix multiplication
void multiplyMatrix(float A[3][3], float B[3][3], float result[3][3]) {
    for (int i = 0; i < 3; ++i) {
        for (int j = 0; j < 3; ++j) {
            result[i][j] = 0;
            for (int k = 0; k < 3; ++k) {
                result[i][j] += A[i][k] * B[k][j];
            }
        }
    }
}

// Matrix-vector multiplication
void multiplyMatrixVector(float matrix[3][3], const float vec[3], float result[3]) {
    for (int i = 0; i < 3; ++i) {
        result[i] = 0;
        for (int j = 0; j < 3; ++j) {
            result[i] += matrix[i][j] * vec[j];
        }
    }
}

// Calculate the Euclidean distance between two points
float distance(const float point1[3], const float point2[3]) {
    return fastSqrtf(fsquare(point2[0] - point1[0]) + fsquare(point2[1] - point1[1]) + fsquare(point2[2] - point1[2]));
}

// Interpolation calculation (linear interpolation)
float interpolate(float start, float end, float t) {
    return start + t * (end - start);
}

// Calculate the total path length
float calculateTotalPathLength(
    const float startPos[3], const float endPos[3],
    float startBeta, float endBeta, float startGamma, float endGamma, int steps)
{
    float totalLength = 0.0;

    float prevPosition[3] = {startPos[0], startPos[1], startPos[2]};
    float prevBeta = startBeta;
    float prevGamma = startGamma;

    // Rotation matrix cache
    float prerotationMatrixB_i[3][3], prerotationMatrixC_i[3][3], prerotationMatrixTotal_i[3][3];
    float rotationMatrixB_i[3][3], rotationMatrixC_i[3][3], rotationMatrixTotal_i[3][3];
    float prevMachinePosition[3], currentMachinePosition[3];

    for (int i = 1; i <= steps; ++i) {
        float t = (float)i / steps;

        // Interpolate the current position
        float currentPosition[3] = {
            interpolate(startPos[0], endPos[0], t),
            interpolate(startPos[1], endPos[1], t),
            interpolate(startPos[2], endPos[2], t)
        };

        float currentBeta = interpolate(startBeta, endBeta, t);
        float currentGamma = interpolate(startGamma, endGamma, t);

        rotationMatrixB(prevBeta, prerotationMatrixB_i);
        rotationMatrixC(prevGamma, prerotationMatrixC_i);
        multiplyMatrix(prerotationMatrixC_i, prerotationMatrixB_i, prerotationMatrixTotal_i);
        // Calculate the current rotation matrix
        rotationMatrixB(currentBeta, rotationMatrixB_i);
        rotationMatrixC(currentGamma, rotationMatrixC_i);
        multiplyMatrix(rotationMatrixC_i, rotationMatrixB_i, rotationMatrixTotal_i);

        // Calculate the current step's position
        multiplyMatrixVector(prerotationMatrixTotal_i, prevPosition, prevMachinePosition);
        multiplyMatrixVector(rotationMatrixTotal_i, currentPosition, currentMachinePosition);

        // Calculate the distance between the two points
        totalLength += distance(prevMachinePosition, currentMachinePosition);

        // Update the previous point
        prevPosition[0] = currentPosition[0];
        prevPosition[1] = currentPosition[1];
        prevPosition[2] = currentPosition[2];
        prevBeta = currentBeta;
        prevGamma = currentGamma;
    }

    return totalLength;
}

I only considered XYZBC printers.

droftarts · 25 Oct 2024, 11:01

@OttoJiang This is @JoergS5 and @dc42's area of expertise, not mine!

Ian

OttoJiang · 25 Oct 2024, 11:07

@droftarts Ok, I got it, sry

droftarts · 25 Oct 2024, 11:08

@OttoJiang No need to apologise, it's just that I am not a programmer.

Ian

JoergS5 · 27 Oct 2024, 08:25

@OttoJiang said in voron5x with RTCP:

It should actually move in a curve to keep the nozzle's XYZ coordinates in the workpiece coordinate system constant during the movement.

Hello,

correction of the XYZ, so the nozzle stays at the position when BC changes, is exactly what the code shall do, and it worked in my tests. Please tell me where you mean the error in the code lies.

Your other post, you want constant velocity:
Segmentation works to linearly segment every axis movement/rotation angle. To achieve what you want, you need a change of the segmentation code. First to change is the BC angle change. As an example, to rotate in 10 segments 45 degrees, segmentation is 4.5 degrees for every segment. But needed is something like the Slerp algorithm to divide the BC into a constant velocity. But the code needed is located outside of the kinematics code. Currently angles are simply divided for segmentation, B divided and C divided.

(to be more exact: you cannot make all rotation movements with BC alone, you would need 3 rotation axes. There is an article to approach the correct movement with BC, missing the A axis, with minimal error. The code uses Slerp code also. Slerp is easiest calculated using Quaternions)

To make it worse, if you need true constant velocity, you must know your path and combine the movements of the linear axes with the rotation Slerp-based calcuation and then calculate the segmentation needes of every axis. Calcuation of the exact path needs a lot of mathematical calculation. It is better made in advance by a slicer and sending the single segments as G-Code, instead of letting Duet calculate the segmentation. You need in the range of 10 microseconds for every segment to have an efficient movement. (You want a low millisecond for a movement, with maybe 100 segments). The current code is in this range, but a path movement (including BSpline, Bezier and such) calculation will add time.

The current RRF code is based on time framed calculation: how many steps needs every axis, then put them together into the time frame, then try to send one step signal to as many axes as possible at the same time (Duet sends one signal to multiple motor drivers). You requirement needs separate signal calcuations and step signals for every axis at his own, each one at a given time. It's not impossable, but needs more caculating power.

JoergS5 · 27 Oct 2024, 08:30

@OttoJiang

I am confused a bit by your mp4 video file, because the printer behaves exactly as expected. Is the video with your bug code correction?

Maybe you means the Z moving up and down a bit during the movement. This may also be because of Z backlash *) in the construction. How is your Z axis built? It needs to be without backlash. The requirement of Z movement is much higher now than a simple movement in one direction for a 3-axis printer.

*) you will probably know what I mean with backlash. But for other readers: I mean when the Z axis motor changes direction, some steps are lost because there is play in the gear/ball gears/axes with the effect that the opposite movement is later than expected. Some steps are lost. The 3-axis printers don't have backlash because they move only upward and gravity assures that the gear/beal gear are always at one flange of the machanics.

OttoJiang · 27 Oct 2024, 16:09

@JoergS5 The video shows the result after I corrected the code bug. Before fixing the bug, the nozzle didn’t follow the needle movement on the table but instead traveled in a straight line from the starting point to the endpoint.

OttoJiang · 27 Oct 2024, 16:41

@JoergS5 Your code indeed implements the correct five-axis kinematic calculations. However, in the DoStraightMove function of the RRF source code (GCodes.cpp), the segmentation is calculated after determining the moveLength.

// This kinematics approximates linear motion by means of segmentation
float moveLengthSquared = fsquare(ms.currentUserPosition[X_AXIS] - initialUserPosition[X_AXIS]) + fsquare(ms.currentUserPosition[Y_AXIS] - initialUserPosition[Y_AXIS]);
if (st.useZSegmentation)
{
	moveLengthSquared += fsquare(ms.currentUserPosition[Z_AXIS] - initialUserPosition[Z_AXIS]);
}
const float moveLength = fastSqrtf(moveLengthSquared);
const float moveTime = moveLength/(ms.feedRate * StepClockRate);		// this is a best-case time, often the move will take longer
ms.totalSegments = (unsigned int)max<long>(1, lrintf(min<float>(moveLength * kin.GetReciprocalMinSegmentLength(), moveTime * kin.GetSegmentsPerSecond())));

With the introduction of the BC axis, the movement becomes nonlinear. When the XYZ coordinates remain unchanged, the original DoStraightMove function would interpret the moveLength as 0, resulting in a calculated segmentation of 1. Therefore, we need to calculate the true moveLength to ensure that the correct segmentation is derived when the XYZ coordinates do not change.

JoergS5 · 27 Oct 2024, 17:20

@OttoJiang said in voron5x with RTCP:

This kinematics approximates linear motion by means of segmentation

thank you that you found the solution of a problem I was not aware of.

@dc42 could you please add this code to the GCodes part, maybe with some flag condition for specific kinematics?

OttoJiang · 27 Oct 2024, 17:51

@JoergS5 I was wondering if we could collaborate to maintain the five-axis kinematics code for RepRapFirmware and submit pull requests on GitHub. I am a master’s student in the School of Mechanical Engineering at Zhejiang University, currently researching five-axis printing. I believe contributing such code to RRF would be both interesting and meaningful.

JoergS5 · 28 Oct 2024, 15:53

@OttoJiang I don't recommend it, because this kinematics is only one of several ones of robotics kinematics. When they are complete, we can github it.

As an overview please see https://docs.duet3d.com/User_manual/Machine_configuration/Configuring_RepRapFirmware_for_a_Robot_printer with additional documentation pages.