Skip to content

Commit 44d22c2

Browse files
jtimonjtimon
committed
Hangprinter: Loop around the anchors instead of naming them directly
1 parent 6e47080 commit 44d22c2

File tree

1 file changed

+169
-80
lines changed

1 file changed

+169
-80
lines changed

‎src/Movement/Kinematics/HangprinterKinematics.cpp‎

Lines changed: 169 additions & 80 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], 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)
@@ -256,36 +248,97 @@ bool HangprinterKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, const
256248
}
257249
else
258250
{
259-
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-
);
251+
reply.printf("M666 Q%.4f\n", (double)spoolBuildupFactor);
252+
reply.lcat("R:Spool r ");
253+
for (size_t i = 0; i < HANGPRINTER_AXES; ++i)
254+
{
255+
if (i != 0) {
256+
reply.cat(", ");
257+
}
258+
reply.catf("%.2f", (double)spoolRadii[i]);
259+
}
260+
reply.lcat("U:Mech Adv ");
261+
for (size_t i = 0; i < HANGPRINTER_AXES; ++i)
262+
{
263+
if (i != 0) {
264+
reply.cat(", ");
265+
}
266+
reply.catf("%d", (int)mechanicalAdvantage[i]);
267+
}
268+
reply.lcat("O:Lines/spool ");
269+
for (size_t i = 0; i < HANGPRINTER_AXES; ++i)
270+
{
271+
if (i != 0) {
272+
reply.cat(", ");
273+
}
274+
reply.catf("%d", (int)linesPerSpool[i]);
275+
}
276+
reply.lcat("L:Motor gear teeth ");
277+
for (size_t i = 0; i < HANGPRINTER_AXES; ++i)
278+
{
279+
if (i != 0) {
280+
reply.cat(", ");
281+
}
282+
reply.catf("%d", (int)motorGearTeeth[i]);
283+
}
284+
reply.lcat("H:Spool gear teeth ");
285+
for (size_t i = 0; i < HANGPRINTER_AXES; ++i)
286+
{
287+
if (i != 0) {
288+
reply.cat(", ");
289+
}
290+
reply.catf("%d", (int)spoolGearTeeth[i]);
291+
}
292+
reply.lcat("J:Full steps/rev ");
293+
for (size_t i = 0; i < HANGPRINTER_AXES; ++i)
294+
{
295+
if (i != 0) {
296+
reply.cat(", ");
297+
}
298+
reply.catf("%d", (int)fullStepsPerMotorRev[i]);
299+
}
300+
301+
reply.lcatf("W %.2f\n", (double)moverWeight_kg);
302+
reply.lcatf("S %.2f\n", (double)springKPerUnitLength);
303+
304+
reply.lcat("I%.1f:%.1f:%.1f:%.1f\n ");
305+
for (size_t i = 0; i < HANGPRINTER_AXES; ++i)
306+
{
307+
if (i != 0) {
308+
reply.cat(", ");
309+
}
310+
reply.catf("%d", (int)minPlannedForce_Newton[i]);
311+
}
312+
313+
reply.lcat("X%.1f:%.1f:%.1f:%.1f\n ");
314+
for (size_t i = 0; i < HANGPRINTER_AXES; ++i)
315+
{
316+
if (i != 0) {
317+
reply.cat(", ");
318+
}
319+
reply.catf("%d", (int)maxPlannedForce_Newton[i]);
320+
}
321+
322+
reply.lcat("Y%.1f:%.1f:%.1f:%.1f\n ");
323+
for (size_t i = 0; i < HANGPRINTER_AXES; ++i)
324+
{
325+
if (i != 0) {
326+
reply.cat(", ");
327+
}
328+
reply.catf("%d", (int)guyWireLengths[i]);
329+
}
330+
331+
reply.lcatf("T%.1f\n", (double)targetForce_Newton);
332+
333+
reply.lcat("C%.4f:%.4f:%.4f:%.4f\n ");
334+
for (size_t i = 0; i < HANGPRINTER_AXES; ++i)
335+
{
336+
if (i != 0) {
337+
reply.cat(", ");
338+
}
339+
reply.catf("%d", (int)torqueConstants[i]);
340+
}
341+
289342
}
290343
}
291344
else
@@ -300,11 +353,9 @@ bool HangprinterKinematics::CartesianToMotorSteps(const float machinePos[], cons
300353
size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const noexcept
301354
{
302355
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-
356+
for (size_t i{0}; i < HANGPRINTER_AXES; ++i) {
357+
distances[i] = hyp3(machinePos, anchors[i]);
358+
}
308359

309360
float springKs[HANGPRINTER_AXES];
310361
for (size_t i{0}; i < HANGPRINTER_AXES; ++i) {
@@ -325,10 +376,10 @@ bool HangprinterKinematics::CartesianToMotorSteps(const float machinePos[], cons
325376
linePos[i] = relaxedSpringLengths[i] - relaxedSpringLengthsOrigin[i];
326377
}
327378

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]));
379+
for (size_t i = 0; i < HANGPRINTER_AXES; ++i)
380+
{
381+
motorPos[i] = lrintf(k0[i] * (fastSqrtf(spoolRadiiSq[i] + linePos[i] * k2[i]) - spoolRadii[i]));
382+
}
332383

