diff --git a/.cproject b/.cproject index 7ee6f1206f..8f26a70214 100644 --- a/.cproject +++ b/.cproject @@ -609,7 +609,7 @@ - + diff --git a/src/Movement/Kinematics/HangprinterKinematics.cpp b/src/Movement/Kinematics/HangprinterKinematics.cpp index 48eafcda1e..486aec2714 100644 --- a/src/Movement/Kinematics/HangprinterKinematics.cpp +++ b/src/Movement/Kinematics/HangprinterKinematics.cpp @@ -14,15 +14,20 @@ #include #include #include +#include #include -constexpr float DefaultAnchors[4][3] = {{ 0.0, -2000.0, -100.0}, +constexpr size_t DefaultNumAnchors = 4; +size_t HangprinterKinematics::numAnchors = DefaultNumAnchors; +constexpr float DefaultAnchors[5][3] = {{ 0.0, -2000.0, -100.0}, { 2000.0, 1000.0, -100.0}, {-2000.0, 1000.0, -100.0}, - { 0.0, 0.0, 3000.0}}; + { 0.0, 0.0, 3000.0}, + { 0.0, 0.0, 0.0}}; constexpr float DefaultPrintRadius = 1500.0; + #if SUPPORT_OBJECT_MODEL // Object model table and functions @@ -43,19 +48,20 @@ constexpr ObjectModelArrayTableEntry HangprinterKinematics::objectModelArrayTabl // 11. Anchors { nullptr, // no lock needed - [] (const ObjectModel *self, const ObjectExplorationContext&) noexcept -> size_t { return HANGPRINTER_AXES; }, + [] (const ObjectModel *self, const ObjectExplorationContext&) noexcept -> size_t { return HANGPRINTER_MAX_ANCHORS; }, [] (const ObjectModel *self, ObjectExplorationContext& context) noexcept -> ExpressionValue { return ExpressionValue(self, 10 | (context.GetLastIndex() << 8), true); } } }; -DEFINE_GET_OBJECT_MODEL_ARRAY_TABLE_WITH_PARENT(HangprinterKinematics, RoundBedKinematics, 10) +// TODO how are this 10 (now 13) and this 11 (now 14) calculated? make it a macro based on HANGPRINTER_MAX_ANCHORS +DEFINE_GET_OBJECT_MODEL_ARRAY_TABLE_WITH_PARENT(HangprinterKinematics, RoundBedKinematics, 13) constexpr ObjectModelTableEntry HangprinterKinematics::objectModelTable[] = { // Within each group, these entries must be in alphabetical order // 0. kinematics members - { "anchors", OBJECT_MODEL_FUNC_ARRAY(11), ObjectModelEntryFlags::none }, - { "name", OBJECT_MODEL_FUNC(self->GetName(true)), ObjectModelEntryFlags::none }, + { "anchors", OBJECT_MODEL_FUNC_ARRAY(14), ObjectModelEntryFlags::none }, + { "name", OBJECT_MODEL_FUNC(self->GetName(true)), ObjectModelEntryFlags::none }, { "printRadius", OBJECT_MODEL_FUNC(self->printRadius, 1), ObjectModelEntryFlags::none }, }; @@ -83,22 +89,24 @@ void HangprinterKinematics::Init() noexcept * In practice you might want to compensate a bit more or a bit less */ constexpr float DefaultSpoolBuildupFactor = 0.007; /* Measure and set spool radii with M669 to achieve better accuracy */ - constexpr float DefaultSpoolRadii[4] = { 75.0, 75.0, 75.0, 75.0}; // HP4 default + constexpr float DefaultSpoolRadii[HANGPRINTER_MAX_ANCHORS] = { 75.0, 75.0, 75.0, 75.0, 75.0}; // HP4 default /* If axis runs lines back through pulley system, set mechanical advantage accordingly with M669 */ - constexpr uint32_t DefaultMechanicalAdvantage[4] = { 2, 2, 2, 4}; // HP4 default - constexpr uint32_t DefaultLinesPerSpool[4] = { 1, 1, 1, 1}; // HP4 default - constexpr uint32_t DefaultMotorGearTeeth[4] = { 20, 20, 20, 20}; // HP4 default - constexpr uint32_t DefaultSpoolGearTeeth[4] = { 255, 255, 255, 255}; // HP4 default - constexpr uint32_t DefaultFullStepsPerMotorRev[4] = { 25, 25, 25, 25}; + constexpr uint32_t DefaultMechanicalAdvantage[HANGPRINTER_MAX_ANCHORS] = { 2, 2, 2, 2, 4}; // HP4 default + constexpr uint32_t DefaultLinesPerSpool[HANGPRINTER_MAX_ANCHORS] = { 1, 1, 1, 1, 1}; // HP4 default + constexpr uint32_t DefaultMotorGearTeeth[HANGPRINTER_MAX_ANCHORS] = { 20, 20, 20, 20, 20}; // HP4 default + constexpr uint32_t DefaultSpoolGearTeeth[HANGPRINTER_MAX_ANCHORS] = { 255, 255, 255, 255, 255}; // HP4 default + constexpr uint32_t DefaultFullStepsPerMotorRev[HANGPRINTER_MAX_ANCHORS] = { 25, 25, 25, 25, 25}; constexpr float DefaultMoverWeight_kg = 0.0F; // Zero disables flex compensation feature. constexpr float DefaultSpringKPerUnitLength = 20000.0F; // Garda 1.1 is somewhere in the range [20000, 100000] - constexpr float DefaultMinPlannedForce_Newton[4] = { 0.0F }; - constexpr float DefaultMaxPlannedForce_Newton[4] = { 70.0F, 70.0F, 70.0F, 70.0F }; - constexpr float DefaultGuyWireLengths[HANGPRINTER_AXES] = { -1.0F }; // If one of these are negative they will be calculated in Recalc() instead + constexpr float DefaultMinPlannedForce_Newton[HANGPRINTER_MAX_ANCHORS] = { 0.0F }; + constexpr float DefaultMaxPlannedForce_Newton[HANGPRINTER_MAX_ANCHORS] = { 70.0F, 70.0F, 70.0F, 70.0F, 70.0F }; + constexpr float DefaultGuyWireLengths[HANGPRINTER_MAX_ANCHORS] = { -1.0F }; // If one of these are negative they will be calculated in Recalc() instead constexpr float DefaultTargetForce_Newton = 20.0F; // 20 chosen quite arbitrarily - constexpr float DefaultTorqueConstants[HANGPRINTER_AXES] = { 0.0F }; + constexpr float DefaultTorqueConstants[HANGPRINTER_MAX_ANCHORS] = { 0.0F }; ARRAY_INIT(anchors, DefaultAnchors); + anchorMode = HangprinterAnchorMode::LastOnTop; + numAnchors = DefaultNumAnchors; printRadius = DefaultPrintRadius; spoolBuildupFactor = DefaultSpoolBuildupFactor; ARRAY_INIT(spoolRadii, DefaultSpoolRadii); @@ -129,16 +137,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 < numAnchors; ++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 }; + float stepsPerUnitTimesRTmp[HANGPRINTER_MAX_ANCHORS] = { 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 < numAnchors; ++i) { const uint8_t driver = platform.GetAxisDriversConfig(i).driverNumbers[0].localDriver; // Only supports single driver bool dummy; @@ -160,25 +167,37 @@ void HangprinterKinematics::Recalc() noexcept } //// Flex compensation + bool oneGuyWireNegative = false; + for (size_t i{0}; i < numAnchors; ++i) { + if (guyWireLengths[i] < 0.0F) { + oneGuyWireNegative = true; + break; + } + } - // If no guy wire lengths are configured, assume a default configuration - // with all spools stationary located at the D anchor - if (guyWireLengths[A_AXIS] < 0.0F or - guyWireLengths[B_AXIS] < 0.0F or - guyWireLengths[C_AXIS] < 0.0F or - guyWireLengths[D_AXIS] < 0.0F) { - guyWireLengths[A_AXIS] = hyp3(anchors[A_AXIS], anchors[D_AXIS]); - guyWireLengths[B_AXIS] = hyp3(anchors[B_AXIS], anchors[D_AXIS]); - guyWireLengths[C_AXIS] = hyp3(anchors[C_AXIS], anchors[D_AXIS]); - guyWireLengths[D_AXIS] = 0.0F; + if (anchorMode == HangprinterAnchorMode::LastOnTop) { + // If no guy wire lengths are configured, assume a default configuration + // with all spools stationary located at the last anchor, + // and that the last anchor is a top anchor + if (oneGuyWireNegative) { + for (size_t i{0}; i < numAnchors - 1; ++i) { + guyWireLengths[i] = hyp3(anchors[i], anchors[numAnchors - 1]); + } + guyWireLengths[numAnchors - 1] = 0.0F; + } + } else { + // Assumes no guyWires in all other cases + for (size_t i{0}; i < numAnchors; ++i) { + guyWireLengths[i] = 0.0F; + } } - for (size_t i{0}; i < HANGPRINTER_AXES; ++i) { + for (size_t i{0}; i < numAnchors; ++i) { springKsOrigin[i] = SpringK(distancesOrigin[i] * mechanicalAdvantage[i] + guyWireLengths[i]); } float constexpr origin[3] = { 0.0F, 0.0F, 0.0F }; StaticForces(origin, fOrigin); - for (size_t i{0}; i < HANGPRINTER_AXES; ++i) { + for (size_t i{0}; i < numAnchors; ++i) { relaxedSpringLengthsOrigin[i] = distancesOrigin[i] - fOrigin[i] / (springKsOrigin[i] * mechanicalAdvantage[i]); } @@ -203,11 +222,15 @@ 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); - + if (gb.Seen('N')) + { + numAnchors = gb.GetUIValue(); + seen = true; + } + for (size_t i = 0; i < numAnchors; ++i) + { + gb.TryGetFloatArray(ANCHOR_CHARS[i], 3, anchors[i], seen); + } if (gb.Seen('P')) { printRadius = gb.GetPositiveFValue(); @@ -221,71 +244,106 @@ 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 < numAnchors; ++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); - gb.TryGetUIArray('O', HANGPRINTER_AXES, linesPerSpool, seen); - gb.TryGetUIArray('L', HANGPRINTER_AXES, motorGearTeeth, seen); - gb.TryGetUIArray('H', HANGPRINTER_AXES, spoolGearTeeth, seen); - gb.TryGetUIArray('J', HANGPRINTER_AXES, fullStepsPerMotorRev, seen); + gb.TryGetFloatArray('R', numAnchors, spoolRadii, seen); + gb.TryGetUIArray('U', numAnchors, mechanicalAdvantage, seen); + gb.TryGetUIArray('O', numAnchors, linesPerSpool, seen); + gb.TryGetUIArray('L', numAnchors, motorGearTeeth, seen); + gb.TryGetUIArray('H', numAnchors, spoolGearTeeth, seen); + gb.TryGetUIArray('J', numAnchors, fullStepsPerMotorRev, seen); gb.TryGetFValue('W', moverWeight_kg, seen); gb.TryGetFValue('S', springKPerUnitLength, seen); - gb.TryGetFloatArray('I', HANGPRINTER_AXES, minPlannedForce_Newton, seen); - gb.TryGetFloatArray('X', HANGPRINTER_AXES, maxPlannedForce_Newton, seen); - gb.TryGetFloatArray('Y', HANGPRINTER_AXES, guyWireLengths, seen); - gb.TryGetFloatArray('C', HANGPRINTER_AXES, torqueConstants, seen); + gb.TryGetFloatArray('I', numAnchors, minPlannedForce_Newton, seen); + gb.TryGetFloatArray('X', numAnchors, maxPlannedForce_Newton, seen); + gb.TryGetFloatArray('Y', numAnchors, guyWireLengths, seen); + gb.TryGetFloatArray('C', numAnchors, torqueConstants, seen); if (seen) { Recalc(); } 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 < numAnchors; ++i) + { + reply.catf(":%.2f", (double)spoolRadii[i]); + } + + reply.lcatf("U%d", (int)mechanicalAdvantage[0]); + for (size_t i = 1; i < numAnchors; ++i) + { + reply.catf(":%d", (int)mechanicalAdvantage[i]); + } + + reply.lcatf("O%d", (int)linesPerSpool[0]); + for (size_t i = 1; i < numAnchors; ++i) + { + reply.catf(":%d", (int)linesPerSpool[i]); + } + + reply.lcatf("L%d", (int)motorGearTeeth[0]); + for (size_t i = 1; i < numAnchors; ++i) + { + reply.catf(":%d", (int)motorGearTeeth[i]); + } + + reply.lcatf("H%d", (int)spoolGearTeeth[0]); + for (size_t i = 1; i < numAnchors; ++i) + { + reply.catf(":%d", (int)spoolGearTeeth[i]); + } + + reply.lcatf("J%d", (int)fullStepsPerMotorRev[0]); + for (size_t i = 1; i < numAnchors; ++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 < numAnchors; ++i) + { + reply.catf(":%.1f", (double)minPlannedForce_Newton[i]); + } + + reply.lcatf("X%.1f", (double)maxPlannedForce_Newton[0]); + for (size_t i = 1; i < numAnchors; ++i) + { + reply.catf(":%.1f", (double)maxPlannedForce_Newton[i]); + } + + reply.lcatf("Y%.1f", (double)guyWireLengths[0]); + for (size_t i = 1; i < numAnchors; ++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 < numAnchors; ++i) + { + reply.catf(":%.4f", (double)torqueConstants[i]); + } + } } else @@ -299,36 +357,34 @@ bool HangprinterKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, const bool HangprinterKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], 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]); - + float distances[numAnchors]; + for (size_t i = 0; i < numAnchors; ++i) { + distances[i] = hyp3(machinePos, anchors[i]); + } - float springKs[HANGPRINTER_AXES]; - for (size_t i{0}; i < HANGPRINTER_AXES; ++i) { + float springKs[numAnchors]; + for (size_t i = 0; i < numAnchors; ++i) { springKs[i] = SpringK(distances[i] * mechanicalAdvantage[i] + guyWireLengths[i]); } - float F[HANGPRINTER_AXES] = {0.0F}; // desired force in each direction + float F[numAnchors] = { 0.0F }; // desired force in each direction StaticForces(machinePos, F); - float relaxedSpringLengths[HANGPRINTER_AXES]; - for (size_t i{0}; i < HANGPRINTER_AXES; ++i) { + float relaxedSpringLengths[numAnchors]; + for (size_t i{0}; i < numAnchors; ++i) { relaxedSpringLengths[i] = distances[i] - F[i] / (springKs[i] * mechanicalAdvantage[i]); // The second term there is the mover's movement in mm due to flex } - float linePos[HANGPRINTER_AXES]; - for (size_t i = 0; i < HANGPRINTER_AXES; ++i) { + float linePos[numAnchors]; + for (size_t i = 0; i < numAnchors; ++i) { 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 < numAnchors; ++i) + { + motorPos[i] = lrintf(k0[i] * (fastSqrtf(spoolRadiiSq[i] + linePos[i] * k2[i]) - spoolRadii[i])); + } return true; } @@ -340,65 +396,55 @@ inline float HangprinterKinematics::MotorPosToLinePos(const int32_t motorPos, si } -void HangprinterKinematics::flexDistances(float const machinePos[3], float const distanceA, - float const distanceB, float const distanceC, - float const distanceD, float flex[HANGPRINTER_AXES]) const noexcept { - float springKs[HANGPRINTER_AXES] = { - SpringK(distanceA * mechanicalAdvantage[A_AXIS] + guyWireLengths[A_AXIS]), - SpringK(distanceB * mechanicalAdvantage[B_AXIS] + guyWireLengths[B_AXIS]), - SpringK(distanceC * mechanicalAdvantage[C_AXIS] + guyWireLengths[C_AXIS]), - SpringK(distanceD * mechanicalAdvantage[D_AXIS] + guyWireLengths[D_AXIS]) - }; +void HangprinterKinematics::flexDistances(float const machinePos[3], float const distances[HANGPRINTER_MAX_ANCHORS], + float flex[HANGPRINTER_MAX_ANCHORS]) const noexcept { + float springKs[HANGPRINTER_MAX_ANCHORS] = { 0.0F }; + for (size_t i = 0; i < numAnchors; ++i) { + springKs[i] = SpringK(distances[i] * mechanicalAdvantage[i] + guyWireLengths[i]); + } - float F[HANGPRINTER_AXES] = {0.0F}; // desired force in each direction + float F[HANGPRINTER_MAX_ANCHORS] = { 0.0F }; // desired force in each direction StaticForces(machinePos, F); - float relaxedSpringLengths[HANGPRINTER_AXES] = { - distanceA - F[A_AXIS] / (springKs[A_AXIS] * mechanicalAdvantage[A_AXIS]), - distanceB - F[B_AXIS] / (springKs[B_AXIS] * mechanicalAdvantage[B_AXIS]), - distanceC - F[C_AXIS] / (springKs[C_AXIS] * mechanicalAdvantage[C_AXIS]), - distanceD - F[D_AXIS] / (springKs[D_AXIS] * mechanicalAdvantage[D_AXIS]) + float relaxedSpringLengths[HANGPRINTER_MAX_ANCHORS] = { 0.0F }; + for (size_t i = 0; i < numAnchors; ++i) { + relaxedSpringLengths[i] = distances[i]- F[i] / (springKs[i] * mechanicalAdvantage[i]); }; - float linePos[HANGPRINTER_AXES] = { - relaxedSpringLengths[A_AXIS] - relaxedSpringLengthsOrigin[A_AXIS], - relaxedSpringLengths[B_AXIS] - relaxedSpringLengthsOrigin[B_AXIS], - relaxedSpringLengths[C_AXIS] - relaxedSpringLengthsOrigin[C_AXIS], - relaxedSpringLengths[D_AXIS] - relaxedSpringLengthsOrigin[D_AXIS] + float linePos[HANGPRINTER_MAX_ANCHORS] = { 0.0F }; + for (size_t i = 0; i < numAnchors; ++i) { + linePos[i] = relaxedSpringLengths[i] - relaxedSpringLengthsOrigin[i]; }; - float distanceDifferences[HANGPRINTER_AXES] = { - distanceA - distancesOrigin[A_AXIS], - distanceB - distancesOrigin[B_AXIS], - distanceC - distancesOrigin[C_AXIS], - distanceD - distancesOrigin[D_AXIS] + float distanceDifferences[HANGPRINTER_MAX_ANCHORS] = { 0.0F }; + for (size_t i = 0; i < numAnchors; ++i) { + distanceDifferences[i] = distances[i] - distancesOrigin[i]; }; - flex[A_AXIS] = linePos[A_AXIS] - distanceDifferences[A_AXIS]; - flex[B_AXIS] = linePos[B_AXIS] - distanceDifferences[B_AXIS]; - flex[C_AXIS] = linePos[C_AXIS] - distanceDifferences[C_AXIS]; - flex[D_AXIS] = linePos[D_AXIS] - distanceDifferences[D_AXIS]; + for (size_t i = 0; i < numAnchors; ++i) { + flex[i] = linePos[i] - distanceDifferences[i]; + } } // Convert motor coordinates to machine coordinates. // Assumes lines are tight and anchor location norms are followed void HangprinterKinematics::MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const noexcept { - float const distanceA = MotorPosToLinePos(motorPos[A_AXIS], A_AXIS) + distancesOrigin[A_AXIS]; - float const distanceB = MotorPosToLinePos(motorPos[B_AXIS], B_AXIS) + distancesOrigin[B_AXIS]; - float const distanceC = MotorPosToLinePos(motorPos[C_AXIS], C_AXIS) + distancesOrigin[C_AXIS]; - float const distanceD = MotorPosToLinePos(motorPos[D_AXIS], D_AXIS) + distancesOrigin[D_AXIS]; - ForwardTransform(distanceA, distanceB, distanceC, distanceD, machinePos); + float distances[HANGPRINTER_MAX_ANCHORS] = { 0.0F }; + for (size_t i = 0; i < numAnchors; ++i) { + distances[i] = MotorPosToLinePos(motorPos[i], i) + distancesOrigin[i]; + }; + ForwardTransform(distances, machinePos); // Now we have an approximate machinePos // Let's correct for line flex - float flex[HANGPRINTER_AXES] = { 0.0F }; - flexDistances(machinePos, distanceA, distanceB, distanceC, distanceD, flex); - float const adjustedDistanceA = distanceA - flex[A_AXIS]; - float const adjustedDistanceB = distanceB - flex[B_AXIS]; - float const adjustedDistanceC = distanceC - flex[C_AXIS]; - float const adjustedDistanceD = distanceD - flex[D_AXIS]; - ForwardTransform(adjustedDistanceA, adjustedDistanceB, adjustedDistanceC, adjustedDistanceD, machinePos); + float flex[HANGPRINTER_MAX_ANCHORS] = { 0.0F }; + flexDistances(machinePos, distances, flex); + float adjustedDistances[HANGPRINTER_MAX_ANCHORS] = { 0.0F }; + for (size_t i = 0; i < numAnchors; ++i) { + adjustedDistances[i] = distances[i] - flex[i]; + } + ForwardTransform(adjustedDistances, machinePos); } static bool isSameSide(float const v0[3], float const v1[3], float const v2[3], float const v3[3], float const p[3]){ @@ -419,17 +465,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 < numAnchors - 1; ++i) { + reachable = reachable && isSameSide(anchors[i], anchors[(i+1) % (numAnchors - 1)], anchors[numAnchors - 1], anchors[(i+2) % (numAnchors - 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 < numAnchors - 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) % (numAnchors - 1)], lower_point, anchors[(i+2) % (numAnchors - 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 @@ -508,7 +590,10 @@ AxesBitmap HangprinterKinematics::AxesAssumedHomed(AxesBitmap g92Axes) const noe // If all of X, Y and Z have been specified then we know the positions of all 4 spool motors, otherwise we don't if ((g92Axes & XyzAxes) == XyzAxes) { - g92Axes.SetBit(D_AXIS); + for (size_t i = 3; i < numAnchors; ++i) + { + g92Axes.SetBit(i); + } } else { @@ -532,69 +617,116 @@ 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; + + scratchString.printf("N%d", numAnchors); + for (size_t i = 0; i < numAnchors; ++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 < numAnchors; ++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 < numAnchors; ++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 < numAnchors; ++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 < numAnchors; ++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 < numAnchors; ++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 < numAnchors; ++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 < numAnchors; ++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 < numAnchors; ++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 < numAnchors; ++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 < numAnchors; ++i) + { + scratchString.catf(":%.4f", (double)torqueConstants[i]); + } + ok = f->Write(scratchString.c_str()); + return ok; } @@ -606,12 +738,32 @@ bool HangprinterKinematics::WriteResumeSettings(FileStore *f) const noexcept #endif + +void HangprinterKinematics::ForwardTransform(float const distances[HANGPRINTER_MAX_ANCHORS], float machinePos[3]) const noexcept { + switch (anchorMode) { + case HangprinterAnchorMode::LastOnTop: + if (numAnchors == 4) { + ForwardTransformTetrahedron(distances, machinePos); + return; + } else if (numAnchors == 5) { + ForwardTransformQuadrilateralPyramid(distances, machinePos); + return; + } + // Intentional fall-through to next case + // if no forward transform + case HangprinterAnchorMode::None: + case HangprinterAnchorMode::AllOnTop: + default: + return; + } +} + /** - * Hangprinter forward kinematics + * Hangprinter forward kinematics tetrahedron case (three low anchors, one high) * Basic idea is to subtract squared line lengths to get linear equations, * and then to solve with variable substitution. * - * If we assume anchor location norms are followed + * If we assume (enforce by rotations) that anchor location norms are followed * Ax=0 Dx=0 Dy=0 * then * we get a fairly clean derivation by @@ -638,10 +790,14 @@ bool HangprinterKinematics::WriteResumeSettings(FileStore *f) const noexcept * * Warning: truncation errors will typically be in the order of a few tens of microns. */ -void HangprinterKinematics::ForwardTransform(float const a, float const b, float const c, float const d, float machinePos[3]) const noexcept +void HangprinterKinematics::ForwardTransformTetrahedron(float const distances[HANGPRINTER_MAX_ANCHORS], float machinePos[3]) const noexcept { // Force the anchor location norms Ax=0, Dx=0, Dy=0 // through a series of rotations. + static constexpr size_t A_AXIS = 0; + static constexpr size_t B_AXIS = 1; + static constexpr size_t C_AXIS = 2; + static constexpr size_t D_AXIS = 3; float const x_angle = atanf(anchors[D_AXIS][Y_AXIS]/anchors[D_AXIS][Z_AXIS]); float const rxt[3][3] = {{1, 0, 0}, {0, cosf(x_angle), sinf(x_angle)}, {0, -sinf(x_angle), cosf(x_angle)}}; float anchors_tmp0[4][3] = { 0 }; @@ -670,10 +826,10 @@ void HangprinterKinematics::ForwardTransform(float const a, float const b, float const float Bsq = fsquare(distancesOrigin[B_AXIS]); const float Csq = fsquare(distancesOrigin[C_AXIS]); const float Dsq = fsquare(distancesOrigin[D_AXIS]); - const float aa = fsquare(a); - const float dd = fsquare(d); - const float k0b = (-fsquare(b) + Bsq - Dsq + dd) / (2.0 * anchors_tmp0[B_AXIS][X_AXIS]) + (anchors_tmp0[B_AXIS][Y_AXIS] / (2.0 * anchors_tmp0[A_AXIS][Y_AXIS] * anchors_tmp0[B_AXIS][X_AXIS])) * (Dsq - Asq + aa - dd); - const float k0c = (-fsquare(c) + Csq - Dsq + dd) / (2.0 * anchors_tmp0[C_AXIS][X_AXIS]) + (anchors_tmp0[C_AXIS][Y_AXIS] / (2.0 * anchors_tmp0[A_AXIS][Y_AXIS] * anchors_tmp0[C_AXIS][X_AXIS])) * (Dsq - Asq + aa - dd); + const float aa = fsquare(distances[A_AXIS]); + const float dd = fsquare(distances[D_AXIS]); + const float k0b = (-fsquare(distances[B_AXIS]) + Bsq - Dsq + dd) / (2.0 * anchors_tmp0[B_AXIS][X_AXIS]) + (anchors_tmp0[B_AXIS][Y_AXIS] / (2.0 * anchors_tmp0[A_AXIS][Y_AXIS] * anchors_tmp0[B_AXIS][X_AXIS])) * (Dsq - Asq + aa - dd); + const float k0c = (-fsquare(distances[C_AXIS]) + Csq - Dsq + dd) / (2.0 * anchors_tmp0[C_AXIS][X_AXIS]) + (anchors_tmp0[C_AXIS][Y_AXIS] / (2.0 * anchors_tmp0[A_AXIS][Y_AXIS] * anchors_tmp0[C_AXIS][X_AXIS])) * (Dsq - Asq + aa - dd); const float k1b = (anchors_tmp0[B_AXIS][Y_AXIS] * (anchors_tmp0[A_AXIS][Z_AXIS] - anchors_tmp0[D_AXIS][Z_AXIS])) / (anchors_tmp0[A_AXIS][Y_AXIS] * anchors_tmp0[B_AXIS][X_AXIS]) + (anchors_tmp0[D_AXIS][Z_AXIS] - anchors_tmp0[B_AXIS][Z_AXIS]) / anchors_tmp0[B_AXIS][X_AXIS]; const float k1c = (anchors_tmp0[C_AXIS][Y_AXIS] * (anchors_tmp0[A_AXIS][Z_AXIS] - anchors_tmp0[D_AXIS][Z_AXIS])) / (anchors_tmp0[A_AXIS][Y_AXIS] * anchors_tmp0[C_AXIS][X_AXIS]) + (anchors_tmp0[D_AXIS][Z_AXIS] - anchors_tmp0[C_AXIS][Z_AXIS]) / anchors_tmp0[C_AXIS][X_AXIS]; @@ -695,13 +851,127 @@ void HangprinterKinematics::ForwardTransform(float const a, float const b, float } } +static inline bool det(FixedMatrix M){ + return M(0, 0) * (M(1, 1) * M(2, 2) - M(1, 2) * M(2, 1)) + M(0, 1) * (M(1, 2) * M(2, 0) - M(2, 2) * M(1, 0)) + M(0, 2) * (M(1, 0) * M(2, 1) - M(1, 1) * M(2, 0)); +} + +static inline bool singular_3x3(FixedMatrix M){ + float const threshold = 1e-1; + return fabsf(det(M)) < threshold; +} + + +/* A quadrilateral pyramid has five anchors: four low and one high. + * Our input variable `distances` contain the distance (L2-norm, euclidian distance) from each corner + * to some point that is encapsulated by the five anchors. + * We want to determine the xyz-coordinates of that point. + * + * Since this gives us 5 known variables and 3 unknown, this is an overdetermined system, + * and we're not guaranteed that the 5 known variables define one exact point in space. + * The `distance` values have been calculated from motors' rotational positions, and they don't + * know if lines are slack or over tight, and much less which lines are slack and which are not in that case. + * The `distance` values will generally be slightly off, and define a region in which we might wiggle, instead of a point. + * + * We approach this by solving four linear systems and averaging over the result, + * which corresponds to all lines being ca equally slack. + * + * To get the four linear systems, we start with five non-linear ones. + * |A - p| = l_a + * |B - p| = l_b + * |C - p| = l_c + * |D - p| = l_d + * |I - p| = l_i, + * + * where p is our unknown (x, y, z), A is the xyz of our A-anchor, and I is the top anchor. + * + * Square both sides and move |A|² to the right hand side: + * + * -2*A*p + |p|² = l_a² - |A|² + * + * Now subtract the last equation to get rid of |p|² (we expect the last equality to always hold, vertical lines are never slack). + * + * -2*A*p - 2*I*p = l_a² - l_i² - (|A|² - |I|²) + * + * Divide by -2 and simplify to get four linear equations + * + * (A - I)*p = -(l_a² - l_i² - (|A|² - |I|²))/2 = k_a + * (B - I)*p = -(l_b² - l_i² - (|B|² - |I|²))/2 = k_b + * (C - I)*p = -(l_c² - l_i² - (|C|² - |I|²))/2 = k_c + * (D - I)*p = -(l_d² - l_i² - (|D|² - |I|²))/2 = k_d + * + * Say A' = A - I, and we get + * + * A'x A'y A'z | p_x k_a + * B'x B'y B'z | p_y = k_b + * C'x C'y C'z | p_z k_c + * D'x D'y D'z | k_d + * + * This is a linear but still overdetermined system (4x3). Skip each row in turn to obtain four different 3x3 matrices, and four different solution vectors k. + * Solve each by GaussJordan elminiation and average over the result. + * Voila. + */ +void HangprinterKinematics::ForwardTransformQuadrilateralPyramid(float const distances[HANGPRINTER_MAX_ANCHORS], float machinePos[3]) const noexcept { + float anch_prim[4][3]{{0.0}}; + float distancesOriginSq[5]{0.0}; + float distancesSq[5]{0.0}; + float k[4]{0.0}; + FixedMatrix M[4]; + float machinePos_tmp[3]{0.0}; + + for (size_t i{0}; i < 4; ++i) { + for (size_t j{0}; j < 3; ++j) { + anch_prim[i][j] = anchors[i][j] - anchors[4][j]; + } + } + + for (size_t i{0}; i < 5; ++i) { + distancesOriginSq[i] = fsquare(distancesOrigin[i]); + distancesSq[i] = fsquare(distances[i]); + } + + for (size_t i{0}; i < 4; ++i) { + k[i] = ((distancesOriginSq[i] - distancesOriginSq[4]) - (distancesSq[i] - distancesSq[4])) / 2.0; + } + + for (size_t matrix_num{0}; matrix_num < 4; matrix_num++){ + for (size_t row{0}; row < 3; ++row) { + size_t r = (row + matrix_num) % 4; + for (size_t col{0}; col < 3; ++col) { + M[matrix_num](row, col) = anch_prim[r][col]; + } + M[matrix_num](row, 3) = k[r]; + } + } + + // Solve the four systems + size_t used{0}; + for (int k = 0; k < 4; ++k) { + if (not singular_3x3(M[k])) { + used++; + M[k].GaussJordan(3, 4); + for (size_t i{0}; i < 3; ++i) { + machinePos_tmp[i] += M[k](i, 3); + } + } + } + if (used != 0) { + for (size_t i{0}; i < 3; ++i) { + machinePos[i] = machinePos_tmp[i]/static_cast(used); + } + } + +} + + // 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 < numAnchors; ++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 @@ -745,8 +1015,8 @@ HangprinterKinematics::ODriveAnswer HangprinterKinematics::GetODrive3MotorCurren HangprinterKinematics::ODriveAnswer HangprinterKinematics::GetODrive3EncoderEstimate(DriverId const driver, bool const makeReference, const StringRef& reply, bool const subtractReference) THROWS(GCodeException) { const uint8_t cmd = CANSimple::MSG_GET_ENCODER_ESTIMATES; - static CanAddress seenDrives[HANGPRINTER_AXES] = { 0, 0, 0, 0 }; - static float referencePositions[HANGPRINTER_AXES] = { 0.0, 0.0, 0.0, 0.0 }; + static CanAddress seenDrives[HANGPRINTER_MAX_ANCHORS] = { 0 }; + static float referencePositions[HANGPRINTER_MAX_ANCHORS] = { 0.0 }; static size_t numSeenDrives = 0; size_t thisDriveIdx = 0; @@ -757,15 +1027,15 @@ HangprinterKinematics::ODriveAnswer HangprinterKinematics::GetODrive3EncoderEsti bool const newOne = (thisDriveIdx == numSeenDrives); if (newOne) { - if (numSeenDrives < HANGPRINTER_AXES) + if (numSeenDrives < numAnchors) { seenDrives[thisDriveIdx] = driver.boardAddress; numSeenDrives++; } else // we don't have space for a new one { - reply.printf("Max CAN addresses we can reference is %d. Can't reference board %d.", HANGPRINTER_AXES, driver.boardAddress); - numSeenDrives = HANGPRINTER_AXES; + reply.printf("Max CAN addresses we can reference is %d. Can't reference board %d.", numAnchors, driver.boardAddress); + numSeenDrives = numAnchors; return {}; } } @@ -826,14 +1096,14 @@ HangprinterKinematics::ODriveAnswer HangprinterKinematics::GetODrive3EncoderEsti #if DUAL_CAN GCodeResult HangprinterKinematics::ReadODrive3AxisForce(DriverId const driver, const StringRef& reply, float setTorqueConstants[], uint32_t setMechanicalAdvantage[], uint32_t setSpoolGearTeeth[], uint32_t setMotorGearTeeth[], float setSpoolRadii[]) THROWS(GCodeException) { - static float torqueConstants_[HANGPRINTER_AXES] = { 0.0 }; - static uint32_t mechanicalAdvantage_[HANGPRINTER_AXES] = { 0 }; - static uint32_t spoolGearTeeth_[HANGPRINTER_AXES] = { 0 }; - static uint32_t motorGearTeeth_[HANGPRINTER_AXES] = { 0 }; - static float spoolRadii_[HANGPRINTER_AXES] = { 0.0 }; + static float torqueConstants_[HANGPRINTER_MAX_ANCHORS] = { 0.0 }; + static uint32_t mechanicalAdvantage_[HANGPRINTER_MAX_ANCHORS] = { 0 }; + static uint32_t spoolGearTeeth_[HANGPRINTER_MAX_ANCHORS] = { 0 }; + static uint32_t motorGearTeeth_[HANGPRINTER_MAX_ANCHORS] = { 0 }; + static float spoolRadii_[HANGPRINTER_MAX_ANCHORS] = { 0.0 }; if (setTorqueConstants != nullptr && setMechanicalAdvantage != nullptr && setSpoolGearTeeth != nullptr && setMotorGearTeeth != nullptr && setSpoolRadii != nullptr) { - for(size_t i{0}; i < HANGPRINTER_AXES; ++i) { + for(size_t i{0}; i < numAnchors; ++i) { torqueConstants_[i] = setTorqueConstants[i]; mechanicalAdvantage_[i] = setMechanicalAdvantage[i]; spoolGearTeeth_[i] = setSpoolGearTeeth[i]; @@ -956,13 +1226,13 @@ GCodeResult HangprinterKinematics::SetODrive3TorqueMode(DriverId const driver, f uint32_t setMechanicalAdvantage[], uint32_t setSpoolGearTeeth[], uint32_t setMotorGearTeeth[], float setSpoolRadii[]) noexcept { - static uint32_t mechanicalAdvantage_[HANGPRINTER_AXES] = { 0 }; - static uint32_t spoolGearTeeth_[HANGPRINTER_AXES] = { 0 }; - static uint32_t motorGearTeeth_[HANGPRINTER_AXES] = { 0 }; - static float spoolRadii_[HANGPRINTER_AXES] = { 0.0 }; + static uint32_t mechanicalAdvantage_[HANGPRINTER_MAX_ANCHORS] = { 0 }; + static uint32_t spoolGearTeeth_[HANGPRINTER_MAX_ANCHORS] = { 0 }; + static uint32_t motorGearTeeth_[HANGPRINTER_MAX_ANCHORS] = { 0 }; + static float spoolRadii_[HANGPRINTER_MAX_ANCHORS] = { 0.0 }; if (setMechanicalAdvantage != nullptr && setSpoolGearTeeth != nullptr && setMotorGearTeeth != nullptr && setSpoolRadii != nullptr) { - for(size_t i{0}; i < HANGPRINTER_AXES; ++i) { + for(size_t i{0}; i < numAnchors; ++i) { mechanicalAdvantage_[i] = setMechanicalAdvantage[i]; spoolGearTeeth_[i] = setSpoolGearTeeth[i]; motorGearTeeth_[i] = setMotorGearTeeth[i]; @@ -1013,38 +1283,177 @@ float HangprinterKinematics::SpringK(float const springLength) const noexcept { return springKPerUnitLength / springLength; } -void HangprinterKinematics::StaticForces(float const machinePos[3], float F[4]) const noexcept { + +void HangprinterKinematics::StaticForces(float const machinePos[3], float F[HANGPRINTER_MAX_ANCHORS]) const noexcept { + switch (anchorMode) { + case HangprinterAnchorMode::LastOnTop: + if (numAnchors == 4) { + StaticForcesTetrahedron(machinePos, F); + return; + } else if (numAnchors == 5) { + StaticForcesQuadrilateralPyramid(machinePos, F); + return; + } + // Intentional fall-through to next case + // if no line flex compensation + case HangprinterAnchorMode::None: + case HangprinterAnchorMode::AllOnTop: + default: + for (size_t i = 0; i < HANGPRINTER_MAX_ANCHORS; ++i){ + F[i] = 0.0; + } + } +} + +void HangprinterKinematics::StaticForcesQuadrilateralPyramid(float const machinePos[3], float F[HANGPRINTER_MAX_ANCHORS]) const noexcept { + // A QuadrilateralPyramid has 5 corners, there's one anchor in each. + // There are many 4's in this function because 4 motors (the lower ones, ABCD) + // are assumed to have unknown forces. + // The forces in the top anchor is assumed to be known and constant, except for gravity's + // effects who are also known. + if (moverWeight_kg < 0.0001) { + return; + } + // Space for four linear 3x3 systems, each with two solution columns, + FixedMatrix M[4]; + + float norm[5]; + norm[4] = hyp3(anchors[4], machinePos); + for (int i = 0; i < 4; ++i) { + norm[i] = hyp3(anchors[i], machinePos); + for (int j = 0; j < 3; ++j) { + for (int k = 0; k < 4; ++k) { + // Fill 3x3 top left corner of system with + // unit vectors toward each ABCD anchor from mover + // If A is the column vector pointing towards A-anchor, we're building these + // four matrices: + // k=0: [BCD], A-direction skipped + // k=1: [ACD], B-direction skipped + // k=2: [ABD], C-direction skipped + // k=3: [ABC], D-direction skipped + if ( k != i) { + if ( i > k ) { + M[k](j, i - 1) = (anchors[i][j] - machinePos[j]) / norm[i]; + } else { + M[k](j, i) = (anchors[i][j] - machinePos[j]) / norm[i]; + } + } + } + } + } + float const mg = moverWeight_kg * 9.81; + + float top_mg = 0.0F; + float top_pre = 0.0F; + + if (anchors[4][Z_AXIS] > machinePos[Z_AXIS]) { + // These force constants will go into the solution column that has to do with gravity + top_mg = mg / ((anchors[4][Z_AXIS] - machinePos[Z_AXIS]) / norm[4]); + top_pre = targetForce_Newton; + } + + // Indices for the two solution columns + size_t const sol_mg = 3; + size_t const sol_pt = 4; + for (int i = 0; i < 3; ++i) { + float const top_dist = (anchors[4][i] - machinePos[i]) / norm[4]; + for (int k = 0; k < 4; ++k) { + M[k](i, sol_mg) = -top_mg * top_dist; // gravity solution column + M[k](i, sol_pt) = -top_pre * top_dist; // pretension solution column + } + } + for (int k = 0; k < 4; ++k) { + // Cancel out top anchor's Z-force with gravity. + M[k](Z_AXIS, sol_mg) += mg; // == 0 + } + + // Solve the four systems + for (int k = 0; k < 4; ++k) { + M[k].GaussJordan(3, 5); + } + + // Weigh/scale the pre-tension solutions so all have equal max force. + float norm_ABCD[4]; + for(size_t k{0}; k < 4; ++k) { + norm_ABCD[k] = fastSqrtf(M[k](0, sol_pt) * M[k](0, sol_pt) + M[k](1, sol_pt) * M[k](1, sol_pt) + M[k](2, sol_pt) * M[k](2, sol_pt)); + } + + // Arrays to hold our weighted combinations of the four (pairs of) solutions + float p[4] = { 0.0F, 0.0F, 0.0F, 0.0F }; + float m[4] = { 0.0F, 0.0F, 0.0F, 0.0F }; + for (size_t i{0}; i < 3; ++i) { + for (size_t j{0}; j < 4; ++j) { + float const pt_weight = targetForce_Newton / norm_ABCD[j]; + // The gravity counter actions are scaled to exactly counter act gravity, and top-line forces neccesary to counter act gravity. + // So the resultant force of all four solutions is the same. Lets add a quarter of each solution to get back that resultant force. + float const mg_weight = 1.0/4.0; + // i can mean BCD, ACD, ABD, or ABC, depending on which matrix we're looking into + // Let's just translate that back into the solutions vectors + size_t const s = j <= i ? i + 1 : i; + p[s] += M[j](i, sol_pt)*pt_weight; + m[s] += M[j](i, sol_mg)*mg_weight; + } + } + + // The pre-tension solution can be scaled up or down however we want. + // Forces in those solution cancel each other out exactly, so any multiple of the solution is also a valid solution. + // + // (The gravity solution can't be scaled since it has to exactly counter act top-line forces that must exactly counter act gravity (mg)) + // + // Use the scaling freedom of the pre-tension solution to assure that we have at least targetForce_Newton in the ABCD lines, + // and that no line (incl top-line) get more tension than the configured maxPlannedForce in that direction. + float preFac = min(max(std::abs((targetForce_Newton - m[3]) / p[3]), + max(std::abs((targetForce_Newton - m[2]) / p[2]), + max(std::abs((targetForce_Newton - m[1]) / p[1]), std::abs((targetForce_Newton - m[0]) / p[0])))), + min(std::abs((maxPlannedForce_Newton[4] - top_mg) / top_pre), + min(min(std::abs((maxPlannedForce_Newton[0] - m[0]) / p[0]), std::abs((maxPlannedForce_Newton[1] - m[1]) / p[1])), + min(std::abs((maxPlannedForce_Newton[2] - m[2]) / p[2]), std::abs((maxPlannedForce_Newton[3] - m[3]) / p[3]))))); + + float tot[5] = { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F }; + tot[0] = m[0] + preFac * p[0]; + tot[1] = m[1] + preFac * p[1]; + tot[2] = m[2] + preFac * p[2]; + tot[3] = m[3] + preFac * p[3]; + tot[4] = top_mg + preFac * top_pre; + + for (size_t i{0}; i < 5; ++i) { + // Negative, or very large forces can still have slipped through the preFac filter. + // Truncate away such forces and assign to the output variable. + // Voila. + // The min( ... ) shouldn't be needed here. Just better safe than sorry. + F[i] = min(max(tot[i], minPlannedForce_Newton[i]), maxPlannedForce_Newton[i]); + } +} + + +void HangprinterKinematics::StaticForcesTetrahedron(float const machinePos[3], float F[HANGPRINTER_MAX_ANCHORS]) const noexcept { + static constexpr size_t A_AXIS = 0; + static constexpr size_t B_AXIS = 1; + static constexpr size_t C_AXIS = 2; + static constexpr size_t D_AXIS = 3; + static constexpr size_t FOUR_ANCH = 4; + static constexpr size_t CARTESIAN_AXES = 3; + if (moverWeight_kg > 0.0001) { // mover weight more than one gram - // Size of D-force in Newtons - float const mg = moverWeight_kg * 9.81; - // Unit vector directions toward each anchor from mover - float const normA = hyp3(anchors[A_AXIS], machinePos); - float const normB = hyp3(anchors[B_AXIS], machinePos); - float const normC = hyp3(anchors[C_AXIS], machinePos); - float const normD = hyp3(anchors[D_AXIS], machinePos); - float ax = (anchors[A_AXIS][0] - machinePos[0])/normA; - float ay = (anchors[A_AXIS][1] - machinePos[1])/normA; - float az = (anchors[A_AXIS][2] - machinePos[2])/normA; - float bx = (anchors[B_AXIS][0] - machinePos[0])/normB; - float by = (anchors[B_AXIS][1] - machinePos[1])/normB; - float bz = (anchors[B_AXIS][2] - machinePos[2])/normB; - float cx = (anchors[C_AXIS][0] - machinePos[0])/normC; - float cy = (anchors[C_AXIS][1] - machinePos[1])/normC; - float cz = (anchors[C_AXIS][2] - machinePos[2])/normC; - float dx = (anchors[D_AXIS][0] - machinePos[0])/normD; - float dy = (anchors[D_AXIS][1] - machinePos[1])/normD; - float dz = (anchors[D_AXIS][2] - machinePos[2])/normD; + float norm[FOUR_ANCH]; // Unit vector directions toward each anchor from mover + FixedMatrix M; + for (size_t i = 0; i < FOUR_ANCH - 1; ++i) { // One anchor above mover + norm[i] = hyp3(anchors[i], machinePos); + for (size_t j = 0; j < CARTESIAN_AXES; ++j) { + M(j, i) = (anchors[i][j] - machinePos[j]) / norm[i]; + } + } + float const mg = moverWeight_kg * 9.81; // Size of gravity force in Newtons float D_mg = 0.0F; float D_pre = 0.0F; - if (dz > 0.0001) { - D_mg = mg / dz; - D_pre = targetForce_Newton; - } + // The D-forces' z-component is always equal to mg + targetForce_Newton. - // This means ABC-motors combined pull downwards targetForce_Newton N. - // I don't know if that's always solvable. - // Still, my tests show that we get very reasonable flex compensation... + // The D-forces' z-component is always equal to mg + + // targetForce_Newton. This means ABC-motors combined pull + // downwards targetForce_Newton N. I don't know if that's always + // solvable. Still, my tests show that we get very reasonable + // flex compensation... // Right hand side of the equation // A + B + C + D + (0,0,-mg)' = 0 @@ -1064,133 +1473,55 @@ void HangprinterKinematics::StaticForces(float const machinePos[3], float F[4]) // x = B, // C // - // anc y is + // and y is // // -D*dx // y = -D*dy . // -D*dz + mg - // - float const yx_mg = -D_mg*dx; - float const yy_mg = -D_mg*dy; - float const yz_mg = -D_mg*dz + mg; - float const yx_pre = -D_pre*dx; - float const yy_pre = -D_pre*dy; - float const yz_pre = -D_pre*dz; - - // Start with saving us from dividing by zero during Gaussian substitution - float constexpr eps = 0.00001; - bool const divZero0 = std::abs(ax) < eps; - if (divZero0) { - float const tmpx = bx; - float const tmpy = by; - float const tmpz = bz; - bx = ax; - by = ay; - bz = az; - ax = tmpx; - ay = tmpy; - az = tmpz; - } - bool const divZero1 = (std::abs(by - (bx / ax) * ay) < eps); - if (divZero1) { - float const tmpx = cx; - float const tmpy = cy; - float const tmpz = cz; - cx = bx; - cy = by; - cz = bz; - bx = tmpx; - by = tmpy; - bz = tmpz; - } - bool const divZero2 = std::abs((cz - (cx / ax) * az) - ((cy - (cx / ax) * ay) / (by - (bx / ax) * ay)) * (bz - (bx / ax) * az)) < eps; - if (divZero2) { - float const tmpx = ax; - float const tmpy = ay; - float const tmpz = az; - ax = cx; - ay = cy; - az = cz; - cx = tmpx; - cy = tmpy; - cz = tmpz; - } - // Solving the two systems by Gaussian substitution - float const q0 = bx / ax; - float const q1 = cx / ax; - float const q2_mg = yx_mg / ax; - float const q2_pre = yx_pre / ax; - float const q3 = by - q0 * ay; - float const q4 = cy - q1 * ay; - float const q5_mg = yy_mg - q2_mg * ay; - float const q5_pre = yy_pre - q2_pre * ay; - float const q6 = bz - q0 * az; - float const q7 = cz - q1 * az; - float const q8_mg = yz_mg - q2_mg * az; - float const q8_pre = yz_pre - q2_pre * az; - float const q9 = q4 / q3; - float const q10_mg = q5_mg / q3; - float const q10_pre = q5_pre / q3; - float const q11 = q7 - q9 * q6; - float const q12_mg = q8_mg - q10_mg * q6; - float const q12_pre = q8_pre - q10_pre * q6; - float const q13_mg = q12_mg / q11; - float const q13_pre = q12_pre / q11; - float const q14_mg = q10_mg - q13_mg * q9; - float const q14_pre = q10_pre - q13_pre * q9; - float const q15_mg = q2_mg - q13_mg * q1; - float const q15_pre = q2_pre - q13_pre * q1; - - // Size of the undetermined forces - float A_mg = q15_mg - q14_mg * q0; - float A_pre = q15_pre - q14_pre * q0; - float B_mg = q14_mg; - float B_pre = q14_pre; - float C_mg = q13_mg; - float C_pre = q13_pre; - - if (divZero2) { - float const tmp_mg = A_mg; - A_mg = C_mg; - C_mg = tmp_mg; - float const tmp_pre = A_pre; - A_pre = C_pre; - C_pre = tmp_pre; + float const normD = hyp3(anchors[D_AXIS], machinePos); + if (anchors[D_AXIS][Z_AXIS] > machinePos[Z_AXIS]) { // D anchor above machine + D_mg = mg / ((anchors[D_AXIS][2] - machinePos[2]) / normD); + D_pre = targetForce_Newton; } - if (divZero1) { - float const tmp_mg = C_mg; - C_mg = B_mg; - B_mg = tmp_mg; - float const tmp_pre = C_pre; - C_pre = B_pre; - B_pre = tmp_pre; + + for (size_t i = 0; i < CARTESIAN_AXES; ++i) { + float const dist = (anchors[D_AXIS][i] - machinePos[i]) / normD; + M(i, FOUR_ANCH - 1) = -D_mg * dist; + M(i, FOUR_ANCH) = -D_pre * dist; } - if (divZero0) { - float const tmp_mg = B_mg; - B_mg = A_mg; - A_mg = tmp_mg; - float const tmp_pre = B_pre; - B_pre = A_pre; - A_pre = tmp_pre; + M(Z_AXIS, D_AXIS) += mg; + + // Solve! + const bool ok = M.GaussJordan(CARTESIAN_AXES, 5); + + if (ok) { + // Size of the undetermined forces + float const A_mg = M(0, 3); + float const B_mg = M(1, 3); + float const C_mg = M(2, 3); + float const A_pre = M(0, 4); + float const B_pre = M(1, 4); + float const C_pre = M(2, 4); + + // Assure at least targetForce in the ABC lines (first argument to outer min()), + // and that no line get more than max planned force (second argument to outer min()). + float const preFac = min(max(std::abs((targetForce_Newton - C_mg) / C_pre), + max(std::abs((targetForce_Newton - B_mg) / B_pre), std::abs((targetForce_Newton - A_mg) / A_pre))), + min(min(std::abs((maxPlannedForce_Newton[A_AXIS] - A_mg) / A_pre), std::abs((maxPlannedForce_Newton[B_AXIS] - B_mg) / B_pre)), + min(std::abs((maxPlannedForce_Newton[C_AXIS] - C_mg) / C_pre), std::abs((maxPlannedForce_Newton[D_AXIS] - D_mg) / D_pre)))); + + float totalForces[FOUR_ANCH] = { + A_mg + preFac * A_pre, + B_mg + preFac * B_pre, + C_mg + preFac * C_pre, + D_mg + preFac * D_pre + }; + + for (size_t i = 0; i < FOUR_ANCH; ++i) { + F[i] = min(max(totalForces[i], minPlannedForce_Newton[i]), maxPlannedForce_Newton[i]); + } } - - // Assure at least targetForce in the ABC lines (first argument to outer min()), - // and that no line get more than max planned force (second argument to outer min()). - float const preFac = min(max(std::abs((targetForce_Newton - C_mg) / C_pre), - max(std::abs((targetForce_Newton - B_mg) / B_pre), std::abs((targetForce_Newton - A_mg) / A_pre))), - min(min(std::abs((maxPlannedForce_Newton[A_AXIS] - A_mg) / A_pre), std::abs((maxPlannedForce_Newton[B_AXIS] - B_mg) / B_pre)), - min(std::abs((maxPlannedForce_Newton[C_AXIS] - C_mg) / C_pre), std::abs((maxPlannedForce_Newton[D_AXIS] - D_mg) / D_pre)))); - - float const A_tot = A_mg + preFac * A_pre; - float const B_tot = B_mg + preFac * B_pre; - float const C_tot = C_mg + preFac * C_pre; - float const D_tot = D_mg + preFac * D_pre; - - F[0] = max(A_tot, minPlannedForce_Newton[A_AXIS]); - F[1] = max(B_tot, minPlannedForce_Newton[B_AXIS]); - F[2] = max(C_tot, minPlannedForce_Newton[C_AXIS]); - F[3] = max(D_tot, minPlannedForce_Newton[D_AXIS]); } } diff --git a/src/Movement/Kinematics/HangprinterKinematics.h b/src/Movement/Kinematics/HangprinterKinematics.h index 8280e73217..af8275be04 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) Results 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,64 +62,66 @@ 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 - static constexpr size_t HANGPRINTER_AXES = 4; - static constexpr size_t A_AXIS = 0; - static constexpr size_t B_AXIS = 1; - static constexpr size_t C_AXIS = 2; - static constexpr size_t D_AXIS = 3; + const char* ANCHOR_CHARS = "ABCDIJKLO"; + static constexpr size_t HANGPRINTER_MAX_ANCHORS = 5; void Init() noexcept; void Recalc() noexcept; - void ForwardTransform(float a, float b, float c, float d, float machinePos[3]) const noexcept; + void ForwardTransform(float const distances[HANGPRINTER_MAX_ANCHORS], float machinePos[3]) const noexcept; + void ForwardTransformTetrahedron(float const distances[HANGPRINTER_MAX_ANCHORS], float machinePos[3]) const noexcept; + void ForwardTransformQuadrilateralPyramid(float const distances[HANGPRINTER_MAX_ANCHORS], float machinePos[3]) const noexcept; float MotorPosToLinePos(const int32_t motorPos, size_t axis) const noexcept; 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; + static size_t numAnchors; float printRadius = 0.0F; - float anchors[HANGPRINTER_AXES][3] = {{ 0.0, 0.0, 0.0}, - { 0.0, 0.0, 0.0}, - { 0.0, 0.0, 0.0}, - { 0.0, 0.0, 0.0}}; + float anchors[HANGPRINTER_MAX_ANCHORS][3]; // Line buildup compensation configurables /* The real defaults are in the Init() function, since we sometimes need to reset * defaults during runtime */ float spoolBuildupFactor = 0.0F; - float spoolRadii[HANGPRINTER_AXES] = { 0.0F }; - uint32_t mechanicalAdvantage[HANGPRINTER_AXES] = { 0 }; - uint32_t linesPerSpool[HANGPRINTER_AXES] = { 0 }; - uint32_t motorGearTeeth[HANGPRINTER_AXES] = { 0 }; - uint32_t spoolGearTeeth[HANGPRINTER_AXES] = { 0 }; - uint32_t fullStepsPerMotorRev[HANGPRINTER_AXES] = { 0 }; + float spoolRadii[HANGPRINTER_MAX_ANCHORS] = { 0.0F }; + uint32_t mechanicalAdvantage[HANGPRINTER_MAX_ANCHORS] = { 0 }; + uint32_t linesPerSpool[HANGPRINTER_MAX_ANCHORS] = { 0 }; + uint32_t motorGearTeeth[HANGPRINTER_MAX_ANCHORS] = { 0 }; + uint32_t spoolGearTeeth[HANGPRINTER_MAX_ANCHORS] = { 0 }; + uint32_t fullStepsPerMotorRev[HANGPRINTER_MAX_ANCHORS] = { 0 }; // Flex compensation configurables float moverWeight_kg = 0.0F; float springKPerUnitLength = 0.0F; - float minPlannedForce_Newton[HANGPRINTER_AXES] = { 0.0F }; - float maxPlannedForce_Newton[HANGPRINTER_AXES] = { 0.0F }; - float guyWireLengths[HANGPRINTER_AXES] = { 0.0F }; + float minPlannedForce_Newton[HANGPRINTER_MAX_ANCHORS] = { 0.0F }; + float maxPlannedForce_Newton[HANGPRINTER_MAX_ANCHORS] = { 0.0F }; + float guyWireLengths[HANGPRINTER_MAX_ANCHORS] = { 0.0F }; float targetForce_Newton = 0.0F; - float torqueConstants[HANGPRINTER_AXES] = { 0.0F }; + float torqueConstants[HANGPRINTER_MAX_ANCHORS] = { 0.0F }; // Derived parameters - float k0[HANGPRINTER_AXES] = { 0.0F }; - float spoolRadiiSq[HANGPRINTER_AXES] = { 0.0F }; - float k2[HANGPRINTER_AXES] = { 0.0F }; - float distancesOrigin[HANGPRINTER_AXES] = { 0.0F }; - float springKsOrigin[HANGPRINTER_AXES] = { 0.0F }; - float relaxedSpringLengthsOrigin[HANGPRINTER_AXES] = { 0.0F }; - float fOrigin[HANGPRINTER_AXES] = { 0.0F }; + float k0[HANGPRINTER_MAX_ANCHORS] = { 0.0F }; + float spoolRadiiSq[HANGPRINTER_MAX_ANCHORS] = { 0.0F }; + float k2[HANGPRINTER_MAX_ANCHORS] = { 0.0F }; + float distancesOrigin[HANGPRINTER_MAX_ANCHORS] = { 0.0F }; + float springKsOrigin[HANGPRINTER_MAX_ANCHORS] = { 0.0F }; + float relaxedSpringLengthsOrigin[HANGPRINTER_MAX_ANCHORS] = { 0.0F }; + float fOrigin[HANGPRINTER_MAX_ANCHORS] = { 0.0F }; float printRadiusSquared = 0.0F; float SpringK(float const springLength) const noexcept; - void StaticForces(float const machinePos[3], float F[4]) const noexcept; - void flexDistances(float const machinePos[3], float const distanceA, - float const distanceB, float const distanceC, - float const distanceD, float flex[HANGPRINTER_AXES]) const noexcept; + void StaticForces(float const machinePos[3], float F[HANGPRINTER_MAX_ANCHORS]) const noexcept; + void StaticForcesTetrahedron(float const machinePos[3], float F[HANGPRINTER_MAX_ANCHORS]) const noexcept; + void StaticForcesQuadrilateralPyramid(float const machinePos[3], float F[HANGPRINTER_MAX_ANCHORS]) const noexcept; + void flexDistances(float const machinePos[3], float const distances[HANGPRINTER_MAX_ANCHORS], + float flex[HANGPRINTER_MAX_ANCHORS]) const noexcept; #if DUAL_CAN // Some CAN helpers