thebears/marlin_skr
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Add custom types for position (#15204)

Browse Source
This commit is contained in:
Scott Lahteine
2019-09-29 04:25:39 -05:00
committed by GitHub
parent 43d6e9fa43
commit 50e4545255
227 changed files with 3147 additions and 3264 deletions
Download Patch File Download Diff File Expand all files Collapse all files

54
Marlin/src/module/planner_bezier.cpp
View File

@@ -108,21 +108,17 @@ static inline float dist1(const float &x1, const float &y1, const float &x2, con
* power available on Arduino, I think it is not wise to implement it.
*/
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
const xyze_pos_t &position, // current position
const xyze_pos_t &target, // target position
const xy_pos_t (&offsets)[2], // a pair of offsets
const feedRate_t &scaled_fr_mm_s, // mm/s scaled by feedrate %
const uint8_t extruder
) {
// Absolute first and second control points are recovered.
const float first0 = position[X_AXIS] + offset[0],
first1 = position[Y_AXIS] + offset[1],
second0 = target[X_AXIS] + offset[2],
second1 = target[Y_AXIS] + offset[3];
const xy_pos_t first = position + offsets[0], second = target + offsets[1];
float bez_target[4];
bez_target[X_AXIS] = position[X_AXIS];
bez_target[Y_AXIS] = position[Y_AXIS];
xyze_pos_t bez_target;
bez_target.set(position.x, position.y);
float step = MAX_STEP;
millis_t next_idle_ms = millis() + 200UL;
@@ -141,15 +137,15 @@ void cubic_b_spline(
bool did_reduce = false;
float new_t = t + step;
NOMORE(new_t, 1);
float new_pos0 = eval_bezier(position[X_AXIS], first0, second0, target[X_AXIS], new_t),
new_pos1 = eval_bezier(position[Y_AXIS], first1, second1, target[Y_AXIS], new_t);
float new_pos0 = eval_bezier(position.x, first.x, second.x, target.x, new_t),
new_pos1 = eval_bezier(position.y, first.y, second.y, target.y, new_t);
for (;;) {
if (new_t - t < (MIN_STEP)) break;
const float candidate_t = 0.5f * (t + new_t),
candidate_pos0 = eval_bezier(position[X_AXIS], first0, second0, target[X_AXIS], candidate_t),
candidate_pos1 = eval_bezier(position[Y_AXIS], first1, second1, 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);
candidate_pos0 = eval_bezier(position.x, first.x, second.x, target.x, candidate_t),
candidate_pos1 = eval_bezier(position.y, first.y, second.y, target.y, candidate_t),
interp_pos0 = 0.5f * (bez_target.x + new_pos0),
interp_pos1 = 0.5f * (bez_target.y + new_pos1);
if (dist1(candidate_pos0, candidate_pos1, interp_pos0, interp_pos1) <= (SIGMA)) break;
new_t = candidate_t;
new_pos0 = candidate_pos0;
@@ -162,10 +158,10 @@ void cubic_b_spline(
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(position[X_AXIS], first0, second0, target[X_AXIS], candidate_t),
candidate_pos1 = eval_bezier(position[Y_AXIS], first1, second1, 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);
const float candidate_pos0 = eval_bezier(position.x, first.x, second.x, target.x, candidate_t),
candidate_pos1 = eval_bezier(position.y, first.y, second.y, target.y, candidate_t),
interp_pos0 = 0.5f * (bez_target.x + candidate_pos0),
interp_pos1 = 0.5f * (bez_target.y + candidate_pos1);
if (dist1(new_pos0, new_pos1, interp_pos0, interp_pos1) > (SIGMA)) break;
new_t = candidate_t;
new_pos0 = candidate_pos0;
@@ -187,19 +183,19 @@ void cubic_b_spline(
t = new_t;
// Compute and send new position
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.
bez_target[Z_AXIS] = interp(position[Z_AXIS], target[Z_AXIS], t);
bez_target[E_AXIS] = interp(position[E_AXIS], target[E_AXIS], t);
apply_motion_limits(bez_target);
xyze_pos_t new_bez = {
new_pos0, new_pos1,
interp(position.z, target.z, t), // FIXME. These two are wrong, since the parameter t is
interp(position.e, target.e, t) // not linear in the distance.
};
apply_motion_limits(new_bez);
bez_target = new_bez;
#if HAS_LEVELING && !PLANNER_LEVELING
float pos[XYZE] = { bez_target[X_AXIS], bez_target[Y_AXIS], bez_target[Z_AXIS], bez_target[E_AXIS] };
xyze_pos_t pos = bez_target;
planner.apply_leveling(pos);
#else
const float (&pos)[XYZE] = bez_target;
const xyze_pos_t &pos = bez_target;
#endif
if (!planner.buffer_line(pos, scaled_fr_mm_s, active_extruder, step))