c1a02b39f720a12f4d3b37c2c95bd2af.mp4
compiled from https://github.com/JoergS5/RepRapFirmware_robot, based on RRF 3.5.1
thanks for @JoergS5's efforts
Best posts made by OttoJiang

voron5x with RTCP

RE: voron5x with RTCP
@JoergS5 I was wondering if we could collaborate to maintain the fiveaxis 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 fiveaxis printing. I believe contributing such code to RRF would be both interesting and meaningful.

RE: 5 axis CoreXYAC Voron ( Robot kinematics) not homing Y axis
@Mafco77 Same problem. It seems that the firmware does not seem to support G1 Hxxx, maybe... I'm not quite sure...
I have a trick which is to use M669 K1 first, then home all axes and use M669 K13 B "CoreXY5AC".
Not perfect but workable. 
RE: voron5x with RTCP
b860ca5040b549c6a01a6eee3e6353da373f17267ac28bce0491e00515ed19fd.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.

RE: voron5x with RTCP
@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 bestcase 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.
Latest posts made by OttoJiang

What's the difference between SBC mode and WIFI mode
I'm wondering if the SBC mode would offload part of the computation to the SBC, thereby improving computational efficiency.

RE: voron5x with RTCP
@JoergS5 I was wondering if we could collaborate to maintain the fiveaxis 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 fiveaxis printing. I believe contributing such code to RRF would be both interesting and meaningful.

RE: voron5x with RTCP
@JoergS5 Your code indeed implements the correct fiveaxis 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 bestcase 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.

RE: voron5x with RTCP
@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.

RE: voron5x with RTCP
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 Yaxis) float endBeta = ms.currentUserPosition[3]; // Example end Beta angle float startGamma = initialUserPosition[4]; // Example start Gamma angle (around the Zaxis) 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 bestcase 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 bestcase 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 Yaxis) 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 Zaxis) 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]; } } } } // Matrixvector 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.

RE: voron5x with RTCP
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

RE: voron5x with RTCP
@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 bestcase 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. 
RE: voron5x with RTCP
b860ca5040b549c6a01a6eee3e6353da373f17267ac28bce0491e00515ed19fd.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.

RE: 5 axis CoreXYAC Voron ( Robot kinematics) not homing Y axis
@Mafco77 Same problem. It seems that the firmware does not seem to support G1 Hxxx, maybe... I'm not quite sure...
I have a trick which is to use M669 K1 first, then home all axes and use M669 K13 B "CoreXY5AC".
Not perfect but workable.