Skip to content

Commit 049b547

Browse files
jtimonjtimon
committed
Hangprinter: Loop around the anchors instead of naming them directly
1 parent ccf9d37 commit 049b547

File tree

1 file changed

+171
-79
lines changed

1 file changed

+171
-79
lines changed

‎src/Movement/Kinematics/HangprinterKinematics.cpp‎

Lines changed: 171 additions & 79 deletions
Original file line numberDiff line numberDiff line change
@@ -129,16 +129,15 @@ void HangprinterKinematics::Recalc() noexcept
129129

130130
// This is the difference between a "line length" and a "line position"
131131
// "line length" == ("line position" + "line length in origin")
132-
distancesOrigin[A_AXIS] = fastSqrtf(fsquare(anchors[A_AXIS][0]) + fsquare(anchors[A_AXIS][1]) + fsquare(anchors[A_AXIS][2]));
133-
distancesOrigin[B_AXIS] = fastSqrtf(fsquare(anchors[B_AXIS][0]) + fsquare(anchors[B_AXIS][1]) + fsquare(anchors[B_AXIS][2]));
134-
distancesOrigin[C_AXIS] = fastSqrtf(fsquare(anchors[C_AXIS][0]) + fsquare(anchors[C_AXIS][1]) + fsquare(anchors[C_AXIS][2]));
135-
distancesOrigin[D_AXIS] = fastSqrtf(fsquare(anchors[D_AXIS][0]) + fsquare(anchors[D_AXIS][1]) + fsquare(anchors[D_AXIS][2]));
136-
132+
for (size_t i = 0; i < HANGPRINTER_AXES; ++i)
133+
{
134+
distancesOrigin[i] = fastSqrtf(fsquare(anchors[i][0]) + fsquare(anchors[i][1]) + fsquare(anchors[i][2]));
135+
}
137136

