Skip to content

Commit e19697e

Browse files
jtimonjtimon
committed
Hangprinter: Loop around the anchors instead of naming them directly
1 parent d227a9c commit e19697e

File tree

1 file changed

+143
-80
lines changed

1 file changed

+143
-80
lines changed

‎src/Movement/Kinematics/HangprinterKinematics.cpp‎

Lines changed: 143 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,71 @@ 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+
253+
reply.lcatf("R%.2f", (double)spoolRadii[0]);
254+
for (size_t i = 1; i < HANGPRINTER_AXES; ++i)
255+
{
256+
reply.catf(":%.2f", (double)spoolRadii[i]);
257+
}
258+
259+
reply.lcatf("U%d", (int)mechanicalAdvantage[0]);
260+
for (size_t i = 1; i < HANGPRINTER_AXES; ++i)
261+
{
262+
reply.catf(":%d", (int)mechanicalAdvantage[i]);
263+
}
264+
265+
reply.lcatf("O%d", (int)linesPerSpool[0]);
266+
for (size_t i = 1; i < HANGPRINTER_AXES; ++i)
267+
{
268+
reply.catf(":%d", (int)linesPerSpool[i]);
269+
}
270+
271+
reply.lcatf("L%d", (int)motorGearTeeth[0]);
272+
for (size_t i = 1; i < HANGPRINTER_AXES; ++i)
273+
{
274+
reply.catf(":%d", (int)motorGearTeeth[i]);
275+
}
276+
277+
reply.lcatf("H%d", (int)spoolGearTeeth[0]);
278+
for (size_t i = 1; i < HANGPRINTER_AXES; ++i)
279+
{
280+
reply.catf(":%d", (int)spoolGearTeeth[i]);
281+
}
282+
283+
reply.lcatf("J%d", (int)fullStepsPerMotorRev[0]);
284+
for (size_t i = 1; i < HANGPRINTER_AXES; ++i)
285+
{
286+
reply.catf(":%d", (int)fullStepsPerMotorRev[i]);
287+
}
288+
reply.lcatf("W%.2f\n", (double)moverWeight_kg);
289+
reply.lcatf("S%.2f\n", (double)springKPerUnitLength);
290+
291+
reply.lcatf("I%.1f", (double)minPlannedForce_Newton[0]);
292+
for (size_t i = 1; i < HANGPRINTER_AXES; ++i)
293+
{
294+
reply.catf(":%.1f", (double)minPlannedForce_Newton[i]);
295+
}
296+
297+
reply.lcatf("X%.1f", (double)maxPlannedForce_Newton[0]);
298+
for (size_t i = 1; i < HANGPRINTER_AXES; ++i)
299+
{
300+
reply.catf(":%.1f", (double)maxPlannedForce_Newton[i]);
301+
}
302+
303+
reply.lcatf("Y%.1f", (double)guyWireLengths[0]);
304+
for (size_t i = 1; i < HANGPRINTER_AXES; ++i)
305+
{
306+
reply.catf(":%.1f", (double)guyWireLengths[i]);
307+
}
308+
reply.lcatf("T%.1f\n", (double)targetForce_Newton);
309+
310+
reply.lcatf("C%.4f", (double)torqueConstants[0]);
311+
for (size_t i = 1; i < HANGPRINTER_AXES; ++i)
312+
{
313+
reply.catf(":%.4f", (double)torqueConstants[i]);
314+
}
315+
289316
}
290317
}
291318
else
@@ -300,11 +327,9 @@ bool HangprinterKinematics::CartesianToMotorSteps(const float machinePos[], cons
300327
size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const noexcept
301328
{
302329
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-
330+
for (size_t i{0}; i < HANGPRINTER_AXES; ++i) {
331+
distances[i] = hyp3(machinePos, anchors[i]);
332+
}
308333

309334
float springKs[HANGPRINTER_AXES];
310335
for (size_t i{0}; i < HANGPRINTER_AXES; ++i) {
@@ -325,10 +350,10 @@ bool HangprinterKinematics::CartesianToMotorSteps(const float machinePos[], cons
325350
linePos[i] = relaxedSpringLengths[i] - relaxedSpringLengthsOrigin[i];
326351
}
327352

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]));
353+
for (size_t i = 0; i < HANGPRINTER_AXES; ++i)
354+
{
355+
motorPos[i] = lrintf(k0[i] * (fastSqrtf(spoolRadiiSq[i] + linePos[i] * k2[i]) - spoolRadii[i]));
356+
}
332357

333358
return true;
334359
}
@@ -555,54 +580,90 @@ bool HangprinterKinematics::WriteCalibrationParameters(FileStore *f) const noexc
555580
ok = f->Write(scratchString.c_str());
556581
if (!ok) return false;
557582

558-
scratchString.printf("R%.3f:%.3f:%.3f:%.3f", (double)spoolRadii[A_AXIS], (double)spoolRadii[B_AXIS], (double)spoolRadii[C_AXIS], (double)spoolRadii[D_AXIS]);
583+
scratchString.printf("R%.3f", (double)spoolRadii[0]);
584+
for (size_t i = 1; i < HANGPRINTER_AXES; ++i)
585+
{
586+
scratchString.catf(":%.3f", (double)spoolRadii[i]);
587+
}
559588
ok = f->Write(scratchString.c_str());
560589
if (!ok) return false;
561590

