void ReachabilityCollector_change(struct ReachabilityCollector* rc, struct Address* nodeAddr)
{
struct ReachabilityCollector_pvt* rcp = Identity_check((struct ReachabilityCollector_pvt*) rc);
struct Allocator* tempAlloc = Allocator_child(rcp->alloc);
change0(rcp, nodeAddr, tempAlloc);
Allocator_free(tempAlloc);
}
// Used from previous segment (if prepared): tp, vq.
uint8_t next_move() { // {{{
settings.probing = false;
moving_to_current = 0;
uint8_t num_cbs = 0;
uint8_t a0;
run_file_fill_queue();
if (settings.queue_start == settings.queue_end && !settings.queue_full) {
//debug("no next move");
computing_move = false;
prepared = false;
return num_cbs;
}
#ifdef DEBUG_MOVE
debug("Next move; queue start = %d, end = %d", settings.queue_start, settings.queue_end);
#endif
// Set everything up for running queue[settings.queue_start].
uint8_t n = (settings.queue_start + 1) % QUEUE_LENGTH;
// Make sure printer state is good. {{{
// If the source is unknown, determine it from current_pos.
//for (uint8_t a = 0; a < num_axes; ++a)
// debug("target %d %f", a, queue[settings.queue_start].data[a]);
for (uint8_t s = 0; s < 2; ++s) {
Space &sp = spaces[s];
for (uint8_t a = 0; a < sp.num_axes; ++a) {
if (isnan(sp.axis[a]->settings.source)) {
space_types[sp.type].reset_pos(&sp);
for (uint8_t aa = 0; aa < sp.num_axes; ++aa)
sp.axis[aa]->settings.current = sp.axis[aa]->settings.source;
break;
}
#ifdef DEBUG_MOVE
else
debug("non-nan: %d %d %f %d", s, a, sp.axis[a]->settings.source, sp.motor[a]->settings.current_pos);
#endif
}
}
// }}}
settings.f0 = settings.fq;
// If no move is prepared, set dist[1] from the queue; it will be used as dist[0] below. {{{
if (!prepared) {
#ifdef DEBUG_MOVE
debug("No move prepared.");
#endif
settings.f0 = 0;
a0 = 0;
change0(settings.queue_start);
for (uint8_t s = 0; s < 2; ++s) {
Space &sp = spaces[s];
space_types[sp.type].check_position(&sp, &queue[settings.queue_start].data[a0]);
sp.settings.dist[0] = 0;
for (int a = 0; a < sp.num_axes; ++a) {
sp.axis[a]->settings.dist[0] = 0;
sp.axis[a]->settings.endpos[0] = sp.axis[a]->settings.source;
}
set_from_queue(s, settings.queue_start, a0, false);
a0 += sp.num_axes;
}
}
// }}}
// Fill unspecified coordinates with previous values. {{{
a0 = 0;
for (uint8_t s = 0; s < 2; ++s) {
Space &sp = spaces[s];
for (uint8_t a = 0; a < sp.num_axes; ++a) {
if (n != settings.queue_end) {
// If only one of them is set, set the other one as well to make the rounded corner work.
if (!isnan(queue[settings.queue_start].data[a0 + a]) && isnan(queue[n].data[a0 + a])) {
queue[n].data[a0 + a] = sp.axis[a]->settings.source + sp.axis[a]->settings.dist[1] - (s == 0 && a == 2 ? zoffset : 0);
#ifdef DEBUG_MOVE
debug("filling next %d with %f", a0 + a, queue[n].data[a0 + a]);
#endif
}
if (isnan(queue[settings.queue_start].data[a]) && !isnan(queue[n].data[a])) {
queue[settings.queue_start].data[a0 + a] = sp.axis[a]->settings.source;
#ifdef DEBUG_MOVE
debug("filling %d with %f", a0 + a, queue[settings.queue_start].data[a0 + a]);
#endif
}
}
if ((!isnan(queue[settings.queue_start].data[a0 + a]) || (n != settings.queue_end && !isnan(queue[n].data[a0 + a]))) && isnan(sp.axis[a]->settings.source)) {
debug("Motor positions are not known, so move cannot take place; aborting move and removing it from the queue: %f %f %f", queue[settings.queue_start].data[a0 + a], queue[n].data[a0 + a], sp.axis[a]->settings.source);
// This possibly removes one move too many, but it shouldn't happen anyway.
if (queue[settings.queue_start].cb)
++num_cbs;
if (settings.queue_end == settings.queue_start)
send_host(CMD_CONTINUE, 0);
settings.queue_start = n;
settings.queue_full = false;
abort_move(current_fragment_pos);
return num_cbs;
}
}
a0 += sp.num_axes;
}
// }}}
// We are prepared and can start the segment.
bool action = false;
double vq;
if (n == settings.queue_end) { // There is no next segment; we should stop at the end. {{{
prepared = false;
#ifdef DEBUG_MOVE
debug("Building final segment.");
#endif
for (uint8_t s = 0; s < 2; ++s) {
Space &sp = spaces[s];
copy_next(s);
if (sp.settings.dist[0] != 0)
action = true;
}
vq = 0;
}
// }}}
else { // There is a next segment; we should connect to it. {{{
prepared = true;
#ifdef DEBUG_MOVE
debug("Building a connecting segment.");
#endif
a0 = 0;
change0(n);
for (uint8_t s = 0; s < 2; ++s) {
Space &sp = spaces[s];
space_types[sp.type].check_position(&sp, &queue[n].data[a0]);
copy_next(s);
set_from_queue(s, n, a0, true);
if (sp.settings.dist[1] != 0 || sp.settings.dist[0] != 0)
action = true;
a0 += sp.num_axes;
}
vq = queue[n].f[0] * feedrate;
}
// }}}
double v0 = queue[settings.queue_start].f[0] * feedrate;
double vp = queue[settings.queue_start].f[1] * feedrate;
settings.probing = queue[settings.queue_start].probe;
settings.run_time = queue[settings.queue_start].time;
settings.run_dist = queue[settings.queue_start].dist;
if (queue[settings.queue_start].cb)
cbs_after_current_move += 1;
//debug("add cb to current starting at %d", current_fragment);
if (settings.queue_end == settings.queue_start)
send_host(CMD_CONTINUE, 0);
settings.queue_full = false;
settings.queue_start = n;
if (!action) { // Skip zero-distance move. {{{
#ifdef DEBUG_MOVE
debug("Skipping zero-distance prepared move");
#endif
num_cbs += cbs_after_current_move;
cbs_after_current_move = 0;
for (uint8_t s = 0; s < 2; ++s) {
Space &sp = spaces[s];
sp.settings.dist[0] = NAN;
for (uint8_t a = 0; a < sp.num_axes; ++a)
sp.axis[a]->settings.dist[0] = NAN;
}
settings.fq = 0;
return num_cbs + next_move();
} // }}}
// Currently set up:
// f0: fraction of move already done by connection.
// v0: this move's requested starting speed.
// vp: this move's requested ending speed.
// vq: next move's requested starting speed.
// cbs_after_current_move: number of cbs that should be fired after this segment is complete.
// dist[0]: total distance of this segment (mm).
// dist[1]: total distance of next segment (mm).
// mtr->dist[0]: motor distance of this segment (mm).
// mtr->dist[1]: motor distance of next segment (mm).
#ifdef DEBUG_MOVE
debug("Set up: v0 = %f /s, vp = %f /s, vq = %f /s", v0, vp, vq);
#endif
// Limit v0, vp, vq. {{{
for (uint8_t s = 0; s < 2; ++s) {
Space &sp = spaces[s];
double limit;
if (s == 0)
limit = max_v;
else if (current_extruder < sp.num_motors)
limit = sp.motor[current_extruder]->limit_v;
else
continue;
if (isnan(limit) || isinf(limit) || limit <= 0)
continue;
// max_mm is the maximum speed in mm/s.
double max_mm = settings.probing ? space_types[sp.type].probe_speed(&sp) : limit;
double max = max_mm / sp.settings.dist[0];
if (v0 < 0)
v0 = -v0 / sp.settings.dist[0];
if (vp < 0)
vp = -vp / sp.settings.dist[0];
if (vq < 0)
vq = -vq / sp.settings.dist[1];
if (v0 > max)
v0 = max;
if (vp > max)
vp = max;
max = max_mm / sp.settings.dist[1];
if (vq > max)
vq = max;
}
#ifdef DEBUG_MOVE
debug("After limiting, v0 = %f /s, vp = %f /s and vq = %f /s", v0, vp, vq);
#endif
// }}}
// Already set up: f0, v0, vp, vq, dist[0], dist[1], mtr->dist[0], mtr->dist[1].
// To do: start_time, t0, tp, fmain, fp, fq, mtr->main_dist
#ifdef DEBUG_MOVE
debug("Preparation did f0 = %f", settings.f0);
#endif
// Use maximum deviation to find fraction where to start rounded corner. {{{
double factor = vq / vp;
done_factor = NAN;
if (vq == 0) {
settings.fp = 0;
settings.fq = 0;
}
else {
settings.fp = factor > 1 ? .5 / factor : .5;
for (uint8_t s = 0; s < 2; ++s) {
Space &sp = spaces[s];
if (sp.num_axes < 2)
continue;
if (s != 0 || max_deviation == 0) {
settings.fp = 0;
break;
}
double nd = sp.settings.dist[1] * factor;
double d = sp.settings.dist[0] - sp.settings.dist[1];
// Calculate distances and ignore spaces which don't have two segments.
if (nd <= 0)
continue;
if (sp.settings.dist[0] <= 0)
continue;
double done = 1 - max_deviation / sp.settings.dist[0];
// Set it also if done_factor is NaN.
if (!(done <= done_factor))
done_factor = done;
double new_fp = max_deviation / sqrt(nd / (sp.settings.dist[0] + nd) * d);
#ifdef DEBUG_MOVE
debug("Space %d fp %f dev %f", s, settings.fp, max_deviation);
#endif
if (new_fp < settings.fp)
settings.fp = new_fp;
}
settings.fq = settings.fp * factor;
}
if (isnan(done_factor))
done_factor = 1;
// }}}
settings.t0 = (1 - settings.fp) / (fabs(v0 + vp) / 2);
settings.tp = settings.fp / (fabs(vp) / 2);
settings.f1 = .5 * fabs(v0) * settings.t0;
settings.f2 = 1 - settings.fp - settings.f1;
// Set up endpos. {{{
for (uint8_t s = 0; s < 2; ++s) {
Space &sp = spaces[s];
for (uint8_t a = 0; a < sp.num_axes; ++a) {
sp.axis[a]->settings.main_dist = sp.axis[a]->settings.dist[0] * (1 - settings.fp);
// Fill target for filling endpos below.
if ((sp.axis[a]->settings.dist[0] > 0 && sp.axis[a]->settings.dist[1] < 0) || (sp.axis[a]->settings.dist[0] < 0 && sp.axis[a]->settings.dist[1] > 0))
sp.axis[a]->settings.target = sp.axis[a]->settings.source + sp.axis[a]->settings.dist[0];
else
sp.axis[a]->settings.target = sp.axis[a]->settings.source + sp.axis[a]->settings.dist[0] + sp.axis[a]->settings.dist[1] * settings.fq;
#ifdef DEBUG_MOVE
debug("Axis %d %d dist %f main dist = %f, next dist = %f currentpos = %d current = %f", s, a, sp.axis[a]->settings.dist[0], sp.axis[a]->settings.main_dist, sp.axis[a]->settings.dist[1], sp.motor[a]->settings.current_pos, sp.axis[a]->settings.current);
#endif
}
bool ok = true;
// Using NULL as target fills endpos.
space_types[sp.type].xyz2motors(&sp, NULL, &ok);
}
// }}}
// Enable motors if they weren't. {{{
if (!motors_busy) {
for (uint8_t s = 0; s < 2; ++s) {
Space &sp = spaces[s];
for (uint8_t m = 0; m < sp.num_motors; ++m)
SET(sp.motor[m]->enable_pin);
}
motors_busy = true;
} // }}}
#ifdef DEBUG_MOVE
debug("Segment has been set up: f0=%f fp=%f fq=%f v0=%f /s vp=%f /s vq=%f /s t0=%f s tp=%f s", settings.f0, settings.fp, settings.fq, v0, vp, vq, settings.t0, settings.tp);
#endif
// Reset time. {{{
settings.hwtime = 0;
settings.last_time = 0;
settings.last_current_time = 0;
settings.start_time = settings.last_time - uint32_t(settings.f0 / fabs(vp) * 1e6);
// }}}
if (!computing_move) { // Set up source if this is a new move. {{{
#ifdef DEBUG_MOVE
debug("starting new move");
#endif
//debug("current %d running %d", current_fragment, running_fragment);
for (uint8_t s = 0; s < 2; ++s) {
Space &sp = spaces[s];
for (uint8_t a = 0; a < sp.num_axes; ++a)
sp.axis[a]->settings.source = sp.axis[a]->settings.current;
}
store_settings();
#ifdef DEBUG_PATH
fprintf(stderr, "\n");
#endif
} // }}}
first_fragment = current_fragment; // Do this every time, because otherwise the queue must be regenerated. TODO: send partial fragment to make sure this hack actually works, or fix it properly.
computing_move = true;
return num_cbs;
} // }}}
// For documentation about variables used here, see struct History in cdriver.h
int next_move(int32_t start_time) { // {{{
// Clean up state before starting the move.
if (current_fragment_pos > 0) {
//debug("sending because new move is started");
send_fragment();
}
settings.probing = false;
settings.factor = 0;
int num_cbs = 0;
run_file_fill_queue();
if (settings.queue_start == settings.queue_end && !settings.queue_full) {
//debug("no next move");
computing_move = false;
return num_cbs;
}
mdebug("Next move; queue start = %d, end = %d", settings.queue_start, settings.queue_end);
// Set everything up for running queue[settings.queue_start].
int q = settings.queue_start;
int n = (settings.queue_start + 1) % QUEUE_LENGTH;
settings.single = queue[q].single;
if (queue[q].cb)
++num_cbs;
// Make sure machine state is good. {{{
// If the source is unknown, determine it from current_pos.
for (int s = 0; s < NUM_SPACES; ++s) {
if (s == 1) {
// Extruders are handled later.
continue;
}
Space &sp = spaces[s];
for (int a = 0; a < sp.num_axes; ++a) {
if (std::isnan(sp.axis[a]->settings.source)) {
if (!std::isnan(sp.axis[a]->settings.current)) {
sp.axis[a]->settings.source = sp.axis[a]->settings.current;
continue;
}
reset_pos(&sp);
for (int aa = 0; aa < sp.num_axes; ++aa)
sp.axis[aa]->settings.current = sp.axis[aa]->settings.source;
break;
}
else
mdebug("non-nan: %d %d %f %f", s, a, sp.axis[a]->settings.source, sp.motor[a]->settings.current_pos);
}
// Followers don't have good motor data, so initialize them here.
for (int a = 0; a < sp.num_axes; ++a) {
if (s == 2)
sp.motor[a]->settings.current_pos = sp.axis[a]->settings.source;
}
}
// }}}
change0(q);
// Fill unspecified coordinates with previous values. {{{
Space &sp0 = spaces[0];
for (int a = 0; a < sp0.num_axes; ++a) {
if (n != settings.queue_end) {
// If only one of them is set, set the other one as well to make the rounded corner work.
if (!std::isnan(queue[q].X[a]) && std::isnan(queue[n].X[a])) {
queue[n].X[a] = queue[q].X[a];
mdebug("filling next %d with %f", a, queue[n].X[a]);
}
if (std::isnan(queue[q].X[a]) && !std::isnan(queue[n].X[a])) {
queue[q].X[a] = sp0.axis[a]->settings.source;
mdebug("filling %d with %f", a, queue[q].X[a]);
}
}
if ((!std::isnan(queue[q].X[a]) || (n != settings.queue_end && !std::isnan(queue[n].X[a]))) && std::isnan(sp0.axis[a]->settings.source)) {
debug("Motor position for axis %d is not known, so move cannot take place; aborting move and removing it from the queue: q1=%f q2=%f src=%f", a, queue[q].X[a], queue[n].X[a], sp0.axis[a]->settings.source);
// This possibly removes one move too many, but it shouldn't happen anyway.
settings.queue_start = n;
settings.queue_full = false;
abort_move(current_fragment_pos);
return num_cbs;
}
}
// }}}
// Reset time. {{{
if (computing_move) {
settings.hwtime -= start_time;
//debug("time -= %d, now %d", start_time, settings.hwtime);
}
else {
settings.hwtime = 0;
}
// }}}
if (!computing_move) { // Set up source if this is a new move. {{{
mdebug("starting new move");
//debug("current %d running %d", current_fragment, running_fragment);
for (int s = 0; s < NUM_SPACES; ++s) {
Space &sp = spaces[s];
for (int a = 0; a < sp.num_axes; ++a) {
if (!std::isnan(sp.axis[a]->settings.current)) {
mdebug("setting source for %d %d to current %f (was %f)", s, a, sp.axis[a]->settings.current, sp.axis[a]->settings.source);
sp.axis[a]->settings.source = sp.axis[a]->settings.current;
}
}
}
} // }}}
settings.v0 = queue[q].v0 * feedrate;
settings.v1 = queue[q].v1 * feedrate;
double dot = 0, norma = 0, normb = 0, normab = 0;
for (int i = 0; i < 3; ++i) {
bool use = i < spaces[0].num_axes;
double p = (use ? spaces[0].axis[i]->settings.source : 0);
settings.P[i] = (use ? (std::isnan(queue[q].X[i]) ? p : (queue[q].X[i] + (i == 2 ? zoffset : 0) + p) / 2) : 0);
settings.A[i] = settings.P[i] - p;
settings.B[i] = (use ? queue[q].B[i] : 0);
double ab = settings.A[i] + settings.B[i];
dot += settings.A[i] * ab;
norma += settings.A[i] * settings.A[i];
normb += settings.B[i] * settings.B[i];
normab += ab * ab;
}
norma = sqrt(norma);
normb = sqrt(normb);
normab = sqrt(normab);
settings.alpha_max = acos(dot / (norma * normab));
if (std::isnan(settings.alpha_max))
settings.alpha_max = 0;
settings.dist = (normb > 1e-5 ? norma * (normab / normb) * settings.alpha_max : norma) * 2;
if (std::isnan(settings.dist) || std::fabs(settings.dist) < 1e-10) {
//debug("no space dist, using other system. dist=%f a=%f ab=%f b=%f", settings.dist, norma, normab, normb);
if (queue[q].tool >= 0 && queue[q].tool < spaces[1].num_axes)
settings.dist = std::fabs(queue[q].e - spaces[1].axis[queue[q].tool]->settings.source);
else if (queue[q].single && queue[q].tool < 0 && ~queue[q].tool < spaces[2].num_axes)
settings.dist = std::fabs(queue[q].e - spaces[2].axis[~queue[q].tool]->settings.source);
}
double dt = settings.dist / ((settings.v0 + settings.v1) / 2);
settings.end_time = (std::isnan(dt) ? 0 : 1e6 * dt);
if (queue[q].tool >= 0 && queue[q].tool < spaces[1].num_axes && !std::isnan(queue[q].e)) {
spaces[1].axis[queue[q].tool]->settings.endpos = queue[q].e;
mdebug("move extruder to %f", queue[q].e);
}
else if (queue[q].single && queue[q].tool < 0 && ~queue[q].tool < spaces[2].num_axes && !std::isnan(queue[q].e)) {
spaces[2].axis[~queue[q].tool]->settings.endpos = queue[q].e;
mdebug("move follower to %f, current=%f source=%f current_pos=%f", queue[q].e, spaces[2].axis[~queue[q].tool]->settings.current, spaces[2].axis[~queue[q].tool]->settings.source, spaces[2].motor[~queue[q].tool]->settings.current_pos);
}
auto last_hwtime_step = settings.hwtime_step;
if (queue[q].pattern_size > 0) {
memcpy(settings.pattern, queue[q].pattern, queue[q].pattern_size);
settings.hwtime_step = dt * 1e6 / (queue[q].pattern_size * 8);
if (settings.hwtime_step < min_hwtime_step)
settings.hwtime_step = min_hwtime_step;
}
settings.pattern_size = queue[q].pattern_size;
if (settings.hwtime_step != last_hwtime_step)
arch_globals_change();
/*
if (spaces[0].num_axes > 2) {
debug("move prepared, from=(%f,%f,%f) Q=(%f,%f,%f) P=(%f,%f,%f), A=(%f,%f,%f), B=(%f,%f,%f), max alpha=%f, dist=%f, e=%f, v0=%f, v1=%f, end time=%f, single=%d, UVW=(%f,%f,%f)", spaces[0].axis[0]->settings.source, spaces[0].axis[1]->settings.source, spaces[0].axis[2]->settings.source, queue[q].X[0], queue[q].X[1], queue[q].X[2], settings.P[0], settings.P[1], settings.P[2], settings.A[0], settings.A[1], settings.A[2], settings.B[0], settings.B[1], settings.B[2], settings.alpha_max, settings.dist, queue[q].e, settings.v0, settings.v1, settings.end_time / 1e6, queue[q].single, spaces[0].motor[0]->settings.current_pos, spaces[0].motor[1]->settings.current_pos, spaces[0].motor[2]->settings.current_pos);
}
else if (spaces[0].num_axes > 1) {
debug("move prepared, from=(%f,%f) Q=(%f,%f,%f) P=(%f,%f,%f), A=(%f,%f,%f), B=(%f,%f,%f), max alpha=%f, dist=%f, e=%f, v0=%f, v1=%f, end time=%f, single=%d, UV=(%f,%f)", spaces[0].axis[0]->settings.source, spaces[0].axis[1]->settings.source, queue[q].X[0], queue[q].X[1], queue[q].X[2], settings.P[0], settings.P[1], settings.P[2], settings.A[0], settings.A[1], settings.A[2], settings.B[0], settings.B[1], settings.B[2], settings.alpha_max, settings.dist, queue[q].e, settings.v0, settings.v1, settings.end_time / 1e6, queue[q].single, spaces[0].motor[0]->settings.current_pos, spaces[0].motor[1]->settings.current_pos);
}
// */
//debug("times end %d current %d dist %f v0 %f v1 %f", settings.end_time, settings.hwtime, settings.dist, settings.v0, settings.v1);
settings.queue_start = n;
first_fragment = current_fragment; // Do this every time, because otherwise the queue must be regenerated. TODO: send partial fragment to make sure this hack actually works, or fix it properly.
if (!computing_move)
store_settings();
computing_move = true;
return num_cbs;
} // }}}
