diff --git a/Marlin/planner_bezier.cpp b/Marlin/planner_bezier.cpp index 5e820c4040..b72478d75e 100644 --- a/Marlin/planner_bezier.cpp +++ b/Marlin/planner_bezier.cpp @@ -113,9 +113,9 @@ void cubic_b_spline(const float position[NUM_AXIS], const float target[NUM_AXIS] float second1 = target[Y_AXIS] + offset[3]; float t = 0.0; - float tmp[4]; - tmp[X_AXIS] = position[X_AXIS]; - tmp[Y_AXIS] = position[Y_AXIS]; + float bez_target[4]; + bez_target[X_AXIS] = position[X_AXIS]; + bez_target[Y_AXIS] = position[Y_AXIS]; float step = MAX_STEP; uint8_t idle_counter = 0; @@ -141,8 +141,8 @@ void cubic_b_spline(const float position[NUM_AXIS], const float target[NUM_AXIS] float candidate_t = 0.5 * (t + new_t); float candidate_pos0 = eval_bezier(position[X_AXIS], first0, second0, target[X_AXIS], candidate_t); float candidate_pos1 = eval_bezier(position[Y_AXIS], first1, second1, target[Y_AXIS], candidate_t); - float interp_pos0 = 0.5 * (tmp[X_AXIS] + new_pos0); - float interp_pos1 = 0.5 * (tmp[Y_AXIS] + new_pos1); + float interp_pos0 = 0.5 * (bez_target[X_AXIS] + new_pos0); + float interp_pos1 = 0.5 * (bez_target[Y_AXIS] + new_pos1); if (dist1(candidate_pos0, candidate_pos1, interp_pos0, interp_pos1) <= (SIGMA)) break; new_t = candidate_t; new_pos0 = candidate_pos0; @@ -157,8 +157,8 @@ void cubic_b_spline(const float position[NUM_AXIS], const float target[NUM_AXIS] if (candidate_t >= 1.0) break; float candidate_pos0 = eval_bezier(position[X_AXIS], first0, second0, target[X_AXIS], candidate_t); float candidate_pos1 = eval_bezier(position[Y_AXIS], first1, second1, target[Y_AXIS], candidate_t); - float interp_pos0 = 0.5 * (tmp[X_AXIS] + candidate_pos0); - float interp_pos1 = 0.5 * (tmp[Y_AXIS] + candidate_pos1); + float interp_pos0 = 0.5 * (bez_target[X_AXIS] + candidate_pos0); + float interp_pos1 = 0.5 * (bez_target[Y_AXIS] + candidate_pos1); if (dist1(new_pos0, new_pos1, interp_pos0, interp_pos1) > (SIGMA)) break; new_t = candidate_t; new_pos0 = candidate_pos0; @@ -180,14 +180,23 @@ void cubic_b_spline(const float position[NUM_AXIS], const float target[NUM_AXIS] t = new_t; // Compute and send new position - tmp[X_AXIS] = new_pos0; - tmp[Y_AXIS] = new_pos1; + bez_target[X_AXIS] = new_pos0; + bez_target[Y_AXIS] = new_pos1; // FIXME. The following two are wrong, since the parameter t is // not linear in the distance. - tmp[Z_AXIS] = interp(position[Z_AXIS], target[Z_AXIS], t); - tmp[E_AXIS] = interp(position[E_AXIS], target[E_AXIS], t); - clamp_to_software_endstops(tmp); - planner.buffer_line(tmp[X_AXIS], tmp[Y_AXIS], tmp[Z_AXIS], tmp[E_AXIS], feed_rate, extruder); + bez_target[Z_AXIS] = interp(position[Z_AXIS], target[Z_AXIS], t); + bez_target[E_AXIS] = interp(position[E_AXIS], target[E_AXIS], t); + clamp_to_software_endstops(bez_target); + + #if ENABLED(DELTA) || ENABLED(SCARA) + calculate_delta(bez_target); + #if ENABLED(AUTO_BED_LEVELING_FEATURE) + adjust_delta(bez_target); + #endif + planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], bez_target[E_AXIS], feed_rate, extruder); + #else + planner.buffer_line(bez_target[X_AXIS], bez_target[Y_AXIS], bez_target[Z_AXIS], bez_target[E_AXIS], feed_rate, extruder); + #endif } }