1717
1818#include < General/Portability.h>
1919
20+ constexpr size_t DefaultNumAnchors = 4 ;
2021// Default anchor coordinates
2122// These are only placeholders. Each machine must have these values calibrated in order to work correctly.
22- constexpr float DefaultAnchors[4 ][3 ] = {{ 0.0 , -2000.0 , -100.0 },
23+ constexpr float DefaultAnchors[5 ][3 ] = {{ 0.0 , -2000.0 , -100.0 },
2324 { 2000.0 , 1000.0 , -100.0 },
2425 {-2000.0 , 1000.0 , -100.0 },
25- { 0.0 , 0.0 , 3000.0 }};
26+ { 0.0 , 0.0 , 3000.0 },
27+ { 0.0 , 0.0 , 0.0 }};
2628constexpr float DefaultPrintRadius = 1500.0 ;
2729
2830#if SUPPORT_OBJECT_MODEL
@@ -44,7 +46,7 @@ constexpr ObjectModelArrayDescriptor HangprinterKinematics::anchorCoordinatesArr
4446constexpr ObjectModelArrayDescriptor HangprinterKinematics::anchorsArrayDescriptor =
4547{
4648 nullptr , // no lock needed
47- [] (const ObjectModel *self, const ObjectExplorationContext&) noexcept -> size_t { return HANGPRINTER_AXES ; },
49+ [] (const ObjectModel *self, const ObjectExplorationContext&) noexcept -> size_t { return HANGPRINTER_MAX_AXES ; },
4850 [] (const ObjectModel *self, ObjectExplorationContext& context) noexcept -> ExpressionValue { return ExpressionValue (&anchorCoordinatesArrayDescriptor); }
4951};
5052
@@ -81,14 +83,15 @@ void HangprinterKinematics::Init() noexcept
8183 * In practice you might want to compensate a bit more or a bit less */
8284 constexpr float DefaultSpoolBuildupFactor = 0.007 ;
8385 /* Measure and set spool radii with M669 to achieve better accuracy */
84- constexpr float DefaultSpoolRadii[4 ] = { 75.0 , 75.0 , 75.0 , 75.0 }; // HP4 default
86+ constexpr float DefaultSpoolRadii[HANGPRINTER_MAX_AXES ] = { 75.0 , 75.0 , 75.0 , 75.0 , 75.0 }; // HP4 default
8587 /* If axis runs lines back through pulley system, set mechanical advantage accordingly with M669 */
86- constexpr uint32_t DefaultMechanicalAdvantage[4 ] = { 2 , 2 , 2 , 4 }; // HP4 default
87- constexpr uint32_t DefaultLinesPerSpool[4 ] = { 1 , 1 , 1 , 1 }; // HP4 default
88- constexpr uint32_t DefaultMotorGearTeeth[4 ] = { 20 , 20 , 20 , 20 }; // HP4 default
89- constexpr uint32_t DefaultSpoolGearTeeth[4 ] = { 255 , 255 , 255 , 255 }; // HP4 default
90- constexpr uint32_t DefaultFullStepsPerMotorRev[4 ] = { 25 , 25 , 25 , 25 };
88+ constexpr uint32_t DefaultMechanicalAdvantage[HANGPRINTER_MAX_AXES ] = { 2 , 2 , 2 , 2 , 4 }; // HP4 default
89+ constexpr uint32_t DefaultLinesPerSpool[HANGPRINTER_MAX_AXES ] = { 1 , 1 , 1 , 1 , 1 }; // HP4 default
90+ constexpr uint32_t DefaultMotorGearTeeth[HANGPRINTER_MAX_AXES ] = { 20 , 20 , 20 , 20 , 20 }; // HP4 default
91+ constexpr uint32_t DefaultSpoolGearTeeth[HANGPRINTER_MAX_AXES ] = { 255 , 255 , 255 , 255 , 255 }; // HP4 default
92+ constexpr uint32_t DefaultFullStepsPerMotorRev[HANGPRINTER_MAX_AXES ] = { 25 , 25 , 25 , 25 , 25 };
9193 ARRAY_INIT (anchors, DefaultAnchors);
94+ numAnchors = DefaultNumAnchors;
9295 printRadius = DefaultPrintRadius;
9396 spoolBuildupFactor = DefaultSpoolBuildupFactor;
9497 ARRAY_INIT (spoolRadii, DefaultSpoolRadii);
@@ -107,15 +110,15 @@ void HangprinterKinematics::Recalc() noexcept
107110
108111 // This is the difference between a "line length" and a "line position"
109112 // "line length" == ("line position" + "line length in origin")
110- for (size_t i = 0 ; i < HANGPRINTER_AXES ; ++i)
113+ for (size_t i = 0 ; i < numAnchors ; ++i)
111114 {
112- lineLengthsOrigin[i] = fastSqrtf (fsquare (anchors[i][0 ]) + fsquare (anchors[i][1 ]) + fsquare (anchors[i][2 ]));
115+ lineLengthsOrigin[i] = fastSqrtf (fsquare (anchors[i][0 ]) + fsquare (anchors[i][1 ]) + fsquare (anchors[i][2 ]));
113116 }
114117
115118 // Line buildup compensation
116- float stepsPerUnitTimesRTmp[HANGPRINTER_AXES] = { 0.0 } ;
119+ float stepsPerUnitTimesRTmp[numAnchors] ;
117120 Platform& platform = reprap.GetPlatform (); // No const because we want to set drive steper per unit
118- for (size_t i = 0 ; i < HANGPRINTER_AXES ; ++i)
121+ for (size_t i = 0 ; i < numAnchors ; ++i)
119122 {
120123 const uint8_t driver = platform.GetAxisDriversConfig (i).driverNumbers [0 ].localDriver ; // Only supports single driver
121124 bool dummy;
@@ -151,7 +154,12 @@ bool HangprinterKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, const
151154 if (mCode == 669 )
152155 {
153156 const bool seenNonGeometry = TryConfigureSegmentation (gb);
154- for (size_t i = 0 ; i < HANGPRINTER_AXES; ++i)
157+ if (gb.Seen (' N' ))
158+ {
159+ numAnchors = gb.GetUIValue ();
160+ seen = true ;
161+ }
162+ for (size_t i = 0 ; i < numAnchors; ++i)
155163 {
156164 if (gb.TryGetFloatArray (ANCHOR_CHARS[i], 3 , anchors[i], reply, seen))
157165 {
@@ -173,7 +181,7 @@ bool HangprinterKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, const
173181 {
174182 Kinematics::Configure (mCode , gb, reply, error);
175183 reply.lcatf (" P:Print radius: %.1f" , (double )printRadius);
176- for (size_t i = 0 ; i < HANGPRINTER_AXES ; ++i)
184+ for (size_t i = 0 ; i < numAnchors ; ++i)
177185 {
178186 reply.lcatf (" %c:%.2f, %.2f, %.2f" ,
179187 ANCHOR_CHARS[i], (double )anchors[i][X_AXIS], (double )anchors[i][Y_AXIS], (double )anchors[i][Z_AXIS]);
@@ -183,32 +191,32 @@ bool HangprinterKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, const
183191 else if (mCode == 666 )
184192 {
185193 gb.TryGetFValue (' Q' , spoolBuildupFactor, seen);
186- if (gb.TryGetFloatArray (' R' , HANGPRINTER_AXES , spoolRadii, reply, seen))
194+ if (gb.TryGetFloatArray (' R' , numAnchors , spoolRadii, reply, seen))
187195 {
188196 error = true ;
189197 return true ;
190198 }
191- if (gb.TryGetUIArray (' U' , HANGPRINTER_AXES , mechanicalAdvantage, reply, seen))
199+ if (gb.TryGetUIArray (' U' , numAnchors , mechanicalAdvantage, reply, seen))
192200 {
193201 error = true ;
194202 return true ;
195203 }
196- if (gb.TryGetUIArray (' O' , HANGPRINTER_AXES , linesPerSpool, reply, seen))
204+ if (gb.TryGetUIArray (' O' , numAnchors , linesPerSpool, reply, seen))
197205 {
198206 error = true ;
199207 return true ;
200208 }
201- if (gb.TryGetUIArray (' L' , HANGPRINTER_AXES , motorGearTeeth, reply, seen))
209+ if (gb.TryGetUIArray (' L' , numAnchors , motorGearTeeth, reply, seen))
202210 {
203211 error = true ;
204212 return true ;
205213 }
206- if (gb.TryGetUIArray (' H' , HANGPRINTER_AXES , spoolGearTeeth, reply, seen))
214+ if (gb.TryGetUIArray (' H' , numAnchors , spoolGearTeeth, reply, seen))
207215 {
208216 error = true ;
209217 return true ;
210218 }
211- if (gb.TryGetUIArray (' J' , HANGPRINTER_AXES , fullStepsPerMotorRev, reply, seen))
219+ if (gb.TryGetUIArray (' J' , numAnchors , fullStepsPerMotorRev, reply, seen))
212220 {
213221 error = true ;
214222 return true ;
@@ -222,47 +230,47 @@ bool HangprinterKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, const
222230 {
223231 reply.printf (" Q:Buildup fac %.4f" , (double )spoolBuildupFactor);
224232 reply.lcat (" R:Spool r " );
225- for (size_t i = 0 ; i < HANGPRINTER_AXES ; ++i)
233+ for (size_t i = 0 ; i < numAnchors ; ++i)
226234 {
227235 if (i != 0 ) {
228236 reply.cat (" , " );
229237 }
230238 reply.catf (" %.2f" , (double )spoolRadii[i]);
231239 }
232240 reply.lcat (" U:Mech Adv " );
233- for (size_t i = 0 ; i < HANGPRINTER_AXES ; ++i)
241+ for (size_t i = 0 ; i < numAnchors ; ++i)
234242 {
235243 if (i != 0 ) {
236244 reply.cat (" , " );
237245 }
238246 reply.catf (" %d" , (int )mechanicalAdvantage[i]);
239247 }
240248 reply.lcat (" O:Lines/spool " );
241- for (size_t i = 0 ; i < HANGPRINTER_AXES ; ++i)
249+ for (size_t i = 0 ; i < numAnchors ; ++i)
242250 {
243251 if (i != 0 ) {
244252 reply.cat (" , " );
245253 }
246254 reply.catf (" %d" , (int )linesPerSpool[i]);
247255 }
248256 reply.lcat (" L:Motor gear teeth " );
249- for (size_t i = 0 ; i < HANGPRINTER_AXES ; ++i)
257+ for (size_t i = 0 ; i < numAnchors ; ++i)
250258 {
251259 if (i != 0 ) {
252260 reply.cat (" , " );
253261 }
254262 reply.catf (" %d" , (int )motorGearTeeth[i]);
255263 }
256264 reply.lcat (" H:Spool gear teeth " );
257- for (size_t i = 0 ; i < HANGPRINTER_AXES ; ++i)
265+ for (size_t i = 0 ; i < numAnchors ; ++i)
258266 {
259267 if (i != 0 ) {
260268 reply.cat (" , " );
261269 }
262270 reply.catf (" %d" , (int )spoolGearTeeth[i]);
263271 }
264272 reply.lcat (" J:Full steps/rev " );
265- for (size_t i = 0 ; i < HANGPRINTER_AXES ; ++i)
273+ for (size_t i = 0 ; i < numAnchors ; ++i)
266274 {
267275 if (i != 0 ) {
268276 reply.cat (" , " );
@@ -288,19 +296,19 @@ inline float HangprinterKinematics::LineLengthSquared(const float machinePos[3],
288296bool HangprinterKinematics::CartesianToMotorSteps (const float machinePos[], const float stepsPerMm[],
289297 size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const noexcept
290298{
291- float squaredLineLengths[HANGPRINTER_AXES ];
292- for (size_t i = 0 ; i < HANGPRINTER_AXES ; ++i)
299+ float squaredLineLengths[numAnchors ];
300+ for (size_t i = 0 ; i < numAnchors ; ++i)
293301 {
294302 squaredLineLengths[i] = LineLengthSquared (machinePos, anchors[i]);
295303 }
296304
297- float linePos[HANGPRINTER_AXES ];
298- for (size_t i = 0 ; i < HANGPRINTER_AXES ; ++i)
305+ float linePos[numAnchors ];
306+ for (size_t i = 0 ; i < numAnchors ; ++i)
299307 {
300308 linePos[i] = fastSqrtf (squaredLineLengths[i]) - lineLengthsOrigin[i];
301309 }
302310
303- for (size_t i = 0 ; i < HANGPRINTER_AXES ; ++i)
311+ for (size_t i = 0 ; i < numAnchors ; ++i)
304312 {
305313 motorPos[i] = lrintf (k0[i] * (fastSqrtf (spoolRadiiSq[i] + linePos[i] * k2[i]) - spoolRadii[i]));
306314 }
@@ -318,6 +326,7 @@ inline float HangprinterKinematics::MotorPosToLinePos(const int32_t motorPos, si
318326// Assumes lines are tight and anchor location norms are followed
319327void HangprinterKinematics::MotorStepsToCartesian (const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const noexcept
320328{
329+ // TODO Use all anchors when numAnchors > 4, to have less error
321330 ForwardTransform (
322331 MotorPosToLinePos (motorPos[A_AXIS], A_AXIS) + lineLengthsOrigin[A_AXIS],
323332 MotorPosToLinePos (motorPos[B_AXIS], B_AXIS) + lineLengthsOrigin[B_AXIS],
@@ -349,19 +358,19 @@ bool HangprinterKinematics::IsReachable(float axesCoords[MaxAxes], AxesBitmap ax
349358{
350359 float const coords[3 ] = {axesCoords[X_AXIS], axesCoords[Y_AXIS], axesCoords[Z_AXIS]};
351360
352- bool reachable = isSameSide (anchors[0 ], anchors[1 ], anchors[2 ], anchors[HANGPRINTER_AXES - 1 ], coords);
361+ bool reachable = isSameSide (anchors[0 ], anchors[1 ], anchors[2 ], anchors[numAnchors - 1 ], coords);
353362
354- for (size_t i = 1 ; reachable && i < HANGPRINTER_AXES - 3 ; ++i)
363+ for (size_t i = 1 ; reachable && i < numAnchors - 3 ; ++i)
355364 {
356- reachable = isSameSide (anchors[i], anchors[i + 1 ], anchors[HANGPRINTER_AXES - 1 ], anchors[i + 2 ], coords);
365+ reachable = isSameSide (anchors[i], anchors[i + 1 ], anchors[numAnchors - 1 ], anchors[i + 2 ], coords);
357366 }
358367 if (reachable)
359368 {
360- reachable = isSameSide (anchors[HANGPRINTER_AXES - 3 ], anchors[HANGPRINTER_AXES - 2 ], anchors[HANGPRINTER_AXES - 1 ], anchors[0 ], coords);
369+ reachable = isSameSide (anchors[numAnchors - 3 ], anchors[numAnchors - 2 ], anchors[numAnchors - 1 ], anchors[0 ], coords);
361370 }
362371 if (reachable)
363372 {
364- reachable = isSameSide (anchors[HANGPRINTER_AXES - 2 ], anchors[0 ], anchors[HANGPRINTER_AXES - 1 ], anchors[1 ], coords);
373+ reachable = isSameSide (anchors[numAnchors - 2 ], anchors[0 ], anchors[numAnchors - 1 ], anchors[1 ], coords);
365374 }
366375
367376 return reachable;
@@ -443,7 +452,10 @@ AxesBitmap HangprinterKinematics::AxesAssumedHomed(AxesBitmap g92Axes) const noe
443452 // If all of X, Y and Z have been specified then we know the positions of all 4 spool motors, otherwise we don't
444453 if ((g92Axes & XyzAxes) == XyzAxes)
445454 {
446- g92Axes.SetBit (D_AXIS);
455+ for (size_t i = 3 ; i < numAnchors; ++i)
456+ {
457+ g92Axes.SetBit (i);
458+ }
447459 }
448460 else
449461 {
@@ -475,7 +487,7 @@ bool HangprinterKinematics::WriteCalibrationParameters(FileStore *f) const noexc
475487 String<255 > scratchString;
476488 scratchString.printf (" M669 K6 P%.1f " , (double )printRadius);
477489
478- for (size_t i = 0 ; i < HANGPRINTER_AXES ; ++i)
490+ for (size_t i = 0 ; i < numAnchors ; ++i)
479491 {
480492 scratchString.catf (" %c%.3f:%.3f:%.3f " , ANCHOR_CHARS[i], (double )anchors[i][X_AXIS], (double )anchors[i][Y_AXIS], (double )anchors[i][Z_AXIS]);
481493 }
@@ -491,7 +503,7 @@ bool HangprinterKinematics::WriteCalibrationParameters(FileStore *f) const noexc
491503 }
492504
493505 scratchString.printf (" R%.3f" , (double )spoolRadii[0 ]);
494- for (size_t i = 1 ; i < HANGPRINTER_AXES ; ++i)
506+ for (size_t i = 1 ; i < numAnchors ; ++i)
495507 {
496508 scratchString.catf (" :%.3f" , (double )spoolRadii[i]);
497509 }
@@ -501,7 +513,7 @@ bool HangprinterKinematics::WriteCalibrationParameters(FileStore *f) const noexc
501513 }
502514
503515 scratchString.printf (" U%.3f" , (double )mechanicalAdvantage[0 ]);
504- for (size_t i = 1 ; i < HANGPRINTER_AXES ; ++i)
516+ for (size_t i = 1 ; i < numAnchors ; ++i)
505517 {
506518 scratchString.catf (" :%.3f" , (double )mechanicalAdvantage[i]);
507519 }
@@ -511,7 +523,7 @@ bool HangprinterKinematics::WriteCalibrationParameters(FileStore *f) const noexc
511523 }
512524
513525 scratchString.printf (" O%.3f" , (double )linesPerSpool[0 ]);
514- for (size_t i = 1 ; i < HANGPRINTER_AXES ; ++i)
526+ for (size_t i = 1 ; i < numAnchors ; ++i)
515527 {
516528 scratchString.catf (" :%.3f" , (double )linesPerSpool[i]);
517529 }
@@ -521,7 +533,7 @@ bool HangprinterKinematics::WriteCalibrationParameters(FileStore *f) const noexc
521533 }
522534
523535 scratchString.printf (" L%.3f" , (double )motorGearTeeth[0 ]);
524- for (size_t i = 1 ; i < HANGPRINTER_AXES ; ++i)
536+ for (size_t i = 1 ; i < numAnchors ; ++i)
525537 {
526538 scratchString.catf (" :%.3f" , (double )motorGearTeeth[i]);
527539 }
@@ -531,7 +543,7 @@ bool HangprinterKinematics::WriteCalibrationParameters(FileStore *f) const noexc
531543 }
532544
533545 scratchString.printf (" H%.3f" , (double )spoolGearTeeth[0 ]);
534- for (size_t i = 1 ; i < HANGPRINTER_AXES ; ++i)
546+ for (size_t i = 1 ; i < numAnchors ; ++i)
535547 {
536548 scratchString.catf (" :%.3f" , (double )spoolGearTeeth[i]);
537549 }
@@ -541,7 +553,7 @@ bool HangprinterKinematics::WriteCalibrationParameters(FileStore *f) const noexc
541553 }
542554
543555 scratchString.printf (" J%.3f" , (double )fullStepsPerMotorRev[0 ]);
544- for (size_t i = 1 ; i < HANGPRINTER_AXES ; ++i)
556+ for (size_t i = 1 ; i < numAnchors ; ++i)
545557 {
546558 scratchString.catf (" :%.3f" , (double )fullStepsPerMotorRev[i]);
547559 }
@@ -649,7 +661,7 @@ void HangprinterKinematics::ForwardTransform(float const a, float const b, float
649661void HangprinterKinematics::PrintParameters (const StringRef& reply) const noexcept
650662{
651663 reply.printf (" Anchor coordinates" );
652- for (size_t i = 0 ; i < HANGPRINTER_AXES ; ++i)
664+ for (size_t i = 0 ; i < numAnchors ; ++i)
653665 {
654666 reply.catf (" (%.2f,%.2f,%.2f)" , (double )anchors[i][X_AXIS], (double )anchors[i][Y_AXIS], (double )anchors[i][Z_AXIS]);
655667 }
@@ -660,8 +672,8 @@ void HangprinterKinematics::PrintParameters(const StringRef& reply) const noexce
660672HangprinterKinematics::ODriveAnswer HangprinterKinematics::GetODrive3EncoderEstimate (DriverId const driver, bool const makeReference, const StringRef& reply, bool const subtractReference) THROWS(GCodeException)
661673{
662674 const uint8_t cmd = CANSimple::MSG_GET_ENCODER_ESTIMATES;
663- static CanAddress seenDrives[HANGPRINTER_AXES ] = { 0 , 0 , 0 , 0 };
664- static float referencePositions[HANGPRINTER_AXES ] = { 0.0 , 0.0 , 0.0 , 0.0 };
675+ static CanAddress seenDrives[HANGPRINTER_MAX_AXES ] = { 0 , 0 , 0 , 0 , 0 };
676+ static float referencePositions[HANGPRINTER_MAX_AXES ] = { 0.0 , 0.0 , 0.0 , 0.0 , 0.0 };
665677 static size_t numSeenDrives = 0 ;
666678 size_t thisDriveIdx = 0 ;
667679
@@ -672,15 +684,15 @@ HangprinterKinematics::ODriveAnswer HangprinterKinematics::GetODrive3EncoderEsti
672684 bool const newOne = (thisDriveIdx == numSeenDrives);
673685 if (newOne)
674686 {
675- if (numSeenDrives < HANGPRINTER_AXES )
687+ if (numSeenDrives < numAnchors )
676688 {
677689 seenDrives[thisDriveIdx] = driver.boardAddress ;
678690 numSeenDrives++;
679691 }
680692 else // we don't have space for a new one
681693 {
682- reply.printf (" Max CAN addresses we can reference is %d. Can't reference board %d." , HANGPRINTER_AXES , driver.boardAddress );
683- numSeenDrives = HANGPRINTER_AXES ;
694+ reply.printf (" Max CAN addresses we can reference is %d. Can't reference board %d." , numAnchors , driver.boardAddress );
695+ numSeenDrives = numAnchors ;
684696 return {};
685697 }
686698 }
0 commit comments