Blame | Last modification | View Log | RSS feed
/*** Marlin 3D Printer Firmware* Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]** Based on Sprinter and grbl.* Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm** This program is free software: you can redistribute it and/or modify* it under the terms of the GNU General Public License as published by* the Free Software Foundation, either version 3 of the License, or* (at your option) any later version.** This program is distributed in the hope that it will be useful,* but WITHOUT ANY WARRANTY; without even the implied warranty of* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the* GNU General Public License for more details.** You should have received a copy of the GNU General Public License* along with this program. If not, see <http://www.gnu.org/licenses/>.**//*** planner_bezier.cpp** Compute and buffer movement commands for bezier curves**/#include "MarlinConfig.h"#if ENABLED(BEZIER_CURVE_SUPPORT)#include "planner.h"#include "language.h"#include "temperature.h"#include "Marlin.h"// See the meaning in the documentation of cubic_b_spline().#define MIN_STEP 0.002f#define MAX_STEP 0.1f#define SIGMA 0.1f// Compute the linear interpolation between two real numbers.inline static float interp(float a, float b, float t) { return (1.0f - t) * a + t * b; }/*** Compute a Bézier curve using the De Casteljau's algorithm (see* https://en.wikipedia.org/wiki/De_Casteljau%27s_algorithm), which is* easy to code and has good numerical stability (very important,* since Arudino works with limited precision real numbers).*/inline static float eval_bezier(float a, float b, float c, float d, float t) {float iab = interp(a, b, t);float ibc = interp(b, c, t);float icd = interp(c, d, t);float iabc = interp(iab, ibc, t);float ibcd = interp(ibc, icd, t);float iabcd = interp(iabc, ibcd, t);return iabcd;}/*** We approximate Euclidean distance with the sum of the coordinates* offset (so-called "norm 1"), which is quicker to compute.*/inline static float dist1(float x1, float y1, float x2, float y2) { return ABS(x1 - x2) + ABS(y1 - y2); }/*** The algorithm for computing the step is loosely based on the one in Kig* (See https://sources.debian.net/src/kig/4:15.08.3-1/misc/kigpainter.cpp/#L759)* However, we do not use the stack.** The algorithm goes as it follows: the parameters t runs from 0.0 to* 1.0 describing the curve, which is evaluated by eval_bezier(). At* each iteration we have to choose a step, i.e., the increment of the* t variable. By default the step of the previous iteration is taken,* and then it is enlarged or reduced depending on how straight the* curve locally is. The step is always clamped between MIN_STEP/2 and* 2*MAX_STEP. MAX_STEP is taken at the first iteration.** For some t, the step value is considered acceptable if the curve in* the interval [t, t+step] is sufficiently straight, i.e.,* sufficiently close to linear interpolation. In practice the* following test is performed: the distance between eval_bezier(...,* t+step/2) is evaluated and compared with 0.5*(eval_bezier(...,* t)+eval_bezier(..., t+step)). If it is smaller than SIGMA, then the* step value is considered acceptable, otherwise it is not. The code* seeks to find the larger step value which is considered acceptable.** At every iteration the recorded step value is considered and then* iteratively halved until it becomes acceptable. If it was already* acceptable in the beginning (i.e., no halving were done), then* maybe it was necessary to enlarge it; then it is iteratively* doubled while it remains acceptable. The last acceptable value* found is taken, provided that it is between MIN_STEP and MAX_STEP* and does not bring t over 1.0.** Caveat: this algorithm is not perfect, since it can happen that a* step is considered acceptable even when the curve is not linear at* all in the interval [t, t+step] (but its mid point coincides "by* chance" with the midpoint according to the parametrization). This* kind of glitches can be eliminated with proper first derivative* estimates; however, given the improbability of such configurations,* the mitigation offered by MIN_STEP and the small computational* power available on Arduino, I think it is not wise to implement it.*/void cubic_b_spline(const float pos[XYZE], const float cart_target[XYZE], const float offset[4], float fr_mm_s, uint8_t extruder) {// Absolute first and second control points are recovered.const float first0 = pos[X_AXIS] + offset[0],first1 = pos[Y_AXIS] + offset[1],second0 = cart_target[X_AXIS] + offset[2],second1 = cart_target[Y_AXIS] + offset[3];float t = 0;float bez_target[XYZE];bez_target[X_AXIS] = pos[X_AXIS];bez_target[Y_AXIS] = pos[Y_AXIS];float step = MAX_STEP;millis_t next_idle_ms = millis() + 200UL;while (t < 1) {thermalManager.manage_heater();millis_t now = millis();if (ELAPSED(now, next_idle_ms)) {next_idle_ms = now + 200UL;idle();}// First try to reduce the step in order to make it sufficiently// close to a linear interpolation.bool did_reduce = false;float new_t = t + step;NOMORE(new_t, 1);float new_pos0 = eval_bezier(pos[X_AXIS], first0, second0, cart_target[X_AXIS], new_t),new_pos1 = eval_bezier(pos[Y_AXIS], first1, second1, cart_target[Y_AXIS], new_t);for (;;) {if (new_t - t < (MIN_STEP)) break;const float candidate_t = 0.5f * (t + new_t),candidate_pos0 = eval_bezier(pos[X_AXIS], first0, second0, cart_target[X_AXIS], candidate_t),candidate_pos1 = eval_bezier(pos[Y_AXIS], first1, second1, cart_target[Y_AXIS], candidate_t),interp_pos0 = 0.5f * (bez_target[X_AXIS] + new_pos0),interp_pos1 = 0.5f * (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;new_pos1 = candidate_pos1;did_reduce = true;}// If we did not reduce the step, maybe we should enlarge it.if (!did_reduce) for (;;) {if (new_t - t > MAX_STEP) break;const float candidate_t = t + 2 * (new_t - t);if (candidate_t >= 1) break;const float candidate_pos0 = eval_bezier(pos[X_AXIS], first0, second0, cart_target[X_AXIS], candidate_t),candidate_pos1 = eval_bezier(pos[Y_AXIS], first1, second1, cart_target[Y_AXIS], candidate_t),interp_pos0 = 0.5f * (bez_target[X_AXIS] + candidate_pos0),interp_pos1 = 0.5f * (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;new_pos1 = candidate_pos1;}// Check some postcondition; they are disabled in the actual// Marlin build, but if you test the same code on a computer you// may want to check they are respect./*assert(new_t <= 1.0);if (new_t < 1.0) {assert(new_t - t >= (MIN_STEP) / 2.0);assert(new_t - t <= (MAX_STEP) * 2.0);}*/step = new_t - t;t = new_t;// Compute and send new positionbez_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.bez_target[Z_AXIS] = interp(pos[Z_AXIS], cart_target[Z_AXIS], t);bez_target[E_CART] = interp(pos[E_CART], cart_target[E_CART], t);clamp_to_software_endstops(bez_target);#if HAS_UBL_AND_CURVESfloat bez_copy[XYZ] = { bez_target[X_AXIS], bez_target[Y_AXIS], bez_target[Z_AXIS] };planner.apply_leveling(bez_copy);if (!planner.buffer_segment(bez_copy[X_AXIS], bez_copy[Y_AXIS], bez_copy[Z_AXIS], bez_target[E_CART], fr_mm_s, active_extruder))break;#elseif (!planner.buffer_line_kinematic(bez_target, fr_mm_s, extruder))break;#endif}}#endif // BEZIER_CURVE_SUPPORT