Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

3.4 NOCHANGE: hangprinter: simplify things by turning unrolled stuff into loops #584

Closed
wants to merge 2 commits into from
Closed

3.4 NOCHANGE: hangprinter: simplify things by turning unrolled stuff into loops #584

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
84d88c0
HangprinterKinematics::WriteCalibrationParameters: flat return style
jtimon Sep 11, 2022
7f8ee5b
Hangprinter: Loop around the anchors instead of naming them directly
jtimon Sep 8, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
263 changes: 158 additions & 105 deletions src/Movement/Kinematics/HangprinterKinematics.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -107,16 +107,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")
lineLengthsOrigin[A_AXIS] = fastSqrtf(fsquare(anchors[A_AXIS][0]) + fsquare(anchors[A_AXIS][1]) + fsquare(anchors[A_AXIS][2]));
lineLengthsOrigin[B_AXIS] = fastSqrtf(fsquare(anchors[B_AXIS][0]) + fsquare(anchors[B_AXIS][1]) + fsquare(anchors[B_AXIS][2]));
lineLengthsOrigin[C_AXIS] = fastSqrtf(fsquare(anchors[C_AXIS][0]) + fsquare(anchors[C_AXIS][1]) + fsquare(anchors[C_AXIS][2]));
lineLengthsOrigin[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)
{
lineLengthsOrigin[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)
jtimon marked this conversation as resolved.
Show resolved Hide resolved
{
const uint8_t driver = platform.GetAxisDriversConfig(i).driverNumbers[0].localDriver; // Only supports single driver
bool dummy;
Expand Down Expand Up @@ -152,27 +151,14 @@ bool HangprinterKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, const
if (mCode == 669)
{
const bool seenNonGeometry = TryConfigureSegmentation(gb);
if (gb.TryGetFloatArray('A', 3, anchors[A_AXIS], reply, seen))
{
error = true;
return true;
}
if (gb.TryGetFloatArray('B', 3, anchors[B_AXIS], reply, seen))
{
error = true;
return true;
}
if (gb.TryGetFloatArray('C', 3, anchors[C_AXIS], reply, seen))
for (size_t i = 0; i < HANGPRINTER_AXES; ++i)
{
error = true;
return true;
}
if (gb.TryGetFloatArray('D', 3, anchors[D_AXIS], reply, seen))
{
error = true;
return true;
if (gb.TryGetFloatArray(ANCHOR_CHARS[i], 3, anchors[i], reply, seen))
{
error = true;
return true;
}
}

if (gb.Seen('P'))
{
printRadius = gb.GetFValue();
Expand All @@ -186,18 +172,12 @@ 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
);
reply.lcatf("P:Print radius: %.1f", (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]);
}
}
}
else if (mCode == 666)
Expand Down Expand Up @@ -233,28 +213,62 @@ bool HangprinterKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, const
error = true;
return true;
}

