andrey
/
Marlin_FB4S
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Bezier style and DELTA patch

pull/1/head
Scott Lahteine 9 years ago
parent
3016dfe484
commit
ecec5c5e58
1 changed files with 22 additions and 13 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 35
      Marlin/planner_bezier.cpp

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
} }
} }

Write Preview
Loading…
Cancel
Save