Skip to content

Commit 327e943

Browse files
committed
Hangprinter: Allow Hangprinter to be configured with 5 anchors
This allows to print square pyramids instead of tetrahedrons, at the cost of an extra motor.
1 parent a54cbac commit 327e943

File tree

2 files changed

+71
-58
lines changed

2 files changed

+71
-58
lines changed

src/Movement/Kinematics/HangprinterKinematics.cpp

Lines changed: 63 additions & 51 deletions
Original file line numberDiff line numberDiff line change
@@ -17,12 +17,14 @@
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}};
2628
constexpr float DefaultPrintRadius = 1500.0;
2729

2830
#if SUPPORT_OBJECT_MODEL
@@ -44,7 +46,7 @@ constexpr ObjectModelArrayDescriptor HangprinterKinematics::anchorCoordinatesArr
4446
constexpr 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],
288296
bool 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
319327
void 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
649661
void 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
660672
HangprinterKinematics::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
}

src/Movement/Kinematics/HangprinterKinematics.h

Lines changed: 8 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -52,8 +52,8 @@ class HangprinterKinematics : public RoundBedKinematics
5252

5353
private:
5454
// Basic facts about movement system
55-
const char* ANCHOR_CHARS = "ABCD";
56-
static constexpr size_t HANGPRINTER_AXES = 4;
55+
const char* ANCHOR_CHARS = "ABCDE";
56+
static constexpr size_t HANGPRINTER_MAX_AXES = 5;
5757
static constexpr size_t A_AXIS = 0;
5858
static constexpr size_t B_AXIS = 1;
5959
static constexpr size_t C_AXIS = 2;
@@ -67,16 +67,17 @@ class HangprinterKinematics : public RoundBedKinematics
6767

6868
void PrintParameters(const StringRef& reply) const noexcept; // Print all the parameters for debugging
6969

70-
float anchors[HANGPRINTER_AXES][3]; // XYZ coordinates of the anchors
70+
size_t numAnchors;
71+
float anchors[HANGPRINTER_MAX_AXES][3]; // XYZ coordinates of the anchors
7172
float printRadius;
7273
// Line buildup compensation
7374
float spoolBuildupFactor;
74-
float spoolRadii[HANGPRINTER_AXES];
75-
uint32_t mechanicalAdvantage[HANGPRINTER_AXES], linesPerSpool[HANGPRINTER_AXES];
76-
uint32_t motorGearTeeth[HANGPRINTER_AXES], spoolGearTeeth[HANGPRINTER_AXES], fullStepsPerMotorRev[HANGPRINTER_AXES];
75+
float spoolRadii[HANGPRINTER_MAX_AXES];
76+
uint32_t mechanicalAdvantage[HANGPRINTER_MAX_AXES], linesPerSpool[HANGPRINTER_MAX_AXES];
77+
uint32_t motorGearTeeth[HANGPRINTER_MAX_AXES], spoolGearTeeth[HANGPRINTER_MAX_AXES], fullStepsPerMotorRev[HANGPRINTER_MAX_AXES];
7778

7879
// Derived parameters
79-
float k0[HANGPRINTER_AXES], spoolRadiiSq[HANGPRINTER_AXES], k2[HANGPRINTER_AXES], lineLengthsOrigin[HANGPRINTER_AXES];
80+
float k0[HANGPRINTER_MAX_AXES], spoolRadiiSq[HANGPRINTER_MAX_AXES], k2[HANGPRINTER_MAX_AXES], lineLengthsOrigin[HANGPRINTER_MAX_AXES];
8081
float printRadiusSquared;
8182

8283
#if DUAL_CAN

0 commit comments

Comments
 (0)