MPCNC/Firmware2
1
0
Fork 0
Code Issues 1 Pull Requests Releases Wiki Activity

Bezier style and DELTA patch

Browse Source
This commit is contained in:
Scott Lahteine 2016-05-17 18:05:37 -07:00
parent 3016dfe484
commit ecec5c5e58
1 changed files with 22 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
Marlin/planner_bezier.cpp
View File

@ -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 second1 = target[Y_AXIS] + offset[3];
float t = 0.0; float t = 0.0;
float tmp[4]; float bez_target[4];
tmp[X_AXIS] = position[X_AXIS]; bez_target[X_AXIS] = position[X_AXIS];
tmp[Y_AXIS] = position[Y_AXIS]; bez_target[Y_AXIS] = position[Y_AXIS];
float step = MAX_STEP; float step = MAX_STEP;
uint8_t idle_counter = 0; 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_t = 0.5 * (t + new_t);
float candidate_pos0 = eval_bezier(position[X_AXIS], first0, second0, target[X_AXIS], candidate_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 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_pos0 = 0.5 * (bez_target[X_AXIS] + new_pos0);
float interp_pos1 = 0.5 * (tmp[Y_AXIS] + new_pos1); float interp_pos1 = 0.5 * (bez_target[Y_AXIS] + new_pos1);
if (dist1(candidate_pos0, candidate_pos1, interp_pos0, interp_pos1) <= (SIGMA)) break; if (dist1(candidate_pos0, candidate_pos1, interp_pos0, interp_pos1) <= (SIGMA)) break;
new_t = candidate_t; new_t = candidate_t;
new_pos0 = candidate_pos0; 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; if (candidate_t >= 1.0) break;
float candidate_pos0 = eval_bezier(position[X_AXIS], first0, second0, target[X_AXIS], candidate_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 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_pos0 = 0.5 * (bez_target[X_AXIS] + candidate_pos0);
float interp_pos1 = 0.5 * (tmp[Y_AXIS] + candidate_pos1); float interp_pos1 = 0.5 * (bez_target[Y_AXIS] + candidate_pos1);
if (dist1(new_pos0, new_pos1, interp_pos0, interp_pos1) > (SIGMA)) break; if (dist1(new_pos0, new_pos1, interp_pos0, interp_pos1) > (SIGMA)) break;
new_t = candidate_t; new_t = candidate_t;
new_pos0 = candidate_pos0; 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; t = new_t;
// Compute and send new position // Compute and send new position
tmp[X_AXIS] = new_pos0; bez_target[X_AXIS] = new_pos0;
tmp[Y_AXIS] = new_pos1; bez_target[Y_AXIS] = new_pos1;
// FIXME. The following two are wrong, since the parameter t is // FIXME. The following two are wrong, since the parameter t is
// not linear in the distance. // not linear in the distance.
tmp[Z_AXIS] = interp(position[Z_AXIS], target[Z_AXIS], t); bez_target[Z_AXIS] = interp(position[Z_AXIS], target[Z_AXIS], t);
tmp[E_AXIS] = interp(position[E_AXIS], target[E_AXIS], t); bez_target[E_AXIS] = interp(position[E_AXIS], target[E_AXIS], t);
clamp_to_software_endstops(tmp); clamp_to_software_endstops(bez_target);
planner.buffer_line(tmp[X_AXIS], tmp[Y_AXIS], tmp[Z_AXIS], tmp[E_AXIS], feed_rate, extruder);
#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
} }
} }