Merge pull request #292 from Paciente8159/motion-control-opt

minor optimization to motion control
Paciente8159 · Oct 5, 2022 · 51d3f7d · 51d3f7d
2 parents 54f81e5 + cc7e7c3
commit 51d3f7d
Showing 1 changed file with 40 additions and 56 deletions.
diff --git a/uCNC/src/core/motion_control.c b/uCNC/src/core/motion_control.c
@@ -52,13 +52,8 @@ bool mc_toogle_checkmode(void)
 	return mc_checkmode;
 }
 
-static uint8_t mc_line_segment(float *target, motion_data_t *block_data)
+static uint8_t mc_line_segment(int32_t *step_new_pos, motion_data_t *block_data)
 {
-	int32_t step_new_pos[STEPPER_COUNT];
-
-	// applies the inverse kinematic to get next position in steps
-	kinematics_apply_inverse(target, step_new_pos);
-
 // resets accumulator vars of the block
 #ifdef ENABLE_LINACT_PLANNER
 	block_data->full_steps = 0;
@@ -140,10 +135,6 @@ static uint8_t mc_line_segment(float *target, motion_data_t *block_data)
 				}
 			}
 
-			planner_add_line(&backlash_block_data);
-			// dwell should only execute on the first request
-			block_data->dwell = 0;
-
 			while (planner_buffer_is_full())
 			{
 				if (!cnc_dotasks())
@@ -152,10 +143,21 @@ static uint8_t mc_line_segment(float *target, motion_data_t *block_data)
 				}
 			}
 
+			planner_add_line(&backlash_block_data);
+			// dwell should only execute on the first request
+			block_data->dwell = 0;
 			mc_last_dirbits = block_data->dirbits;
 		}
 #endif
 
+		while (planner_buffer_is_full())
+		{
+			if (!cnc_dotasks())
+			{
+				return STATUS_CRITICAL_FAIL;
+			}
+		}
+
 		planner_add_line(block_data);
 		// dwell should only execute on the first request
 		block_data->dwell = 0;
@@ -176,7 +178,7 @@ uint8_t mc_line(float *target, motion_data_t *block_data)
 	float prev_target[AXIS_COUNT];
 	block_data->dirbits = 0; // reset dirbits (this prevents odd behaviour generated by long arcs)
 
-	// In homming mode no kinematics modifications is applied to prevent unwanted axis movements
+	// In homing mode no kinematics modifications is applied to prevent unwanted axis movements
 	if (!cnc_get_exec_state(EXEC_HOMING))
 	{
 		kinematics_apply_transform(target);
@@ -193,14 +195,6 @@ uint8_t mc_line(float *target, motion_data_t *block_data)
 		return STATUS_OK;
 	}
 
-	while (planner_buffer_is_full())
-	{
-		if (!cnc_dotasks())
-		{
-			return STATUS_CRITICAL_FAIL;
-		}
-	}
-
 	uint8_t error = STATUS_OK;
 	int32_t step_new_pos[STEPPER_COUNT];
 	// converts transformed target to stepper position
@@ -270,9 +264,18 @@ uint8_t mc_line(float *target, motion_data_t *block_data)
 	float inv_delta = (!CHECKFLAG(block_data->motion_mode, MOTIONCONTROL_MODE_INVERSEFEED) ? (block_data->feed * inv_dist) : block_data->feed);
 	block_data->feed = (float)max_steps * inv_delta;
 
+#if ((KINEMATIC == KINEMATIC_DELTA) || defined(BRESENHAM_16BIT))
 	// this contains a motion. Any tool update will be done here
 	uint32_t line_segments = 1;
-#if (KINEMATIC != KINEMATIC_DELTA)
+#if (KINEMATIC == KINEMATIC_DELTA)
+	line_segments = (uint32_t)ceilf(line_dist * DELTA_MOTION_SEGMENT_FACTOR);
+	float m_inv = 1.0f / (float)line_segments;
+	for (uint8_t i = AXIS_COUNT; i != 0;)
+	{
+		i--;
+		motion_segment[i] *= m_inv;
+	}
+#else
 	if (max_steps > MAX_STEPS_PER_LINE)
 	{
 		line_segments += (max_steps >> MAX_STEPS_PER_LINE_BITS);
@@ -283,52 +286,33 @@ uint8_t mc_line(float *target, motion_data_t *block_data)
 			motion_segment[i] *= m_inv;
 		}
 	}
-#else
-	line_segments = (uint32_t)ceilf(line_dist * DELTA_MOTION_SEGMENT_FACTOR);
-	float m_inv = 1.0f / (float)line_segments;
-	for (uint8_t i = AXIS_COUNT; i != 0;)
-	{
-		i--;
-		motion_segment[i] *= m_inv;
-	}
 #endif
 
-	while (line_segments--)
+	while (--line_segments)
 	{
-		while (planner_buffer_is_full())
+		block_data->motion_flags.bit.is_subsegment = 1;
+		for (uint8_t i = AXIS_COUNT; i != 0;)
 		{
-			if (!cnc_dotasks())
-			{
-				block_data->feed = feed;
-				return STATUS_CRITICAL_FAIL;
-			}
+			i--;
+			prev_target[i] += motion_segment[i];
 		}
 
-		if (line_segments)
-		{
-			for (uint8_t i = AXIS_COUNT; i != 0;)
-			{
-				i--;
-				prev_target[i] += motion_segment[i];
-			}
-
-			error = mc_line_segment(prev_target, block_data);
-			if (error)
-			{
-				break;
-			}
-		}
-		else
+		kinematics_apply_inverse(prev_target, step_new_pos);
+		error = mc_line_segment(step_new_pos, block_data);
+		if (error)
 		{
-			error = mc_line_segment(target, block_data);
-			if (error)
-			{
-				break;
-			}
+			memcpy(target, prev_target, sizeof(prev_target));
+			block_data->feed = feed;
+			return error;
 		}
-		block_data->motion_flags.bit.is_subsegment = 1;
 	}
 
+	if (block_data->motion_flags.bit.is_subsegment)
+	{
+		kinematics_apply_inverse(target, step_new_pos);
+	}
+#endif
+	error = mc_line_segment(step_new_pos, block_data);
 	// stores the new position for the next motion
 	memcpy(mc_last_target, target, sizeof(mc_last_target));
 	block_data->feed = feed;