Hangprinter: Allow Hangprinter to be configured with 6 anchors

Author: jtimon

src/Movement/Kinematics/HangprinterKinematics.cpp

@@ -20,10 +20,11 @@
 constexpr size_t DefaultNumAnchors = 4;
 // Default anchor coordinates
 // These are only placeholders. Each machine must have these values calibrated in order to work correctly.
-constexpr float DefaultAnchors[5][3] = {{    0.0, -2000.0, -100.0},
+constexpr float DefaultAnchors[6][3] = {{    0.0, -2000.0, -100.0},
                                         { 2000.0,  1000.0, -100.0},
                                         {-2000.0,  1000.0, -100.0},
                                         {    0.0,     0.0, 3000.0},
+                                        {    0.0,     0.0,    0.0},
                                         {    0.0,     0.0,    0.0}};
 constexpr float DefaultPrintRadius = 1500.0;
 
@@ -83,13 +84,13 @@ void HangprinterKinematics::Init() noexcept
 	 * In practice you might want to compensate a bit more or a bit less */
 	constexpr float DefaultSpoolBuildupFactor = 0.007;
 	/* Measure and set spool radii with M669 to achieve better accuracy */
-	constexpr float DefaultSpoolRadii[HANGPRINTER_MAX_AXES] = { 75.0, 75.0, 75.0, 75.0, 75.0}; // HP4 default
+	constexpr float DefaultSpoolRadii[HANGPRINTER_MAX_AXES] = { 75.0, 75.0, 75.0, 75.0, 75.0, 75.0}; // HP4 default
 	/* If axis runs lines back through pulley system, set mechanical advantage accordingly with M669 */
-	constexpr uint32_t DefaultMechanicalAdvantage[HANGPRINTER_MAX_AXES] = { 2, 2, 2, 2, 4}; // HP4 default
-	constexpr uint32_t DefaultLinesPerSpool[HANGPRINTER_MAX_AXES] = { 1, 1, 1, 1, 1}; // HP4 default
-	constexpr uint32_t DefaultMotorGearTeeth[HANGPRINTER_MAX_AXES] = {  20,  20,  20,  20,  20}; // HP4 default
-	constexpr uint32_t DefaultSpoolGearTeeth[HANGPRINTER_MAX_AXES] = { 255, 255, 255, 255, 255}; // HP4 default
-	constexpr uint32_t DefaultFullStepsPerMotorRev[HANGPRINTER_MAX_AXES] = { 25, 25, 25, 25, 25};
+	constexpr uint32_t DefaultMechanicalAdvantage[HANGPRINTER_MAX_AXES] = { 2, 2, 2, 2, 2, 4}; // HP4 default
+	constexpr uint32_t DefaultLinesPerSpool[HANGPRINTER_MAX_AXES] = { 1, 1, 1, 1, 1, 1}; // HP4 default
+	constexpr uint32_t DefaultMotorGearTeeth[HANGPRINTER_MAX_AXES] = {  20,  20,  20,  20,  20,  20}; // HP4 default
+	constexpr uint32_t DefaultSpoolGearTeeth[HANGPRINTER_MAX_AXES] = { 255, 255, 255, 255, 255, 255}; // HP4 default
+	constexpr uint32_t DefaultFullStepsPerMotorRev[HANGPRINTER_MAX_AXES] = { 25, 25, 25, 25, 25, 25};
 	ARRAY_INIT(anchors, DefaultAnchors);
 	numAnchors = DefaultNumAnchors;
 	printRadius = DefaultPrintRadius;
@@ -187,6 +188,14 @@ bool HangprinterKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, const
 				return true;
 			}
 		}
+		if (numAnchors > 5)
+		{
+			if (gb.TryGetFloatArray('F', 3, anchors[F_AXIS], reply, seen))
+			{
+				error = true;
+				return true;
+			}
+		}
 
 		if (gb.Seen('P'))
 		{
@@ -218,6 +227,11 @@ bool HangprinterKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, const
         			reply.lcatf("E:%.2f, %.2f, %.2f\n",
         				(double)anchors[E_AXIS][X_AXIS], (double)anchors[E_AXIS][Y_AXIS], (double)anchors[E_AXIS][Z_AXIS]);
 			}
+			if (numAnchors > 5)
+			{
+        			reply.lcatf("F:%.2f, %.2f, %.2f\n",
+        				(double)anchors[F_AXIS][X_AXIS], (double)anchors[F_AXIS][Y_AXIS], (double)anchors[F_AXIS][Z_AXIS]);
+			}
 		}
 	}
 	else if (mCode == 666)
@@ -533,6 +547,16 @@ bool HangprinterKinematics::WriteCalibrationParameters(FileStore *f) const noexc
         	}
 	}
 
+	if (numAnchors > 5)
+	{
+        	scratchString.printf(" F%.3f:%.3f:%.3f",
+        			     (double)anchors[F_AXIS][X_AXIS], (double)anchors[F_AXIS][Y_AXIS], (double)anchors[F_AXIS][Z_AXIS]);
+        	if (!f->Write(scratchString.c_str()))
+        	{
+        		return false;
+        	}
+	}
+
 	scratchString.printf(" P%.1f\n", (double)printRadius);
 	if (!f->Write(scratchString.c_str()))
 	{
@@ -749,8 +773,8 @@ void HangprinterKinematics::PrintParameters(const StringRef& reply) const noexce
 HangprinterKinematics::ODriveAnswer HangprinterKinematics::GetODrive3EncoderEstimate(DriverId const driver, bool const makeReference, const StringRef& reply, bool const subtractReference) THROWS(GCodeException)
 {
 	const uint8_t cmd = CANSimple::MSG_GET_ENCODER_ESTIMATES;
-	static CanAddress seenDrives[HANGPRINTER_MAX_AXES] = { 0, 0, 0, 0, 0 };
-	static float referencePositions[HANGPRINTER_MAX_AXES] = { 0.0, 0.0, 0.0, 0.0, 0.0 };
+	static CanAddress seenDrives[HANGPRINTER_MAX_AXES] = { 0, 0, 0, 0, 0, 0 };
+	static float referencePositions[HANGPRINTER_MAX_AXES] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
 	static size_t numSeenDrives = 0;
 	size_t thisDriveIdx = 0;
 

src/Movement/Kinematics/HangprinterKinematics.h

@@ -52,12 +52,13 @@ class HangprinterKinematics : public RoundBedKinematics
 
 private:
 	// Basic facts about movement system
-	static constexpr size_t HANGPRINTER_MAX_AXES = 5;
+	static constexpr size_t HANGPRINTER_MAX_AXES = 6;
 	static constexpr size_t A_AXIS = 0;
 	static constexpr size_t B_AXIS = 1;
 	static constexpr size_t C_AXIS = 2;
 	static constexpr size_t D_AXIS = 3;
 	static constexpr size_t E_AXIS = 4;
+	static constexpr size_t F_AXIS = 5;
 
 	void Init() noexcept;
 	void Recalc() noexcept;