138137
//// Line buildup compensation
139138
float stepsPerUnitTimesRTmp[HANGPRINTER_AXES] = { 0.0 };
140139
Platform& platform = reprap.GetPlatform(); // No const because we want to set drive steper per unit
141-
for (size_t i = 0; i < HANGPRINTER_AXES; i++)
140+
for (size_t i = 0; i < HANGPRINTER_AXES; ++i)
142141
{
143142
const uint8_t driver = platform.GetAxisDriversConfig(i).driverNumbers[0].localDriver; // Only supports single driver
144143
bool dummy;
@@ -203,11 +202,10 @@ bool HangprinterKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, const
203202
if (mCode == 669)
204203
{
205204
const bool seenNonGeometry = TryConfigureSegmentation(gb);
206-
gb.TryGetFloatArray('A', 3, anchors[A_AXIS], seen);
207-
gb.TryGetFloatArray('B', 3, anchors[B_AXIS], seen);
208-
gb.TryGetFloatArray('C', 3, anchors[C_AXIS], seen);
209-
gb.TryGetFloatArray('D', 3, anchors[D_AXIS], seen);
210-
205+
for (size_t i = 0; i < HANGPRINTER_AXES; ++i)
206+
{
207+
gb.TryGetFloatArray(ANCHOR_CHARS[i], 3, anchors[i], reply, seen);
208+
}
211209
if (gb.Seen('P'))
212210
{
213211
printRadius = gb.GetPositiveFValue();
@@ -221,18 +219,12 @@ bool HangprinterKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, const
221219
else if (!seenNonGeometry && !gb.Seen('K'))
222220
{
223221
Kinematics::Configure(mCode, gb, reply, error);
224-
reply.lcatf(
225-
"A:%.2f, %.2f, %.2f\n"
226-
"B:%.2f, %.2f, %.2f\n"
227-
"C:%.2f, %.2f, %.2f\n"
228-
"D:%.2f, %.2f, %.2f\n"
229-
"P:Print radius: %.1f",
230-
(double)anchors[A_AXIS][X_AXIS], (double)anchors[A_AXIS][Y_AXIS], (double)anchors[A_AXIS][Z_AXIS],
231-
(double)anchors[B_AXIS][X_AXIS], (double)anchors[B_AXIS][Y_AXIS], (double)anchors[B_AXIS][Z_AXIS],
232-
(double)anchors[C_AXIS][X_AXIS], (double)anchors[C_AXIS][Y_AXIS], (double)anchors[C_AXIS][Z_AXIS],
233-
(double)anchors[D_AXIS][X_AXIS], (double)anchors[D_AXIS][Y_AXIS], (double)anchors[D_AXIS][Z_AXIS],
234-
(double)printRadius
235-
);
222+
for (size_t i = 0; i < HANGPRINTER_AXES; ++i)
223+
{
224+
reply.lcatf("%c:%.2f, %.2f, %.2f",
225+
ANCHOR_CHARS[i], (double)anchors[i][X_AXIS], (double)anchors[i][Y_AXIS], (double)anchors[i][Z_AXIS]);
226+
}
227+
reply.lcatf("P:Print radius: %.1f", (double)printRadius);
236228
}
237229
}
238230
else if (mCode == 666)
@@ -257,35 +249,99 @@ bool HangprinterKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, const
257249
else
258250
{
259251
reply.printf(
260-
"M666 Q%.4f\n"
261-
"R%.2f:%.2f:%.2f:%.2f\n"
262-
"U%d:%d:%d:%d\n"
263-
"O%d:%d:%d:%d\n"
264-
"L%d:%d:%d:%d\n"
265-
"H%d:%d:%d:%d\n"
266-
"J%d:%d:%d:%d\n"
267-
"W%.2f\n"
268-
"S%.2f\n"
269-
"I%.1f:%.1f:%.1f:%.1f\n"
270-
"X%.1f:%.1f:%.1f:%.1f\n"
271-
"Y%.1f:%.1f:%.1f:%.1f\n"
272-
"T%.1f\n"
273-
"C%.4f:%.4f:%.4f:%.4f",
274-
(double)spoolBuildupFactor,
275-
(double)spoolRadii[A_AXIS], (double)spoolRadii[B_AXIS], (double)spoolRadii[C_AXIS], (double)spoolRadii[D_AXIS],
276-
(int)mechanicalAdvantage[A_AXIS], (int)mechanicalAdvantage[B_AXIS], (int)mechanicalAdvantage[C_AXIS], (int)mechanicalAdvantage[D_AXIS],
277-
(int)linesPerSpool[A_AXIS], (int)linesPerSpool[B_AXIS], (int)linesPerSpool[C_AXIS], (int)linesPerSpool[D_AXIS],
278-
(int)motorGearTeeth[A_AXIS], (int)motorGearTeeth[B_AXIS], (int)motorGearTeeth[C_AXIS], (int)motorGearTeeth[D_AXIS],
279-
(int)spoolGearTeeth[A_AXIS], (int)spoolGearTeeth[B_AXIS], (int)spoolGearTeeth[C_AXIS], (int)spoolGearTeeth[D_AXIS],
280-
(int)fullStepsPerMotorRev[A_AXIS], (int)fullStepsPerMotorRev[B_AXIS], (int)fullStepsPerMotorRev[C_AXIS], (int)fullStepsPerMotorRev[D_AXIS],
281-
(double)moverWeight_kg,
282-
(double)springKPerUnitLength,
283-
(double)minPlannedForce_Newton[A_AXIS], (double)minPlannedForce_Newton[B_AXIS], (double)minPlannedForce_Newton[C_AXIS], (double)minPlannedForce_Newton[D_AXIS],
284-
(double)maxPlannedForce_Newton[A_AXIS], (double)maxPlannedForce_Newton[B_AXIS], (double)maxPlannedForce_Newton[C_AXIS], (double)maxPlannedForce_Newton[D_AXIS],
285-
(double)guyWireLengths[A_AXIS], (double)guyWireLengths[B_AXIS], (double)guyWireLengths[C_AXIS], (double)guyWireLengths[D_AXIS],
286-
(double)targetForce_Newton,
287-
(double)torqueConstants[A_AXIS], (double)torqueConstants[B_AXIS], (double)torqueConstants[C_AXIS], (double)torqueConstants[D_AXIS]
288-
);
252+
"M666 Q%.4f\n",
253+
(double)spoolBuildupFactor,
254+
);
255+
reply.lcat("R:Spool r ");
256+
for (size_t i = 0; i < HANGPRINTER_AXES; ++i)
257+
{
258+
if (i != 0) {
259+
reply.cat(", ");
260+
}
261+
reply.catf("%.2f", (double)spoolRadii[i]);
262+
}
263+
reply.lcat("U:Mech Adv ");
264+
for (size_t i = 0; i < HANGPRINTER_AXES; ++i)
265+
{
266+
if (i != 0) {
267+
reply.cat(", ");
268+
}
269+
reply.catf("%d", (int)mechanicalAdvantage[i]);
270+
}
271+
reply.lcat("O:Lines/spool ");
272+
for (size_t i = 0; i < HANGPRINTER_AXES; ++i)
273+
{
274+
if (i != 0) {
275+
reply.cat(", ");
276+
}
277+
reply.catf("%d", (int)linesPerSpool[i]);
278+
}
279+
reply.lcat("L:Motor gear teeth ");
280+
for (size_t i = 0; i < HANGPRINTER_AXES; ++i)
281+
{
282+
if (i != 0) {
283+
reply.cat(", ");
284+
}
285+
reply.catf("%d", (int)motorGearTeeth[i]);
286+
}
287+
reply.lcat("H:Spool gear teeth ");
288+
for (size_t i = 0; i < HANGPRINTER_AXES; ++i)
289+
{
290+
if (i != 0) {
291+
reply.cat(", ");
292+
}
293+
reply.catf("%d", (int)spoolGearTeeth[i]);
294+
}
295+
reply.lcat("J:Full steps/rev ");
296+
for (size_t i = 0; i < HANGPRINTER_AXES; ++i)
297+
{
298+
if (i != 0) {
299+
reply.cat(", ");
300+
}
301+
reply.catf("%d", (int)fullStepsPerMotorRev[i]);
302+
}
303+
304+
reply.lcat("W%.2f\n", (double)moverWeight_kg);
305+
reply.lcat("S%.2f\n", (double)springKPerUnitLength);
306+
307+
reply.lcat("I%.1f:%.1f:%.1f:%.1f\n ");
308+
for (size_t i = 0; i < HANGPRINTER_AXES; ++i)
309+
{
310+
if (i != 0) {
311+
reply.cat(", ");
312+
}
313+
reply.catf("%d", (int)minPlannedForce_Newton[i]);
314+
}
315+
316+
reply.lcat("X%.1f:%.1f:%.1f:%.1f\n ");
317+
for (size_t i = 0; i < HANGPRINTER_AXES; ++i)
318+
{
319+
if (i != 0) {
320+
reply.cat(", ");
321+
}
322+
reply.catf("%d", (int)maxPlannedForce_Newton[i]);
323+
}
324+
325+
reply.lcat("Y%.1f:%.1f:%.1f:%.1f\n ");
326+
for (size_t i = 0; i < HANGPRINTER_AXES; ++i)
327+
{
328+
if (i != 0) {
329+
reply.cat(", ");
330+
}
331+
reply.catf("%d", (int)guyWireLengths[i]);
332+
}
333+
334+
reply.lcat("T%.1f\n", (double)targetForce_Newton);
335+
336+
reply.lcat("C%.4f:%.4f:%.4f:%.4f\n ");
337+
for (size_t i = 0; i < HANGPRINTER_AXES; ++i)
338+
{
339+
if (i != 0) {
340+
reply.cat(", ");
341+
}
342+
reply.catf("%d", (int)torqueConstants[i]);
343+
}
344+
289345
}
290346
}
291347
else
@@ -300,11 +356,9 @@ bool HangprinterKinematics::CartesianToMotorSteps(const float machinePos[], cons
300356
size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const noexcept
301357
{
302358
float distances[HANGPRINTER_AXES];
303-
distances[A_AXIS] = hyp3(machinePos, anchors[A_AXIS]);
304-
distances[B_AXIS] = hyp3(machinePos, anchors[B_AXIS]);
305-
distances[C_AXIS] = hyp3(machinePos, anchors[C_AXIS]);
306-
distances[D_AXIS] = hyp3(machinePos, anchors[D_AXIS]);
307-
359+
for (size_t i{0}; i < HANGPRINTER_AXES; ++i) {
360+
distances[i] = hyp3(machinePos, anchors[i]);
361+
}
308362

309363
float springKs[HANGPRINTER_AXES];
310364
for (size_t i{0}; i < HANGPRINTER_AXES; ++i) {
@@ -325,10 +379,10 @@ bool HangprinterKinematics::CartesianToMotorSteps(const float machinePos[], cons
325379
linePos[i] = relaxedSpringLengths[i] - relaxedSpringLengthsOrigin[i];
326380
}
327381

328-
motorPos[A_AXIS] = lrintf(k0[A_AXIS] * (fastSqrtf(spoolRadiiSq[A_AXIS] + linePos[A_AXIS] * k2[A_AXIS]) - spoolRadii[A_AXIS]));
329-
motorPos[B_AXIS] = lrintf(k0[B_AXIS] * (fastSqrtf(spoolRadiiSq[B_AXIS] + linePos[B_AXIS] * k2[B_AXIS]) - spoolRadii[B_AXIS]));
330-
motorPos[C_AXIS] = lrintf(k0[C_AXIS] * (fastSqrtf(spoolRadiiSq[C_AXIS] + linePos[C_AXIS] * k2[C_AXIS]) - spoolRadii[C_AXIS]));
331-
motorPos[D_AXIS] = lrintf(k0[D_AXIS] * (fastSqrtf(spoolRadiiSq[D_AXIS] + linePos[D_AXIS] * k2[D_AXIS]) - spoolRadii[D_AXIS]));
382+
for (size_t i = 0; i < HANGPRINTER_AXES; ++i)
383+
{
384+
motorPos[i] = lrintf(k0[i] * (fastSqrtf(spoolRadiiSq[i] + linePos[i] * k2[i]) - spoolRadii[i]));
385+
}
332386

333387
return true;
334388
}
@@ -555,54 +609,90 @@ bool HangprinterKinematics::WriteCalibrationParameters(FileStore *f) const noexc
555609
ok = f->Write(scratchString.c_str());
556610
if (!ok) return false;
557611

558-
scratchString.printf("R%.3f:%.3f:%.3f:%.3f", (double)spoolRadii[A_AXIS], (double)spoolRadii[B_AXIS], (double)spoolRadii[C_AXIS], (double)spoolRadii[D_AXIS]);
612+
scratchString.printf(" R%.3f", (double)spoolRadii[0]);
613+
for (size_t i = 1; i < HANGPRINTER_AXES; ++i)
614+
{
615+
scratchString.catf(":%.3f", (double)spoolRadii[i]);
616+
}
559617
ok = f->Write(scratchString.c_str());
560618
if (!ok) return false;
561619

562-
scratchString.printf("U%d:%d:%d:%d", (int)mechanicalAdvantage[A_AXIS], (int)mechanicalAdvantage[B_AXIS], (int)mechanicalAdvantage[C_AXIS], (int)mechanicalAdvantage[D_AXIS]);
620+
scratchString.printf(" U%.3f", (double)mechanicalAdvantage[0]);
621+
for (size_t i = 1; i < HANGPRINTER_AXES; ++i)
622+
{
623+
scratchString.catf(":%.3f", (double)mechanicalAdvantage[i]);
624+
}
563625
ok = f->Write(scratchString.c_str());
564626
if (!ok) return false;
565627

566-
scratchString.printf(" O%d:%d:%d:%d", (int)linesPerSpool[A_AXIS], (int)linesPerSpool[B_AXIS], (int)linesPerSpool[C_AXIS], (int)linesPerSpool[D_AXIS]);
628+
scratchString.printf(" O%.3f", (double)linesPerSpool[0]);
629+
for (size_t i = 1; i < HANGPRINTER_AXES; ++i)
630+
{
631+
scratchString.catf(":%.3f", (double)linesPerSpool[i]);
632+
}
567633
ok = f->Write(scratchString.c_str());
568634
if (!ok) return false;
569635

570-
scratchString.printf(" L%d:%d:%d:%d", (int)motorGearTeeth[A_AXIS], (int)motorGearTeeth[B_AXIS], (int)motorGearTeeth[C_AXIS], (int)motorGearTeeth[D_AXIS]);
636+
scratchString.printf(" L%.3f", (double)motorGearTeeth[0]);
637+
for (size_t i = 1; i < HANGPRINTER_AXES; ++i)
638+
{
639+
scratchString.catf(":%.3f", (double)motorGearTeeth[i]);
640+
}
571641
ok = f->Write(scratchString.c_str());
572642
if (!ok) return false;
573643

574-
scratchString.printf(" H%d:%d:%d:%d", (int)motorGearTeeth[A_AXIS], (int)motorGearTeeth[B_AXIS], (int)motorGearTeeth[C_AXIS], (int)motorGearTeeth[D_AXIS]);
644+
scratchString.printf(" H%.3f", (double)spoolGearTeeth[0]);
645+
for (size_t i = 1; i < HANGPRINTER_AXES; ++i)
646+
{
647+
scratchString.catf(":%.3f", (double)spoolGearTeeth[i]);
648+
}
575649
ok = f->Write(scratchString.c_str());
576650
if (!ok) return false;
577651

578-
scratchString.printf(" J%d:%d:%d:%d", (int)fullStepsPerMotorRev[A_AXIS], (int)fullStepsPerMotorRev[B_AXIS], (int)fullStepsPerMotorRev[C_AXIS], (int)fullStepsPerMotorRev[D_AXIS]);
652+
scratchString.printf(" J%.3f", (double)fullStepsPerMotorRev[0]);
653+
for (size_t i = 1; i < HANGPRINTER_AXES; ++i)
654+
{
655+
scratchString.catf(":%.3f", (double)fullStepsPerMotorRev[i]);
656+
}
579657
ok = f->Write(scratchString.c_str());
580-
if (!ok) return false;
581658

582659
scratchString.printf(" W%.2f S%.2f", (double)moverWeight_kg, (double)springKPerUnitLength);
583660
ok = f->Write(scratchString.c_str());
584661
if (!ok) return false;
585662

586-
scratchString.printf(" I%.1f:%.1f:%.1f:%.1f",
587-
(double)minPlannedForce_Newton[A_AXIS], (double)minPlannedForce_Newton[B_AXIS], (double)minPlannedForce_Newton[C_AXIS], (double)minPlannedForce_Newton[D_AXIS]);
663+
scratchString.printf(" I%.1f", (double)minPlannedForce_Newton[0]);
664+
for (size_t i = 1; i < HANGPRINTER_AXES; ++i)
665+
{
666+
scratchString.catf(":%.1f", (double)minPlannedForce_Newton[i]);
667+
}
588668
ok = f->Write(scratchString.c_str());
589669
if (!ok) return false;
590670

591-
scratchString.printf(" X%.1f:%.1f:%.1f:%.1f",
592-
(double)maxPlannedForce_Newton[A_AXIS], (double)maxPlannedForce_Newton[B_AXIS], (double)maxPlannedForce_Newton[C_AXIS], (double)maxPlannedForce_Newton[D_AXIS]);
671+
scratchString.printf(" X%.1f", (double)maxPlannedForce_Newton[0]);
672+
for (size_t i = 1; i < HANGPRINTER_AXES; ++i)
673+
{
674+
scratchString.catf(":%.1f", (double)maxPlannedForce_Newton[i]);
675+
}
593676
ok = f->Write(scratchString.c_str());
594677
if (!ok) return false;
595678

596-
scratchString.printf(" Y%.1f:%.1f:%.1f:%.1f",
597-
(double)guyWireLengths[A_AXIS], (double)guyWireLengths[B_AXIS], (double)guyWireLengths[C_AXIS], (double)guyWireLengths[D_AXIS]);
679+
scratchString.printf(" Y%.1f", (double)guyWireLengths[0]);
680+
for (size_t i = 1; i < HANGPRINTER_AXES; ++i)
681+
{
682+
scratchString.catf(":%.1f", (double)guyWireLengths[i]);
683+
}
598684
ok = f->Write(scratchString.c_str());
599685
if (!ok) return false;
600686

601687
scratchString.printf(" T%.1f", (double)targetForce_Newton);
602688
ok = f->Write(scratchString.c_str());
603689
if (!ok) return false;
604690

605-
scratchString.printf(" C%.4f:%.4f:%.4f:%.4f\n", (double)torqueConstants[A_AXIS], (double)torqueConstants[B_AXIS], (double)torqueConstants[C_AXIS], (double)torqueConstants[D_AXIS]);
691+
scratchString.printf(" C%.4f", (double)torqueConstants[0]);
692+
for (size_t i = 1; i < HANGPRINTER_AXES; ++i)
693+
{
694+
scratchString.catf(":%.4f", (double)torqueConstants[i]);
695+
}
606696
ok = f->Write(scratchString.c_str());
607697

608698
return ok;
@@ -708,10 +798,12 @@ void HangprinterKinematics::ForwardTransform(float const a, float const b, float
708798
// Print all the parameters for debugging
709799
void HangprinterKinematics::PrintParameters(const StringRef& reply) const noexcept
710800
{
711-
reply.printf("Anchor coordinates (%.2f,%.2f,%.2f) (%.2f,%.2f,%.2f) (%.2f,%.2f,%.2f)\n",
712-
(double)anchors[A_AXIS][X_AXIS], (double)anchors[A_AXIS][Y_AXIS], (double)anchors[A_AXIS][Z_AXIS],
713-
(double)anchors[B_AXIS][X_AXIS], (double)anchors[B_AXIS][Y_AXIS], (double)anchors[B_AXIS][Z_AXIS],
714-
(double)anchors[C_AXIS][X_AXIS], (double)anchors[C_AXIS][Y_AXIS], (double)anchors[C_AXIS][Z_AXIS]);
801+
reply.printf("Anchor coordinates");
802+
for (size_t i = 0; i < HANGPRINTER_AXES; ++i)
803+
{
804+
reply.catf(" (%.2f,%.2f,%.2f)", (double)anchors[i][X_AXIS], (double)anchors[i][Y_AXIS], (double)anchors[i][Z_AXIS]);
805+
}
806+
reply.cat("\n");
715807
}
716808

717809
#if DUAL_CAN

0 commit comments

Comments
 (0)