diff --git a/src/Movement/Kinematics/HangprinterKinematics.cpp b/src/Movement/Kinematics/HangprinterKinematics.cpp index 48eafcda1e..6c282bcf22 100644 --- a/src/Movement/Kinematics/HangprinterKinematics.cpp +++ b/src/Movement/Kinematics/HangprinterKinematics.cpp @@ -99,6 +99,7 @@ void HangprinterKinematics::Init() noexcept constexpr float DefaultTorqueConstants[HANGPRINTER_AXES] = { 0.0F }; ARRAY_INIT(anchors, DefaultAnchors); + anchorMode = HangprinterAnchorMode::LastOnTop; printRadius = DefaultPrintRadius; spoolBuildupFactor = DefaultSpoolBuildupFactor; ARRAY_INIT(spoolRadii, DefaultSpoolRadii); @@ -129,16 +130,15 @@ void HangprinterKinematics::Recalc() noexcept // This is the difference between a "line length" and a "line position" // "line length" == ("line position" + "line length in origin") - distancesOrigin[A_AXIS] = fastSqrtf(fsquare(anchors[A_AXIS][0]) + fsquare(anchors[A_AXIS][1]) + fsquare(anchors[A_AXIS][2])); - distancesOrigin[B_AXIS] = fastSqrtf(fsquare(anchors[B_AXIS][0]) + fsquare(anchors[B_AXIS][1]) + fsquare(anchors[B_AXIS][2])); - distancesOrigin[C_AXIS] = fastSqrtf(fsquare(anchors[C_AXIS][0]) + fsquare(anchors[C_AXIS][1]) + fsquare(anchors[C_AXIS][2])); - distancesOrigin[D_AXIS] = fastSqrtf(fsquare(anchors[D_AXIS][0]) + fsquare(anchors[D_AXIS][1]) + fsquare(anchors[D_AXIS][2])); - + for (size_t i = 0; i < HANGPRINTER_AXES; ++i) + { + distancesOrigin[i] = fastSqrtf(fsquare(anchors[i][0]) + fsquare(anchors[i][1]) + fsquare(anchors[i][2])); + } //// Line buildup compensation float stepsPerUnitTimesRTmp[HANGPRINTER_AXES] = { 0.0 }; Platform& platform = reprap.GetPlatform(); // No const because we want to set drive steper per unit - for (size_t i = 0; i < HANGPRINTER_AXES; i++) + for (size_t i = 0; i < HANGPRINTER_AXES; ++i) { const uint8_t driver = platform.GetAxisDriversConfig(i).driverNumbers[0].localDriver; // Only supports single driver bool dummy; @@ -203,11 +203,10 @@ bool HangprinterKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, const if (mCode == 669) { const bool seenNonGeometry = TryConfigureSegmentation(gb); - gb.TryGetFloatArray('A', 3, anchors[A_AXIS], seen); - gb.TryGetFloatArray('B', 3, anchors[B_AXIS], seen); - gb.TryGetFloatArray('C', 3, anchors[C_AXIS], seen); - gb.TryGetFloatArray('D', 3, anchors[D_AXIS], seen); - + for (size_t i = 0; i < HANGPRINTER_AXES; ++i) + { + gb.TryGetFloatArray(ANCHOR_CHARS[i], 3, anchors[i], seen); + } if (gb.Seen('P')) { printRadius = gb.GetPositiveFValue(); @@ -221,22 +220,22 @@ bool HangprinterKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, const else if (!seenNonGeometry && !gb.Seen('K')) { Kinematics::Configure(mCode, gb, reply, error); - reply.lcatf( - "A:%.2f, %.2f, %.2f\n" - "B:%.2f, %.2f, %.2f\n" - "C:%.2f, %.2f, %.2f\n" - "D:%.2f, %.2f, %.2f\n" - "P:Print radius: %.1f", - (double)anchors[A_AXIS][X_AXIS], (double)anchors[A_AXIS][Y_AXIS], (double)anchors[A_AXIS][Z_AXIS], - (double)anchors[B_AXIS][X_AXIS], (double)anchors[B_AXIS][Y_AXIS], (double)anchors[B_AXIS][Z_AXIS], - (double)anchors[C_AXIS][X_AXIS], (double)anchors[C_AXIS][Y_AXIS], (double)anchors[C_AXIS][Z_AXIS], - (double)anchors[D_AXIS][X_AXIS], (double)anchors[D_AXIS][Y_AXIS], (double)anchors[D_AXIS][Z_AXIS], - (double)printRadius - ); + for (size_t i = 0; i < HANGPRINTER_AXES; ++i) + { + reply.lcatf("%c:%.2f, %.2f, %.2f", + ANCHOR_CHARS[i], (double)anchors[i][X_AXIS], (double)anchors[i][Y_AXIS], (double)anchors[i][Z_AXIS]); + } + reply.lcatf("P:Print radius: %.1f", (double)printRadius); } } else if (mCode == 666) { + // 0=None, 1=last-top, 2=all-top, 3-half-top, etc + uint32_t unsignedAnchorMode = (uint32_t)anchorMode; + gb.TryGetUIValue('A', unsignedAnchorMode, seen); + if (unsignedAnchorMode <= (uint32_t)HangprinterAnchorMode::AllOnTop) { + anchorMode = (HangprinterAnchorMode)unsignedAnchorMode; + } gb.TryGetFValue('Q', spoolBuildupFactor, seen); gb.TryGetFloatArray('R', HANGPRINTER_AXES, spoolRadii, seen); gb.TryGetUIArray('U', HANGPRINTER_AXES, mechanicalAdvantage, seen); @@ -256,36 +255,71 @@ bool HangprinterKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, const } else { - reply.printf( - "M666 Q%.4f\n" - "R%.2f:%.2f:%.2f:%.2f\n" - "U%d:%d:%d:%d\n" - "O%d:%d:%d:%d\n" - "L%d:%d:%d:%d\n" - "H%d:%d:%d:%d\n" - "J%d:%d:%d:%d\n" - "W%.2f\n" - "S%.2f\n" - "I%.1f:%.1f:%.1f:%.1f\n" - "X%.1f:%.1f:%.1f:%.1f\n" - "Y%.1f:%.1f:%.1f:%.1f\n" - "T%.1f\n" - "C%.4f:%.4f:%.4f:%.4f", - (double)spoolBuildupFactor, - (double)spoolRadii[A_AXIS], (double)spoolRadii[B_AXIS], (double)spoolRadii[C_AXIS], (double)spoolRadii[D_AXIS], - (int)mechanicalAdvantage[A_AXIS], (int)mechanicalAdvantage[B_AXIS], (int)mechanicalAdvantage[C_AXIS], (int)mechanicalAdvantage[D_AXIS], - (int)linesPerSpool[A_AXIS], (int)linesPerSpool[B_AXIS], (int)linesPerSpool[C_AXIS], (int)linesPerSpool[D_AXIS], - (int)motorGearTeeth[A_AXIS], (int)motorGearTeeth[B_AXIS], (int)motorGearTeeth[C_AXIS], (int)motorGearTeeth[D_AXIS], - (int)spoolGearTeeth[A_AXIS], (int)spoolGearTeeth[B_AXIS], (int)spoolGearTeeth[C_AXIS], (int)spoolGearTeeth[D_AXIS], - (int)fullStepsPerMotorRev[A_AXIS], (int)fullStepsPerMotorRev[B_AXIS], (int)fullStepsPerMotorRev[C_AXIS], (int)fullStepsPerMotorRev[D_AXIS], - (double)moverWeight_kg, - (double)springKPerUnitLength, - (double)minPlannedForce_Newton[A_AXIS], (double)minPlannedForce_Newton[B_AXIS], (double)minPlannedForce_Newton[C_AXIS], (double)minPlannedForce_Newton[D_AXIS], - (double)maxPlannedForce_Newton[A_AXIS], (double)maxPlannedForce_Newton[B_AXIS], (double)maxPlannedForce_Newton[C_AXIS], (double)maxPlannedForce_Newton[D_AXIS], - (double)guyWireLengths[A_AXIS], (double)guyWireLengths[B_AXIS], (double)guyWireLengths[C_AXIS], (double)guyWireLengths[D_AXIS], - (double)targetForce_Newton, - (double)torqueConstants[A_AXIS], (double)torqueConstants[B_AXIS], (double)torqueConstants[C_AXIS], (double)torqueConstants[D_AXIS] - ); + reply.printf("M666 A%u Q%.4f\n", (unsigned)anchorMode, (double)spoolBuildupFactor); + + reply.lcatf("R%.2f", (double)spoolRadii[0]); + for (size_t i = 1; i < HANGPRINTER_AXES; ++i) + { + reply.catf(":%.2f", (double)spoolRadii[i]); + } + + reply.lcatf("U%d", (int)mechanicalAdvantage[0]); + for (size_t i = 1; i < HANGPRINTER_AXES; ++i) + { + reply.catf(":%d", (int)mechanicalAdvantage[i]); + } + + reply.lcatf("O%d", (int)linesPerSpool[0]); + for (size_t i = 1; i < HANGPRINTER_AXES; ++i) + { + reply.catf(":%d", (int)linesPerSpool[i]); + } + + reply.lcatf("L%d", (int)motorGearTeeth[0]); + for (size_t i = 1; i < HANGPRINTER_AXES; ++i) + { + reply.catf(":%d", (int)motorGearTeeth[i]); + } + + reply.lcatf("H%d", (int)spoolGearTeeth[0]); + for (size_t i = 1; i < HANGPRINTER_AXES; ++i) + { + reply.catf(":%d", (int)spoolGearTeeth[i]); + } + + reply.lcatf("J%d", (int)fullStepsPerMotorRev[0]); + for (size_t i = 1; i < HANGPRINTER_AXES; ++i) + { + reply.catf(":%d", (int)fullStepsPerMotorRev[i]); + } + reply.lcatf("W%.2f\n", (double)moverWeight_kg); + reply.lcatf("S%.2f\n", (double)springKPerUnitLength); + + reply.lcatf("I%.1f", (double)minPlannedForce_Newton[0]); + for (size_t i = 1; i < HANGPRINTER_AXES; ++i) + { + reply.catf(":%.1f", (double)minPlannedForce_Newton[i]); + } + + reply.lcatf("X%.1f", (double)maxPlannedForce_Newton[0]); + for (size_t i = 1; i < HANGPRINTER_AXES; ++i) + { + reply.catf(":%.1f", (double)maxPlannedForce_Newton[i]); + } + + reply.lcatf("Y%.1f", (double)guyWireLengths[0]); + for (size_t i = 1; i < HANGPRINTER_AXES; ++i) + { + reply.catf(":%.1f", (double)guyWireLengths[i]); + } + reply.lcatf("T%.1f\n", (double)targetForce_Newton); + + reply.lcatf("C%.4f", (double)torqueConstants[0]); + for (size_t i = 1; i < HANGPRINTER_AXES; ++i) + { + reply.catf(":%.4f", (double)torqueConstants[i]); + } + } } else @@ -300,11 +334,9 @@ bool HangprinterKinematics::CartesianToMotorSteps(const float machinePos[], cons size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const noexcept { float distances[HANGPRINTER_AXES]; - distances[A_AXIS] = hyp3(machinePos, anchors[A_AXIS]); - distances[B_AXIS] = hyp3(machinePos, anchors[B_AXIS]); - distances[C_AXIS] = hyp3(machinePos, anchors[C_AXIS]); - distances[D_AXIS] = hyp3(machinePos, anchors[D_AXIS]); - + for (size_t i{0}; i < HANGPRINTER_AXES; ++i) { + distances[i] = hyp3(machinePos, anchors[i]); + } float springKs[HANGPRINTER_AXES]; for (size_t i{0}; i < HANGPRINTER_AXES; ++i) { @@ -325,10 +357,10 @@ bool HangprinterKinematics::CartesianToMotorSteps(const float machinePos[], cons linePos[i] = relaxedSpringLengths[i] - relaxedSpringLengthsOrigin[i]; } - motorPos[A_AXIS] = lrintf(k0[A_AXIS] * (fastSqrtf(spoolRadiiSq[A_AXIS] + linePos[A_AXIS] * k2[A_AXIS]) - spoolRadii[A_AXIS])); - motorPos[B_AXIS] = lrintf(k0[B_AXIS] * (fastSqrtf(spoolRadiiSq[B_AXIS] + linePos[B_AXIS] * k2[B_AXIS]) - spoolRadii[B_AXIS])); - motorPos[C_AXIS] = lrintf(k0[C_AXIS] * (fastSqrtf(spoolRadiiSq[C_AXIS] + linePos[C_AXIS] * k2[C_AXIS]) - spoolRadii[C_AXIS])); - motorPos[D_AXIS] = lrintf(k0[D_AXIS] * (fastSqrtf(spoolRadiiSq[D_AXIS] + linePos[D_AXIS] * k2[D_AXIS]) - spoolRadii[D_AXIS])); + for (size_t i = 0; i < HANGPRINTER_AXES; ++i) + { + motorPos[i] = lrintf(k0[i] * (fastSqrtf(spoolRadiiSq[i] + linePos[i] * k2[i]) - spoolRadii[i])); + } return true; } @@ -419,17 +451,53 @@ static bool isSameSide(float const v0[3], float const v1[3], float const v2[3], return dot0*dot1 > 0.0F; } -static bool isInsideTetrahedron(float const point[3], float const tetrahedron[4][3]){ - return isSameSide(tetrahedron[0], tetrahedron[1], tetrahedron[2], tetrahedron[3], point) && - isSameSide(tetrahedron[2], tetrahedron[1], tetrahedron[3], tetrahedron[0], point) && - isSameSide(tetrahedron[2], tetrahedron[3], tetrahedron[0], tetrahedron[1], point) && - isSameSide(tetrahedron[0], tetrahedron[3], tetrahedron[1], tetrahedron[2], point); +bool HangprinterKinematics::IsInsidePyramidSides(float const coords[3]) const noexcept +{ + bool reachable = true; + + // Check all the planes defined by triangle sides in the pyramid + for (size_t i = 0; reachable && i < HANGPRINTER_AXES - 1; ++i) { + reachable = reachable && isSameSide(anchors[i], anchors[(i+1) % (HANGPRINTER_AXES - 1)], anchors[HANGPRINTER_AXES - 1], anchors[(i+2) % (HANGPRINTER_AXES - 1)], coords); + } + return reachable; } +bool HangprinterKinematics::IsInsidePrismSides(float const coords[3], unsigned const discount_last) const noexcept +{ + bool reachable = true; + + // For each side of the base, check the plane formed by side and another point bellow them in z. + for (size_t i = 0; reachable && i < HANGPRINTER_AXES - discount_last; ++i) { + float const lower_point[3] = {anchors[i][0], anchors[i][1], anchors[i][2] - 1}; + reachable = reachable && isSameSide(anchors[i], anchors[(i+1) % (HANGPRINTER_AXES - 1)], lower_point, anchors[(i+2) % (HANGPRINTER_AXES - 1)], coords); + } + return reachable; +} + +// For each triangle side in a pseudo-pyramid, check if the point is inside the pyramid (Except for the base) +// Also check that any point below the line between two exterior anchors (all anchors are exterior except for the last one) +// is in the "inside part" all the way down to min_Z, however low it may be. +// To further limit the movements in the X and Y axes one can simply set a smaller print radius. bool HangprinterKinematics::IsReachable(float axesCoords[MaxAxes], AxesBitmap axes) const noexcept /*override*/ { float const coords[3] = {axesCoords[X_AXIS], axesCoords[Y_AXIS], axesCoords[Z_AXIS]}; - return isInsideTetrahedron(coords, anchors); + bool reachable = true; + + switch (anchorMode) { + case HangprinterAnchorMode::None: + return true; + + // This reaches a pyramid on top of the lower prism if the bed is below the lower anchors + case HangprinterAnchorMode::LastOnTop: + default: + reachable = IsInsidePyramidSides(coords); + return reachable && IsInsidePrismSides(coords, 1); + + case HangprinterAnchorMode::AllOnTop: + return IsInsidePrismSides(coords, 0); + }; + + return reachable; } // Limit the Cartesian position that the user wants to move to returning true if we adjusted the position @@ -532,69 +600,115 @@ AxesBitmap HangprinterKinematics::MustBeHomedAxes(AxesBitmap axesMoving, bool di // Write the parameters to a file, returning true if success bool HangprinterKinematics::WriteCalibrationParameters(FileStore *f) const noexcept { - bool ok = f->Write("; Hangprinter parameters\n"); - if (ok) + bool ok = false; + String<255> scratchString; + + scratchString.printf("; Hangprinter parameters\n"); + scratchString.printf("M669 K6 "); + ok = f->Write(scratchString.c_str()); + if (!ok) return false; + + for (size_t i = 0; i < HANGPRINTER_AXES; ++i) { - String<100> scratchString; - scratchString.printf("M669 K6 A%.3f:%.3f:%.3f B%.3f:%.3f:%.3f", - (double)anchors[A_AXIS][X_AXIS], (double)anchors[A_AXIS][Y_AXIS], (double)anchors[A_AXIS][Z_AXIS], - (double)anchors[B_AXIS][X_AXIS], (double)anchors[B_AXIS][Y_AXIS], (double)anchors[B_AXIS][Z_AXIS]); - ok = f->Write(scratchString.c_str()); - if (ok) - { - scratchString.printf(" C%.3f:%.3f:%.3f D%.3f:%.3f:%.3f P%.1f\n", - (double)anchors[C_AXIS][X_AXIS], (double)anchors[C_AXIS][Y_AXIS], (double)anchors[C_AXIS][Z_AXIS], - (double)anchors[D_AXIS][X_AXIS], (double)anchors[D_AXIS][Y_AXIS], (double)anchors[D_AXIS][Z_AXIS], - (double)printRadius); - ok = f->Write(scratchString.c_str()); - if (ok) - { - scratchString.printf("M666 Q%.6f R%.3f:%.3f:%.3f:%.3f U%d:%d:%d:%d", - (double)spoolBuildupFactor, (double)spoolRadii[A_AXIS], - (double)spoolRadii[B_AXIS], (double)spoolRadii[C_AXIS], (double)spoolRadii[D_AXIS], - (int)mechanicalAdvantage[A_AXIS], (int)mechanicalAdvantage[B_AXIS], - (int)mechanicalAdvantage[C_AXIS], (int)mechanicalAdvantage[D_AXIS] - ); - ok = f->Write(scratchString.c_str()); - if (ok) - { - scratchString.printf(" O%d:%d:%d:%d L%d:%d:%d:%d H%d:%d:%d:%d J%d:%d:%d:%d", - (int)linesPerSpool[A_AXIS], (int)linesPerSpool[B_AXIS], - (int)linesPerSpool[C_AXIS], (int)linesPerSpool[D_AXIS], - (int)motorGearTeeth[A_AXIS], (int)motorGearTeeth[B_AXIS], - (int)motorGearTeeth[C_AXIS], (int)motorGearTeeth[D_AXIS], - (int)spoolGearTeeth[A_AXIS], (int)spoolGearTeeth[B_AXIS], - (int)spoolGearTeeth[C_AXIS], (int)spoolGearTeeth[D_AXIS], - (int)fullStepsPerMotorRev[A_AXIS], (int)fullStepsPerMotorRev[B_AXIS], - (int)fullStepsPerMotorRev[C_AXIS], (int)fullStepsPerMotorRev[D_AXIS] - ); - ok = f->Write(scratchString.c_str()); - if (ok) - { - scratchString.printf(" W%.2f S%.2f I%.1f:%.1f:%.1f:%.1f X%.1f:%.1f:%.1f:%.1f", - (double)moverWeight_kg, (double)springKPerUnitLength, - (double)minPlannedForce_Newton[A_AXIS], (double)minPlannedForce_Newton[B_AXIS], - (double)minPlannedForce_Newton[C_AXIS], (double)minPlannedForce_Newton[D_AXIS], - (double)maxPlannedForce_Newton[A_AXIS], (double)maxPlannedForce_Newton[B_AXIS], - (double)maxPlannedForce_Newton[C_AXIS], (double)maxPlannedForce_Newton[D_AXIS] - ); - ok = f->Write(scratchString.c_str()); - if (ok) - { - scratchString.printf(" Y%.1f:%.1f:%.1f:%.1f T%.1f C%.4f:%.4f:%.4f:%.4f\n", - (double)guyWireLengths[A_AXIS], (double)guyWireLengths[B_AXIS], - (double)guyWireLengths[C_AXIS], (double)guyWireLengths[D_AXIS], - (double)targetForce_Newton, - (double)torqueConstants[A_AXIS], (double)torqueConstants[B_AXIS], - (double)torqueConstants[C_AXIS], (double)torqueConstants[D_AXIS] - ); - ok = f->Write(scratchString.c_str()); - } - } - } - } - } + scratchString.catf("%c%.3f:%.3f:%.3f ", ANCHOR_CHARS[i], (double)anchors[i][X_AXIS], (double)anchors[i][Y_AXIS], (double)anchors[i][Z_AXIS]); } + ok = f->Write(scratchString.c_str()); + if (!ok) return false; + + scratchString.printf(" P%.1f", (double)printRadius); + ok = f->Write(scratchString.c_str()); + if (!ok) return false; + + scratchString.printf("M666 A%u Q%.6f ", (unsigned)anchorMode, (double)spoolBuildupFactor); + ok = f->Write(scratchString.c_str()); + if (!ok) return false; + + scratchString.printf("R%.3f", (double)spoolRadii[0]); + for (size_t i = 1; i < HANGPRINTER_AXES; ++i) + { + scratchString.catf(":%.3f", (double)spoolRadii[i]); + } + ok = f->Write(scratchString.c_str()); + if (!ok) return false; + + scratchString.printf(" U%.3f", (double)mechanicalAdvantage[0]); + for (size_t i = 1; i < HANGPRINTER_AXES; ++i) + { + scratchString.catf(":%.3f", (double)mechanicalAdvantage[i]); + } + ok = f->Write(scratchString.c_str()); + if (!ok) return false; + + scratchString.printf(" O%.3f", (double)linesPerSpool[0]); + for (size_t i = 1; i < HANGPRINTER_AXES; ++i) + { + scratchString.catf(":%.3f", (double)linesPerSpool[i]); + } + ok = f->Write(scratchString.c_str()); + if (!ok) return false; + + scratchString.printf(" L%.3f", (double)motorGearTeeth[0]); + for (size_t i = 1; i < HANGPRINTER_AXES; ++i) + { + scratchString.catf(":%.3f", (double)motorGearTeeth[i]); + } + ok = f->Write(scratchString.c_str()); + if (!ok) return false; + + scratchString.printf(" H%.3f", (double)spoolGearTeeth[0]); + for (size_t i = 1; i < HANGPRINTER_AXES; ++i) + { + scratchString.catf(":%.3f", (double)spoolGearTeeth[i]); + } + ok = f->Write(scratchString.c_str()); + if (!ok) return false; + + scratchString.printf(" J%.3f", (double)fullStepsPerMotorRev[0]); + for (size_t i = 1; i < HANGPRINTER_AXES; ++i) + { + scratchString.catf(":%.3f", (double)fullStepsPerMotorRev[i]); + } + ok = f->Write(scratchString.c_str()); + + scratchString.printf(" W%.2f S%.2f", (double)moverWeight_kg, (double)springKPerUnitLength); + ok = f->Write(scratchString.c_str()); + if (!ok) return false; + + scratchString.printf(" I%.1f", (double)minPlannedForce_Newton[0]); + for (size_t i = 1; i < HANGPRINTER_AXES; ++i) + { + scratchString.catf(":%.1f", (double)minPlannedForce_Newton[i]); + } + ok = f->Write(scratchString.c_str()); + if (!ok) return false; + + scratchString.printf(" X%.1f", (double)maxPlannedForce_Newton[0]); + for (size_t i = 1; i < HANGPRINTER_AXES; ++i) + { + scratchString.catf(":%.1f", (double)maxPlannedForce_Newton[i]); + } + ok = f->Write(scratchString.c_str()); + if (!ok) return false; + + scratchString.printf(" Y%.1f", (double)guyWireLengths[0]); + for (size_t i = 1; i < HANGPRINTER_AXES; ++i) + { + scratchString.catf(":%.1f", (double)guyWireLengths[i]); + } + ok = f->Write(scratchString.c_str()); + if (!ok) return false; + + scratchString.printf(" T%.1f", (double)targetForce_Newton); + ok = f->Write(scratchString.c_str()); + if (!ok) return false; + + scratchString.printf(" C%.4f", (double)torqueConstants[0]); + for (size_t i = 1; i < HANGPRINTER_AXES; ++i) + { + scratchString.catf(":%.4f", (double)torqueConstants[i]); + } + ok = f->Write(scratchString.c_str()); + return ok; } @@ -698,10 +812,12 @@ void HangprinterKinematics::ForwardTransform(float const a, float const b, float // Print all the parameters for debugging void HangprinterKinematics::PrintParameters(const StringRef& reply) const noexcept { - reply.printf("Anchor coordinates (%.2f,%.2f,%.2f) (%.2f,%.2f,%.2f) (%.2f,%.2f,%.2f)\n", - (double)anchors[A_AXIS][X_AXIS], (double)anchors[A_AXIS][Y_AXIS], (double)anchors[A_AXIS][Z_AXIS], - (double)anchors[B_AXIS][X_AXIS], (double)anchors[B_AXIS][Y_AXIS], (double)anchors[B_AXIS][Z_AXIS], - (double)anchors[C_AXIS][X_AXIS], (double)anchors[C_AXIS][Y_AXIS], (double)anchors[C_AXIS][Z_AXIS]); + reply.printf("Anchor coordinates"); + for (size_t i = 0; i < HANGPRINTER_AXES; ++i) + { + reply.catf(" (%.2f,%.2f,%.2f)", (double)anchors[i][X_AXIS], (double)anchors[i][Y_AXIS], (double)anchors[i][Z_AXIS]); + } + reply.cat("\n"); } #if DUAL_CAN diff --git a/src/Movement/Kinematics/HangprinterKinematics.h b/src/Movement/Kinematics/HangprinterKinematics.h index 8280e73217..ea4fa8badb 100644 --- a/src/Movement/Kinematics/HangprinterKinematics.h +++ b/src/Movement/Kinematics/HangprinterKinematics.h @@ -12,6 +12,13 @@ #if SUPPORT_HANGPRINTER +// Different modes can be configured for different tradeoffs in terms of printing volumes and speeds +enum class HangprinterAnchorMode { + None, // All is reacheable in None anchor mode as printing volume + LastOnTop, // (Default) Rsults in a pyramid plus a prism below if the lower anchors are above the printing bed + AllOnTop, // Result in a prism (speeds get limited, specially going down in Z) +}; + class HangprinterKinematics : public RoundBedKinematics { public: @@ -55,8 +62,12 @@ class HangprinterKinematics : public RoundBedKinematics protected: DECLARE_OBJECT_MODEL_WITH_ARRAYS + bool IsInsidePyramidSides(float const coords[3]) const noexcept; + bool IsInsidePrismSides(float const coords[3], unsigned const discount_last) const noexcept; + private: // Basic facts about movement system + const char* ANCHOR_CHARS = "ABCD"; static constexpr size_t HANGPRINTER_AXES = 4; static constexpr size_t A_AXIS = 0; static constexpr size_t B_AXIS = 1; @@ -71,6 +82,7 @@ class HangprinterKinematics : public RoundBedKinematics void PrintParameters(const StringRef& reply) const noexcept; // Print all the parameters for debugging // The real defaults are in the cpp file + HangprinterAnchorMode anchorMode = HangprinterAnchorMode::LastOnTop; float printRadius = 0.0F; float anchors[HANGPRINTER_AXES][3] = {{ 0.0, 0.0, 0.0}, { 0.0, 0.0, 0.0},