333384
return true;
334385
}
@@ -555,54 +606,90 @@ bool HangprinterKinematics::WriteCalibrationParameters(FileStore *f) const noexc
555606
ok = f->Write(scratchString.c_str());
556607
if (!ok) return false;
557608

558-
scratchString.printf("R%.3f:%.3f:%.3f:%.3f", (double)spoolRadii[A_AXIS], (double)spoolRadii[B_AXIS], (double)spoolRadii[C_AXIS], (double)spoolRadii[D_AXIS]);
609+
scratchString.printf(" R%.3f", (double)spoolRadii[0]);
610+
for (size_t i = 1; i < HANGPRINTER_AXES; ++i)
611+
{
612+
scratchString.catf(":%.3f", (double)spoolRadii[i]);
613+
}
559614
ok = f->Write(scratchString.c_str());
560615
if (!ok) return false;
561616

562-
scratchString.printf("U%d:%d:%d:%d", (int)mechanicalAdvantage[A_AXIS], (int)mechanicalAdvantage[B_AXIS], (int)mechanicalAdvantage[C_AXIS], (int)mechanicalAdvantage[D_AXIS]);
617+
scratchString.printf(" U%.3f", (double)mechanicalAdvantage[0]);
618+
for (size_t i = 1; i < HANGPRINTER_AXES; ++i)
619+
{
620+
scratchString.catf(":%.3f", (double)mechanicalAdvantage[i]);
621+
}
563622
ok = f->Write(scratchString.c_str());
564623
if (!ok) return false;
565624

566-
scratchString.printf(" O%d:%d:%d:%d", (int)linesPerSpool[A_AXIS], (int)linesPerSpool[B_AXIS], (int)linesPerSpool[C_AXIS], (int)linesPerSpool[D_AXIS]);
625+
scratchString.printf(" O%.3f", (double)linesPerSpool[0]);
626+
for (size_t i = 1; i < HANGPRINTER_AXES; ++i)
627+
{
628+
scratchString.catf(":%.3f", (double)linesPerSpool[i]);
629+
}
567630
ok = f->Write(scratchString.c_str());
568631
if (!ok) return false;
569632

570-
scratchString.printf(" L%d:%d:%d:%d", (int)motorGearTeeth[A_AXIS], (int)motorGearTeeth[B_AXIS], (int)motorGearTeeth[C_AXIS], (int)motorGearTeeth[D_AXIS]);
633+
scratchString.printf(" L%.3f", (double)motorGearTeeth[0]);
634+
for (size_t i = 1; i < HANGPRINTER_AXES; ++i)
635+
{
636+
scratchString.catf(":%.3f", (double)motorGearTeeth[i]);
637+
}
571638
ok = f->Write(scratchString.c_str());
572639
if (!ok) return false;
573640

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

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

582656
scratchString.printf(" W%.2f S%.2f", (double)moverWeight_kg, (double)springKPerUnitLength);
583657
ok = f->Write(scratchString.c_str());
584658
if (!ok) return false;
585659

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]);
660+
scratchString.printf(" I%.1f", (double)minPlannedForce_Newton[0]);
661+
for (size_t i = 1; i < HANGPRINTER_AXES; ++i)
662+
{
663+
scratchString.catf(":%.1f", (double)minPlannedForce_Newton[i]);
664+
}
588665
ok = f->Write(scratchString.c_str());
589666
if (!ok) return false;
590667

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]);
668+
scratchString.printf(" X%.1f", (double)maxPlannedForce_Newton[0]);
669+
for (size_t i = 1; i < HANGPRINTER_AXES; ++i)
670+
{
671+
scratchString.catf(":%.1f", (double)maxPlannedForce_Newton[i]);
672+
}
593673
ok = f->Write(scratchString.c_str());
594674
if (!ok) return false;
595675

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]);
676+
scratchString.printf(" Y%.1f", (double)guyWireLengths[0]);
677+
for (size_t i = 1; i < HANGPRINTER_AXES; ++i)
678+
{
679+
scratchString.catf(":%.1f", (double)guyWireLengths[i]);
680+
}
598681
ok = f->Write(scratchString.c_str());
599682
if (!ok) return false;
600683

601684
scratchString.printf(" T%.1f", (double)targetForce_Newton);
602685
ok = f->Write(scratchString.c_str());
603686
if (!ok) return false;
604687

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]);
688+
scratchString.printf(" C%.4f", (double)torqueConstants[0]);
689+
for (size_t i = 1; i < HANGPRINTER_AXES; ++i)
690+
{
691+
scratchString.catf(":%.4f", (double)torqueConstants[i]);
692+
}
606693
ok = f->Write(scratchString.c_str());
607694

608695
return ok;
@@ -708,10 +795,12 @@ void HangprinterKinematics::ForwardTransform(float const a, float const b, float
708795
// Print all the parameters for debugging
709796
void HangprinterKinematics::PrintParameters(const StringRef& reply) const noexcept
710797
{
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]);
798+
reply.printf("Anchor coordinates");
799+
for (size_t i = 0; i < HANGPRINTER_AXES; ++i)
800+
{
801+
reply.catf(" (%.2f,%.2f,%.2f)", (double)anchors[i][X_AXIS], (double)anchors[i][Y_AXIS], (double)anchors[i][Z_AXIS]);
802+
}
803+
reply.cat("\n");
715804
}
716805

717806
#if DUAL_CAN

0 commit comments

Comments
 (0)