Merge pull request #3744 from thinkyhead/rc_bezier_curves
Add BEZIER_CURVE_SUPPORT — G5 command
This commit is contained in:
commit
3016dfe484
|
@ -194,6 +194,12 @@ script:
|
|||
- opt_enable ULTIMAKERCONTROLLER FILAMENT_LCD_DISPLAY
|
||||
- build_marlin
|
||||
#
|
||||
# Enable BEZIER_CURVE_SUPPORT
|
||||
#
|
||||
- restore_configs
|
||||
- opt_enable_adv BEZIER_CURVE_SUPPORT
|
||||
- build_marlin
|
||||
#
|
||||
# Enable COREXY
|
||||
#
|
||||
- restore_configs
|
||||
|
|
|
@ -284,8 +284,6 @@
|
|||
|
||||
#define AXIS_RELATIVE_MODES {false, false, false, false}
|
||||
|
||||
// @section machine
|
||||
|
||||
//By default pololu step drivers require an active high signal. However, some high power drivers require an active low signal as step.
|
||||
#define INVERT_X_STEP_PIN false
|
||||
#define INVERT_Y_STEP_PIN false
|
||||
|
@ -459,6 +457,9 @@
|
|||
#define MM_PER_ARC_SEGMENT 1
|
||||
#define N_ARC_CORRECTION 25
|
||||
|
||||
// Support for G5 with XYZE destination and IJPQ offsets
|
||||
//#define BEZIER_CURVE_SUPPORT
|
||||
|
||||
const unsigned int dropsegments = 5; //everything with less than this number of steps will be ignored as move and joined with the next movement
|
||||
|
||||
// @section temperature
|
||||
|
|
|
@ -250,7 +250,6 @@ bool enqueue_and_echo_command(const char* cmd, bool say_ok=false); //put a singl
|
|||
void enqueue_and_echo_command_now(const char* cmd); // enqueue now, only return when the command has been enqueued
|
||||
void enqueue_and_echo_commands_P(const char* cmd); //put one or many ASCII commands at the end of the current buffer, read from flash
|
||||
|
||||
void prepare_arc_move(char isclockwise);
|
||||
void clamp_to_software_endstops(float target[3]);
|
||||
|
||||
extern millis_t previous_cmd_ms;
|
||||
|
|
|
@ -45,6 +45,10 @@
|
|||
#include "mesh_bed_leveling.h"
|
||||
#endif
|
||||
|
||||
#if ENABLED(BEZIER_CURVE_SUPPORT)
|
||||
#include "planner_bezier.h"
|
||||
#endif
|
||||
|
||||
#include "ultralcd.h"
|
||||
#include "planner.h"
|
||||
#include "stepper.h"
|
||||
|
@ -102,6 +106,7 @@
|
|||
* G2 - CW ARC
|
||||
* G3 - CCW ARC
|
||||
* G4 - Dwell S<seconds> or P<milliseconds>
|
||||
* G5 - Cubic B-spline with
|
||||
* G10 - retract filament according to settings of M207
|
||||
* G11 - retract recover filament according to settings of M208
|
||||
* G28 - Home one or more axes
|
||||
|
@ -510,6 +515,10 @@ void process_next_command();
|
|||
void plan_arc(float target[NUM_AXIS], float* offset, uint8_t clockwise);
|
||||
#endif
|
||||
|
||||
#if ENABLED(BEZIER_CURVE_SUPPORT)
|
||||
void plan_cubic_move(const float offset[4]);
|
||||
#endif
|
||||
|
||||
void serial_echopair_P(const char* s_P, int v) { serialprintPGM(s_P); SERIAL_ECHO(v); }
|
||||
void serial_echopair_P(const char* s_P, long v) { serialprintPGM(s_P); SERIAL_ECHO(v); }
|
||||
void serial_echopair_P(const char* s_P, float v) { serialprintPGM(s_P); SERIAL_ECHO(v); }
|
||||
|
@ -2510,6 +2519,43 @@ inline void gcode_G4() {
|
|||
while (PENDING(millis(), codenum)) idle();
|
||||
}
|
||||
|
||||
#if ENABLED(BEZIER_CURVE_SUPPORT)
|
||||
|
||||
/**
|
||||
* Parameters interpreted according to:
|
||||
* http://linuxcnc.org/docs/2.6/html/gcode/gcode.html#sec:G5-Cubic-Spline
|
||||
* However I, J omission is not supported at this point; all
|
||||
* parameters can be omitted and default to zero.
|
||||
*/
|
||||
|
||||
/**
|
||||
* G5: Cubic B-spline
|
||||
*/
|
||||
inline void gcode_G5() {
|
||||
if (IsRunning()) {
|
||||
|
||||
#ifdef SF_ARC_FIX
|
||||
bool relative_mode_backup = relative_mode;
|
||||
relative_mode = true;
|
||||
#endif
|
||||
gcode_get_destination();
|
||||
#ifdef SF_ARC_FIX
|
||||
relative_mode = relative_mode_backup;
|
||||
#endif
|
||||
|
||||
float offset[] = {
|
||||
code_seen('I') ? code_value() : 0.0,
|
||||
code_seen('J') ? code_value() : 0.0,
|
||||
code_seen('P') ? code_value() : 0.0,
|
||||
code_seen('Q') ? code_value() : 0.0
|
||||
};
|
||||
|
||||
plan_cubic_move(offset);
|
||||
}
|
||||
}
|
||||
|
||||
#endif // BEZIER_CURVE_SUPPORT
|
||||
|
||||
#if ENABLED(FWRETRACT)
|
||||
|
||||
/**
|
||||
|
@ -6498,10 +6544,12 @@ void process_next_command() {
|
|||
|
||||
// G2, G3
|
||||
#if ENABLED(ARC_SUPPORT) && DISABLED(SCARA)
|
||||
|
||||
case 2: // G2 - CW ARC
|
||||
case 3: // G3 - CCW ARC
|
||||
gcode_G2_G3(codenum == 2);
|
||||
break;
|
||||
|
||||
#endif
|
||||
|
||||
// G4 Dwell
|
||||
|
@ -6509,6 +6557,15 @@ void process_next_command() {
|
|||
gcode_G4();
|
||||
break;
|
||||
|
||||
#if ENABLED(BEZIER_CURVE_SUPPORT)
|
||||
|
||||
// G5
|
||||
case 5: // G5 - Cubic B_spline
|
||||
gcode_G5();
|
||||
break;
|
||||
|
||||
#endif // BEZIER_CURVE_SUPPORT
|
||||
|
||||
#if ENABLED(FWRETRACT)
|
||||
|
||||
case 10: // G10: retract
|
||||
|
@ -7588,6 +7645,19 @@ void prepare_move() {
|
|||
}
|
||||
#endif
|
||||
|
||||
#if ENABLED(BEZIER_CURVE_SUPPORT)
|
||||
|
||||
void plan_cubic_move(const float offset[4]) {
|
||||
cubic_b_spline(current_position, destination, offset, feedrate * feedrate_multiplier / 60 / 100.0, active_extruder);
|
||||
|
||||
// As far as the parser is concerned, the position is now == target. In reality the
|
||||
// motion control system might still be processing the action and the real tool position
|
||||
// in any intermediate location.
|
||||
set_current_to_destination();
|
||||
}
|
||||
|
||||
#endif // BEZIER_CURVE_SUPPORT
|
||||
|
||||
#if HAS_CONTROLLERFAN
|
||||
|
||||
void controllerFan() {
|
||||
|
|
|
@ -284,8 +284,6 @@
|
|||
|
||||
#define AXIS_RELATIVE_MODES {false, false, false, false}
|
||||
|
||||
// @section machine
|
||||
|
||||
//By default pololu step drivers require an active high signal. However, some high power drivers require an active low signal as step.
|
||||
#define INVERT_X_STEP_PIN false
|
||||
#define INVERT_Y_STEP_PIN false
|
||||
|
@ -459,6 +457,9 @@
|
|||
#define MM_PER_ARC_SEGMENT 1
|
||||
#define N_ARC_CORRECTION 25
|
||||
|
||||
// Support for G5 with XYZE destination and IJPQ offsets
|
||||
//#define BEZIER_CURVE_SUPPORT
|
||||
|
||||
const unsigned int dropsegments = 5; //everything with less than this number of steps will be ignored as move and joined with the next movement
|
||||
|
||||
// @section temperature
|
||||
|
|
|
@ -284,8 +284,6 @@
|
|||
|
||||
#define AXIS_RELATIVE_MODES {false, false, false, false}
|
||||
|
||||
// @section machine
|
||||
|
||||
//By default pololu step drivers require an active high signal. However, some high power drivers require an active low signal as step.
|
||||
#define INVERT_X_STEP_PIN false
|
||||
#define INVERT_Y_STEP_PIN false
|
||||
|
@ -459,6 +457,9 @@
|
|||
#define MM_PER_ARC_SEGMENT 1
|
||||
#define N_ARC_CORRECTION 25
|
||||
|
||||
// Support for G5 with XYZE destination and IJPQ offsets
|
||||
//#define BEZIER_CURVE_SUPPORT
|
||||
|
||||
const unsigned int dropsegments = 5; //everything with less than this number of steps will be ignored as move and joined with the next movement
|
||||
|
||||
// @section temperature
|
||||
|
|
|
@ -284,8 +284,6 @@
|
|||
|
||||
#define AXIS_RELATIVE_MODES {false, false, false, false}
|
||||
|
||||
// @section machine
|
||||
|
||||
//By default pololu step drivers require an active high signal. However, some high power drivers require an active low signal as step.
|
||||
#define INVERT_X_STEP_PIN false
|
||||
#define INVERT_Y_STEP_PIN false
|
||||
|
@ -459,6 +457,9 @@
|
|||
#define MM_PER_ARC_SEGMENT 1
|
||||
#define N_ARC_CORRECTION 25
|
||||
|
||||
// Support for G5 with XYZE destination and IJPQ offsets
|
||||
//#define BEZIER_CURVE_SUPPORT
|
||||
|
||||
const unsigned int dropsegments = 5; //everything with less than this number of steps will be ignored as move and joined with the next movement
|
||||
|
||||
// @section temperature
|
||||
|
|
|
@ -290,8 +290,6 @@
|
|||
|
||||
#define AXIS_RELATIVE_MODES {false, false, false, false}
|
||||
|
||||
// @section machine
|
||||
|
||||
//By default pololu step drivers require an active high signal. However, some high power drivers require an active low signal as step.
|
||||
#define INVERT_X_STEP_PIN false
|
||||
#define INVERT_Y_STEP_PIN false
|
||||
|
@ -465,6 +463,9 @@
|
|||
#define MM_PER_ARC_SEGMENT 1
|
||||
#define N_ARC_CORRECTION 25
|
||||
|
||||
// Support for G5 with XYZE destination and IJPQ offsets
|
||||
//#define BEZIER_CURVE_SUPPORT
|
||||
|
||||
const unsigned int dropsegments = 2; //everything with less than this number of steps will be ignored as move and joined with the next movement
|
||||
|
||||
// @section temperature
|
||||
|
|
|
@ -284,8 +284,6 @@
|
|||
|
||||
#define AXIS_RELATIVE_MODES {false, false, false, false}
|
||||
|
||||
// @section machine
|
||||
|
||||
//By default pololu step drivers require an active high signal. However, some high power drivers require an active low signal as step.
|
||||
#define INVERT_X_STEP_PIN false
|
||||
#define INVERT_Y_STEP_PIN false
|
||||
|
@ -459,6 +457,9 @@
|
|||
#define MM_PER_ARC_SEGMENT 1
|
||||
#define N_ARC_CORRECTION 25
|
||||
|
||||
// Support for G5 with XYZE destination and IJPQ offsets
|
||||
//#define BEZIER_CURVE_SUPPORT
|
||||
|
||||
const unsigned int dropsegments = 5; //everything with less than this number of steps will be ignored as move and joined with the next movement
|
||||
|
||||
// @section temperature
|
||||
|
|
|
@ -284,8 +284,6 @@
|
|||
|
||||
#define AXIS_RELATIVE_MODES {false, false, false, false}
|
||||
|
||||
// @section machine
|
||||
|
||||
//By default pololu step drivers require an active high signal. However, some high power drivers require an active low signal as step.
|
||||
#define INVERT_X_STEP_PIN false
|
||||
#define INVERT_Y_STEP_PIN false
|
||||
|
@ -459,6 +457,9 @@
|
|||
#define MM_PER_ARC_SEGMENT 1
|
||||
#define N_ARC_CORRECTION 25
|
||||
|
||||
// Support for G5 with XYZE destination and IJPQ offsets
|
||||
//#define BEZIER_CURVE_SUPPORT
|
||||
|
||||
const unsigned int dropsegments = 5; //everything with less than this number of steps will be ignored as move and joined with the next movement
|
||||
|
||||
// @section temperature
|
||||
|
|
|
@ -292,8 +292,6 @@
|
|||
|
||||
#define AXIS_RELATIVE_MODES {false, false, false, false}
|
||||
|
||||
// @section machine
|
||||
|
||||
//By default pololu step drivers require an active high signal. However, some high power drivers require an active low signal as step.
|
||||
#define INVERT_X_STEP_PIN false
|
||||
#define INVERT_Y_STEP_PIN false
|
||||
|
@ -467,6 +465,9 @@
|
|||
#define MM_PER_ARC_SEGMENT 1
|
||||
#define N_ARC_CORRECTION 25
|
||||
|
||||
// Support for G5 with XYZE destination and IJPQ offsets
|
||||
//#define BEZIER_CURVE_SUPPORT
|
||||
|
||||
const unsigned int dropsegments = 5; //everything with less than this number of steps will be ignored as move and joined with the next movement
|
||||
|
||||
// @section temperature
|
||||
|
|
|
@ -284,8 +284,6 @@
|
|||
|
||||
#define AXIS_RELATIVE_MODES {false, false, false, false}
|
||||
|
||||
// @section machine
|
||||
|
||||
//By default pololu step drivers require an active high signal. However, some high power drivers require an active low signal as step.
|
||||
#define INVERT_X_STEP_PIN false
|
||||
#define INVERT_Y_STEP_PIN false
|
||||
|
@ -459,6 +457,9 @@
|
|||
#define MM_PER_ARC_SEGMENT 1
|
||||
#define N_ARC_CORRECTION 25
|
||||
|
||||
// Support for G5 with XYZE destination and IJPQ offsets
|
||||
//#define BEZIER_CURVE_SUPPORT
|
||||
|
||||
const unsigned int dropsegments = 5; //everything with less than this number of steps will be ignored as move and joined with the next movement
|
||||
|
||||
// @section temperature
|
||||
|
|
|
@ -284,8 +284,6 @@
|
|||
|
||||
#define AXIS_RELATIVE_MODES {false, false, false, false}
|
||||
|
||||
// @section machine
|
||||
|
||||
//By default pololu step drivers require an active high signal. However, some high power drivers require an active low signal as step.
|
||||
#define INVERT_X_STEP_PIN false
|
||||
#define INVERT_Y_STEP_PIN false
|
||||
|
@ -461,6 +459,9 @@
|
|||
#define MM_PER_ARC_SEGMENT 1
|
||||
#define N_ARC_CORRECTION 25
|
||||
|
||||
// Support for G5 with XYZE destination and IJPQ offsets
|
||||
//#define BEZIER_CURVE_SUPPORT
|
||||
|
||||
const unsigned int dropsegments = 5; //everything with less than this number of steps will be ignored as move and joined with the next movement
|
||||
|
||||
// @section temperature
|
||||
|
|
|
@ -284,8 +284,6 @@
|
|||
|
||||
#define AXIS_RELATIVE_MODES {false, false, false, false}
|
||||
|
||||
// @section machine
|
||||
|
||||
//By default pololu step drivers require an active high signal. However, some high power drivers require an active low signal as step.
|
||||
#define INVERT_X_STEP_PIN false
|
||||
#define INVERT_Y_STEP_PIN false
|
||||
|
@ -461,6 +459,9 @@
|
|||
#define MM_PER_ARC_SEGMENT 1
|
||||
#define N_ARC_CORRECTION 25
|
||||
|
||||
// Support for G5 with XYZE destination and IJPQ offsets
|
||||
//#define BEZIER_CURVE_SUPPORT
|
||||
|
||||
const unsigned int dropsegments = 5; //everything with less than this number of steps will be ignored as move and joined with the next movement
|
||||
|
||||
// @section temperature
|
||||
|
|
|
@ -284,8 +284,6 @@
|
|||
|
||||
#define AXIS_RELATIVE_MODES {false, false, false, false}
|
||||
|
||||
// @section machine
|
||||
|
||||
//By default pololu step drivers require an active high signal. However, some high power drivers require an active low signal as step.
|
||||
#define INVERT_X_STEP_PIN false
|
||||
#define INVERT_Y_STEP_PIN false
|
||||
|
@ -460,6 +458,9 @@
|
|||
#define MM_PER_ARC_SEGMENT 1
|
||||
#define N_ARC_CORRECTION 25
|
||||
|
||||
// Support for G5 with XYZE destination and IJPQ offsets
|
||||
//#define BEZIER_CURVE_SUPPORT
|
||||
|
||||
const unsigned int dropsegments = 5; //everything with less than this number of steps will be ignored as move and joined with the next movement
|
||||
|
||||
// @section temperature
|
||||
|
|
|
@ -289,8 +289,6 @@
|
|||
|
||||
#define AXIS_RELATIVE_MODES {false, false, false, false}
|
||||
|
||||
// @section machine
|
||||
|
||||
//By default pololu step drivers require an active high signal. However, some high power drivers require an active low signal as step.
|
||||
#define INVERT_X_STEP_PIN false
|
||||
#define INVERT_Y_STEP_PIN false
|
||||
|
@ -465,6 +463,9 @@
|
|||
#define MM_PER_ARC_SEGMENT 1
|
||||
#define N_ARC_CORRECTION 25
|
||||
|
||||
// Support for G5 with XYZE destination and IJPQ offsets
|
||||
//#define BEZIER_CURVE_SUPPORT
|
||||
|
||||
const unsigned int dropsegments = 5; //everything with less than this number of steps will be ignored as move and joined with the next movement
|
||||
|
||||
// @section temperature
|
||||
|
|
|
@ -284,8 +284,6 @@
|
|||
|
||||
#define AXIS_RELATIVE_MODES {false, false, false, false}
|
||||
|
||||
// @section machine
|
||||
|
||||
//By default pololu step drivers require an active high signal. However, some high power drivers require an active low signal as step.
|
||||
#define INVERT_X_STEP_PIN false
|
||||
#define INVERT_Y_STEP_PIN false
|
||||
|
@ -461,6 +459,9 @@
|
|||
#define MM_PER_ARC_SEGMENT 1
|
||||
#define N_ARC_CORRECTION 25
|
||||
|
||||
// Support for G5 with XYZE destination and IJPQ offsets
|
||||
//#define BEZIER_CURVE_SUPPORT
|
||||
|
||||
const unsigned int dropsegments = 5; //everything with less than this number of steps will be ignored as move and joined with the next movement
|
||||
|
||||
// @section temperature
|
||||
|
|
|
@ -284,8 +284,6 @@
|
|||
|
||||
#define AXIS_RELATIVE_MODES {false, false, false, false}
|
||||
|
||||
// @section machine
|
||||
|
||||
//By default pololu step drivers require an active high signal. However, some high power drivers require an active low signal as step.
|
||||
#define INVERT_X_STEP_PIN false
|
||||
#define INVERT_Y_STEP_PIN false
|
||||
|
@ -459,6 +457,9 @@
|
|||
#define MM_PER_ARC_SEGMENT 1
|
||||
#define N_ARC_CORRECTION 25
|
||||
|
||||
// Support for G5 with XYZE destination and IJPQ offsets
|
||||
//#define BEZIER_CURVE_SUPPORT
|
||||
|
||||
const unsigned int dropsegments = 5; //everything with less than this number of steps will be ignored as move and joined with the next movement
|
||||
|
||||
// @section temperature
|
||||
|
|
|
@ -284,8 +284,6 @@
|
|||
|
||||
#define AXIS_RELATIVE_MODES {false, false, false, false}
|
||||
|
||||
// @section machine
|
||||
|
||||
//By default pololu step drivers require an active high signal. However, some high power drivers require an active low signal as step.
|
||||
#define INVERT_X_STEP_PIN false
|
||||
#define INVERT_Y_STEP_PIN false
|
||||
|
@ -459,6 +457,9 @@
|
|||
#define MM_PER_ARC_SEGMENT 1
|
||||
#define N_ARC_CORRECTION 25
|
||||
|
||||
// Support for G5 with XYZE destination and IJPQ offsets
|
||||
//#define BEZIER_CURVE_SUPPORT
|
||||
|
||||
const unsigned int dropsegments = 5; //everything with less than this number of steps will be ignored as move and joined with the next movement
|
||||
|
||||
// @section temperature
|
||||
|
|
194
Marlin/planner_bezier.cpp
Normal file
194
Marlin/planner_bezier.cpp
Normal file
|
@ -0,0 +1,194 @@
|
|||
/**
|
||||
* 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 "Marlin.h"
|
||||
|
||||
#if ENABLED(BEZIER_CURVE_SUPPORT)
|
||||
|
||||
#include "planner.h"
|
||||
#include "language.h"
|
||||
#include "temperature.h"
|
||||
|
||||
// See the meaning in the documentation of cubic_b_spline().
|
||||
#define MIN_STEP 0.002
|
||||
#define MAX_STEP 0.1
|
||||
#define SIGMA 0.1
|
||||
|
||||
/* Compute the linear interpolation between to real numbers.
|
||||
*/
|
||||
inline static float interp(float a, float b, float t) { return (1.0 - 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 fabs(x1 - x2) + fabs(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 position[NUM_AXIS], const float target[NUM_AXIS], const float offset[4], float feed_rate, uint8_t extruder) {
|
||||
// Absolute first and second control points are recovered.
|
||||
float first0 = position[X_AXIS] + offset[0];
|
||||
float first1 = position[Y_AXIS] + offset[1];
|
||||
float second0 = target[X_AXIS] + offset[2];
|
||||
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 step = MAX_STEP;
|
||||
|
||||
uint8_t idle_counter = 0;
|
||||
millis_t next_ping_ms = millis() + 200UL;
|
||||
|
||||
while (t < 1.0) {
|
||||
|
||||
millis_t now = millis();
|
||||
if (ELAPSED(now, next_ping_ms)) {
|
||||
next_ping_ms = now + 200UL;
|
||||
(idle_counter++ & 0x03) ? thermalManager.manage_heater() : 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.0);
|
||||
float new_pos0 = eval_bezier(position[X_AXIS], first0, second0, target[X_AXIS], new_t);
|
||||
float new_pos1 = eval_bezier(position[Y_AXIS], first1, second1, target[Y_AXIS], new_t);
|
||||
for (;;) {
|
||||
if (new_t - t < (MIN_STEP)) break;
|
||||
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);
|
||||
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;
|
||||
float candidate_t = t + 2.0 * (new_t - t);
|
||||
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);
|
||||
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 position
|
||||
tmp[X_AXIS] = new_pos0;
|
||||
tmp[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);
|
||||
}
|
||||
}
|
||||
|
||||
#endif // BEZIER_CURVE_SUPPORT
|
43
Marlin/planner_bezier.h
Normal file
43
Marlin/planner_bezier.h
Normal file
|
@ -0,0 +1,43 @@
|
|||
/**
|
||||
* 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.h
|
||||
*
|
||||
* Compute and buffer movement commands for bezier curves
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef PLANNER_BEZIER_H
|
||||
#define PLANNER_BEZIER_H
|
||||
|
||||
#include "Marlin.h"
|
||||
|
||||
void cubic_b_spline(
|
||||
const float position[NUM_AXIS], // current position
|
||||
const float target[NUM_AXIS], // target position
|
||||
const float offset[4], // a pair of offsets
|
||||
float feed_rate,
|
||||
uint8_t extruder
|
||||
);
|
||||
|
||||
#endif // PLANNER_BEZIER_H
|
Loading…
Reference in a new issue