if (seen)
{
Recalc();
}
else
{
reply.printf(
"Q:Buildup fac %.4f\n"
"R:Spool r %.2f, %.2f, %.2f, %.2f\n"
"U:Mech Adv %d, %d, %d, %d\n"
"O:Lines/spool %d, %d, %d, %d\n"
"L:Motor gear teeth %d, %d, %d, %d\n"
"H:Spool gear teeth %d, %d, %d, %d\n"
"J:Full steps/rev %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],
(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]
);
reply.printf("Q:Buildup fac %.4f", (double)spoolBuildupFactor);
reply.lcat("R:Spool r ");
for (size_t i = 0; i < HANGPRINTER_AXES; ++i)
{
if (i != 0) {
reply.cat(", ");
}
reply.catf("%.2f", (double)spoolRadii[i]);
}
reply.lcat("U:Mech Adv ");
for (size_t i = 0; i < HANGPRINTER_AXES; ++i)
{
if (i != 0) {
reply.cat(", ");
}
reply.catf("%d", (int)mechanicalAdvantage[i]);
}
reply.lcat("O:Lines/spool ");
for (size_t i = 0; i < HANGPRINTER_AXES; ++i)
{
if (i != 0) {
reply.cat(", ");
}
reply.catf("%d", (int)linesPerSpool[i]);
}
reply.lcat("L:Motor gear teeth ");
for (size_t i = 0; i < HANGPRINTER_AXES; ++i)
{
if (i != 0) {
reply.cat(", ");
}
reply.catf("%d", (int)motorGearTeeth[i]);
}
reply.lcat("H:Spool gear teeth ");
for (size_t i = 0; i < HANGPRINTER_AXES; ++i)
{
if (i != 0) {
reply.cat(", ");
}
reply.catf("%d", (int)spoolGearTeeth[i]);
}
reply.lcat("J:Full steps/rev ");
for (size_t i = 0; i < HANGPRINTER_AXES; ++i)
{
if (i != 0) {
reply.cat(", ");
}
reply.catf("%d", (int)fullStepsPerMotorRev[i]);
}
}
}
else
Expand All @@ -275,21 +289,21 @@ bool HangprinterKinematics::CartesianToMotorSteps(const float machinePos[], cons
size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const noexcept
{
float squaredLineLengths[HANGPRINTER_AXES];
squaredLineLengths[A_AXIS] = LineLengthSquared(machinePos, anchors[A_AXIS]);
jtimon marked this conversation as resolved.
Show resolved Hide resolved
squaredLineLengths[B_AXIS] = LineLengthSquared(machinePos, anchors[B_AXIS]);
squaredLineLengths[C_AXIS] = LineLengthSquared(machinePos, anchors[C_AXIS]);
squaredLineLengths[D_AXIS] = LineLengthSquared(machinePos, anchors[D_AXIS]);
for (size_t i = 0; i < HANGPRINTER_AXES; ++i)
{
squaredLineLengths[i] = LineLengthSquared(machinePos, anchors[i]);
}

float linePos[HANGPRINTER_AXES];
for (size_t i = 0; i < HANGPRINTER_AXES; ++i)
{
linePos[i] = fastSqrtf(squaredLineLengths[i]) - lineLengthsOrigin[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;
}
Expand Down Expand Up @@ -443,48 +457,85 @@ 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)
if (!f->Write("; Hangprinter parameters\n"))
{
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\n",
(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());
}
}
}
return false;
}

String<255> scratchString;
scratchString.printf("M669 K6 P%.1f ", (double)printRadius);

for (size_t i = 0; i < HANGPRINTER_AXES; ++i)
{
scratchString.catf("%c%.3f:%.3f:%.3f ", ANCHOR_CHARS[i], (double)anchors[i][X_AXIS], (double)anchors[i][Y_AXIS], (double)anchors[i][Z_AXIS]);
}
if (!f->Write(scratchString.c_str()))
{
return false;
}

scratchString.printf("M666 Q%.6f ", (double)spoolBuildupFactor);
if (!f->Write(scratchString.c_str()))
{
return false;
}

scratchString.printf(" R%.3f", (double)spoolRadii[0]);
for (size_t i = 1; i < HANGPRINTER_AXES; ++i)
{
scratchString.catf(":%.3f", (double)spoolRadii[i]);
}
if (!f->Write(scratchString.c_str()))
{
return false;
}

scratchString.printf(" U%.3f", (double)mechanicalAdvantage[0]);
for (size_t i = 1; i < HANGPRINTER_AXES; ++i)
{
scratchString.catf(":%.3f", (double)mechanicalAdvantage[i]);
}
if (!f->Write(scratchString.c_str()))
{
return false;
}

scratchString.printf(" O%.3f", (double)linesPerSpool[0]);
for (size_t i = 1; i < HANGPRINTER_AXES; ++i)
{
scratchString.catf(":%.3f", (double)linesPerSpool[i]);
}
if (!f->Write(scratchString.c_str()))
{
return false;
}

scratchString.printf(" L%.3f", (double)motorGearTeeth[0]);
for (size_t i = 1; i < HANGPRINTER_AXES; ++i)
{
scratchString.catf(":%.3f", (double)motorGearTeeth[i]);
}
if (!f->Write(scratchString.c_str()))
{
return false;
}

scratchString.printf(" H%.3f", (double)spoolGearTeeth[0]);
for (size_t i = 1; i < HANGPRINTER_AXES; ++i)
{
scratchString.catf(":%.3f", (double)spoolGearTeeth[i]);
}
if (!f->Write(scratchString.c_str()))
{
return false;
}
return ok;

scratchString.printf(" J%.3f", (double)fullStepsPerMotorRev[0]);
for (size_t i = 1; i < HANGPRINTER_AXES; ++i)
{
scratchString.catf(":%.3f", (double)fullStepsPerMotorRev[i]);
}
return f->Write(scratchString.c_str());
}

// Write any calibration data that we need to resume a print after power fail, returning true if successful
Expand Down Expand Up @@ -587,10 +638,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
Expand Down
1 change: 1 addition & 0 deletions src/Movement/Kinematics/HangprinterKinematics.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,6 +52,7 @@ class HangprinterKinematics : public RoundBedKinematics

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;
Expand Down