562-
scratchString.printf("U%d:%d:%d:%d", (int)mechanicalAdvantage[A_AXIS], (int)mechanicalAdvantage[B_AXIS], (int)mechanicalAdvantage[C_AXIS], (int)mechanicalAdvantage[D_AXIS]);
591+
scratchString.printf(" U%.3f", (double)mechanicalAdvantage[0]);
592+
for (size_t i = 1; i < HANGPRINTER_AXES; ++i)
593+
{
594+
scratchString.catf(":%.3f", (double)mechanicalAdvantage[i]);
595+
}
563596
ok = f->Write(scratchString.c_str());
564597
if (!ok) return false;
565598

566-
scratchString.printf(" O%d:%d:%d:%d", (int)linesPerSpool[A_AXIS], (int)linesPerSpool[B_AXIS], (int)linesPerSpool[C_AXIS], (int)linesPerSpool[D_AXIS]);
599+
scratchString.printf(" O%.3f", (double)linesPerSpool[0]);
600+
for (size_t i = 1; i < HANGPRINTER_AXES; ++i)
601+
{
602+
scratchString.catf(":%.3f", (double)linesPerSpool[i]);
603+
}
567604
ok = f->Write(scratchString.c_str());
568605
if (!ok) return false;
569606

570-
scratchString.printf(" L%d:%d:%d:%d", (int)motorGearTeeth[A_AXIS], (int)motorGearTeeth[B_AXIS], (int)motorGearTeeth[C_AXIS], (int)motorGearTeeth[D_AXIS]);
607+
scratchString.printf(" L%.3f", (double)motorGearTeeth[0]);
608+
for (size_t i = 1; i < HANGPRINTER_AXES; ++i)
609+
{
610+
scratchString.catf(":%.3f", (double)motorGearTeeth[i]);
611+
}
571612
ok = f->Write(scratchString.c_str());
572613
if (!ok) return false;
573614

574-
scratchString.printf(" H%d:%d:%d:%d", (int)motorGearTeeth[A_AXIS], (int)motorGearTeeth[B_AXIS], (int)motorGearTeeth[C_AXIS], (int)motorGearTeeth[D_AXIS]);
615+
scratchString.printf(" H%.3f", (double)spoolGearTeeth[0]);
616+
for (size_t i = 1; i < HANGPRINTER_AXES; ++i)
617+
{
618+
scratchString.catf(":%.3f", (double)spoolGearTeeth[i]);
619+
}
575620
ok = f->Write(scratchString.c_str());
576621
if (!ok) return false;
577622

578-
scratchString.printf(" J%d:%d:%d:%d", (int)fullStepsPerMotorRev[A_AXIS], (int)fullStepsPerMotorRev[B_AXIS], (int)fullStepsPerMotorRev[C_AXIS], (int)fullStepsPerMotorRev[D_AXIS]);
623+
scratchString.printf(" J%.3f", (double)fullStepsPerMotorRev[0]);
624+
for (size_t i = 1; i < HANGPRINTER_AXES; ++i)
625+
{
626+
scratchString.catf(":%.3f", (double)fullStepsPerMotorRev[i]);
627+
}
579628
ok = f->Write(scratchString.c_str());
580-
if (!ok) return false;
581629

582630
scratchString.printf(" W%.2f S%.2f", (double)moverWeight_kg, (double)springKPerUnitLength);
583631
ok = f->Write(scratchString.c_str());
584632
if (!ok) return false;
585633

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]);
634+
scratchString.printf(" I%.1f", (double)minPlannedForce_Newton[0]);
635+
for (size_t i = 1; i < HANGPRINTER_AXES; ++i)
636+
{
637+
scratchString.catf(":%.1f", (double)minPlannedForce_Newton[i]);
638+
}
588639
ok = f->Write(scratchString.c_str());
589640
if (!ok) return false;
590641

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]);
642+
scratchString.printf(" X%.1f", (double)maxPlannedForce_Newton[0]);
643+
for (size_t i = 1; i < HANGPRINTER_AXES; ++i)
644+
{
645+
scratchString.catf(":%.1f", (double)maxPlannedForce_Newton[i]);
646+
}
593647
ok = f->Write(scratchString.c_str());
594648
if (!ok) return false;
595649

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]);
650+
scratchString.printf(" Y%.1f", (double)guyWireLengths[0]);
651+
for (size_t i = 1; i < HANGPRINTER_AXES; ++i)
652+
{
653+
scratchString.catf(":%.1f", (double)guyWireLengths[i]);
654+
}
598655
ok = f->Write(scratchString.c_str());
599656
if (!ok) return false;
600657

601658
scratchString.printf(" T%.1f", (double)targetForce_Newton);
602659
ok = f->Write(scratchString.c_str());
603660
if (!ok) return false;
604661

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]);
662+
scratchString.printf(" C%.4f", (double)torqueConstants[0]);
663+
for (size_t i = 1; i < HANGPRINTER_AXES; ++i)
664+
{
665+
scratchString.catf(":%.4f", (double)torqueConstants[i]);
666+
}
606667
ok = f->Write(scratchString.c_str());
607668

608669
return ok;
@@ -708,10 +769,12 @@ void HangprinterKinematics::ForwardTransform(float const a, float const b, float
708769
// Print all the parameters for debugging
709770
void HangprinterKinematics::PrintParameters(const StringRef& reply) const noexcept
710771
{
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]);
772+
reply.printf("Anchor coordinates");
773+
for (size_t i = 0; i < HANGPRINTER_AXES; ++i)
774+
{
775+
reply.catf(" (%.2f,%.2f,%.2f)", (double)anchors[i][X_AXIS], (double)anchors[i][Y_AXIS], (double)anchors[i][Z_AXIS]);
776+
}
777+
reply.cat("\n");
715778
}
716779

717780
#if DUAL_CAN

0 commit comments

Comments
 (0)