static double set_targets(double factor) { // {{{
// Set motor targets for the requested factor. If limits are exceeded, return largest acceptable factor.
if (spaces[0].num_axes > 0) {
// Only handle positional move if there are positional axes.
double factor2 = 2 * factor - 1; // convert [0,1] to [-1,1]
double alpha = factor2 * settings.alpha_max; // requested angle.
// Moves are a combination of two vectors:
// A is from the middle of the line from start to end, to the end.
// B is from the middle of the line from start to end, to the middle of the arc.
// (A and B are perpendicular.)
// a and b are weights to use for A and B respectively.
// a = sin(alpha)/sin(alpha_max). For small angles this breaks, so just use factor2.
double denominator = sin(settings.alpha_max);
double a = (denominator < 1e-10 ? factor2 : sin(alpha) / denominator);
// b = cos(alpha)-cos(alpha_max)/(1-cos(alpha_max). For small angles this also breaks, so use 1-abs(factor2).
double cmax = cos(settings.alpha_max);
double b = (cos(alpha) - cmax) / (1 - cmax);
if (std::isnan(b))
b = 1 - std::fabs(factor2); // Doesn't really matter; B == {0, 0, 0}.
// Set position for xyz axes.
for (int i = 0; i < 3; ++i) {
if (i < spaces[0].num_axes) {
spaces[0].axis[i]->settings.target = settings.P[i] + a * settings.A[i] + b * settings.B[i];
mdebug("target %d = %f P %f a %f A %f B %f amax %f factor2 %f", i, spaces[0].axis[i]->settings.target, settings.P[i],a, settings.A[i], settings.B[i], settings.alpha_max, factor2);
}
}
}
// Set all other axes with linear interpolation and compute motor positions, returning maximum allowed factor.
double max_f = 1;
for (int s = 0; s < NUM_SPACES; ++s) {
Space &sp = spaces[s];
if (s == 2 && !settings.single)
continue;
for (int a = (s == 0 ? 3 : 0); a < sp.num_axes; ++a) {
auto ax = sp.axis[a];
if (std::isnan(ax->settings.source)) {
ax->settings.source = 0;
mdebug("setting axis %d %d source from nan to 0", s, a);
}
ax->settings.target = ax->settings.source + factor * (ax->settings.endpos - ax->settings.source);
mdebug("setting target for %d %d to %f (%f -> %f)", s, a, ax->settings.target, ax->settings.source, ax->settings.endpos);
}
double f = move_axes(&sp);
if (max_f > f)
max_f = f;
}
return max_f;
} // }}}
static void handle_motors(unsigned long long current_time) { // {{{
// Check for move.
if (!computing_move) {
movedebug("handle motors not moving");
return;
}
movedebug("handling %d", computing_move);
double factor = 1;
double t = (current_time - settings.start_time) / 1e6;
if (t >= settings.t0 + settings.tp) { // Finish this move and prepare next. {{{
movedebug("finishing %f %f %f %ld %ld", t, settings.t0, settings.tp, long(current_time), long(settings.start_time));
for (uint8_t s = 0; s < 2; ++s) {
Space &sp = spaces[s];
bool new_move = false;
if (!isnan(sp.settings.dist[0])) {
for (uint8_t a = 0; a < sp.num_axes; ++a) {
if (!isnan(sp.axis[a]->settings.dist[0])) {
sp.axis[a]->settings.source += sp.axis[a]->settings.dist[0];
sp.axis[a]->settings.dist[0] = NAN;
//debug("new source %d %f", a, sp.axis[a]->settings.source);
}
}
sp.settings.dist[0] = NAN;
}
for (uint8_t a = 0; a < sp.num_axes; ++a)
sp.axis[a]->settings.target = sp.axis[a]->settings.source;
move_axes(&sp, current_time, factor);
//debug("f %f", factor);
}
//debug("f2 %f %ld %ld", factor, settings.last_time, current_time);
bool did_steps = do_steps(factor, current_time);
//debug("f3 %f", factor);
// Start time may have changed; recalculate t.
t = (current_time - settings.start_time) / 1e6;
if (t / (settings.t0 + settings.tp) >= done_factor) {
uint8_t had_cbs = cbs_after_current_move;
cbs_after_current_move = 0;
run_file_fill_queue();
if (settings.queue_start != settings.queue_end || settings.queue_full) {
had_cbs += next_move();
if (!aborting && had_cbs > 0) {
//debug("adding %d cbs to fragment %d", had_cbs, current_fragment);
history[current_fragment].cbs += had_cbs;
}
return;
}
cbs_after_current_move += had_cbs;
if (factor == 1) {
//debug("queue done");
if (!did_steps) {
//debug("done move");
computing_move = false;
}
for (uint8_t s = 0; s < 2; ++s) {
Space &sp = spaces[s];
for (uint8_t m = 0; m < sp.num_motors; ++m)
sp.motor[m]->settings.last_v = 0;
}
if (cbs_after_current_move > 0) {
if (!aborting) {
//debug("adding %d cbs to final fragment %d", cbs_after_current_move, current_fragment);
history[current_fragment].cbs += cbs_after_current_move;
}
cbs_after_current_move = 0;
}
}
}
return;
} // }}}
if (t < settings.t0) { // Main part. {{{
double t_fraction = t / settings.t0;
double current_f = (settings.f1 * (2 - t_fraction) + settings.f2 * t_fraction) * t_fraction;
movedebug("main t %f t0 %f tp %f tfrac %f f1 %f f2 %f cf %f", t, settings.t0, settings.tp, t_fraction, settings.f1, settings.f2, current_f);
for (int 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.dist[0])) {
sp.axis[a]->settings.target = NAN;
continue;
}
sp.axis[a]->settings.target = sp.axis[a]->settings.source;
}
make_target(sp, current_f, false);
move_axes(&sp, current_time, factor);
}
} // }}}
else { // Connector part. {{{
movedebug("connector %f %f %f", t, settings.t0, settings.tp);
double tc = t - settings.t0;
double t_fraction = tc / settings.tp;
double current_f2 = settings.fp * (2 - t_fraction) * t_fraction;
double current_f3 = settings.fq * t_fraction * t_fraction;
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.dist[0]) && isnan(sp.axis[a]->settings.dist[1])) {
sp.axis[a]->settings.target = NAN;
continue;
}
sp.axis[a]->settings.target = sp.axis[a]->settings.source;
}
make_target(sp, (1 - settings.fp) + current_f2, false);
make_target(sp, current_f3, true);
move_axes(&sp, current_time, factor);
}
} // }}}
do_steps(factor, current_time);
} // }}}
static bool do_steps(double &factor, uint32_t current_time) { // {{{
//debug("steps");
if (factor <= 0) {
movedebug("end move");
return false;
}
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.target))
sp.axis[a]->settings.current += (sp.axis[a]->settings.target - sp.axis[a]->settings.current) * factor;
}
}
if (factor < 1) {
// Recalculate steps; ignore resulting factor.
double dummy_factor = 1;
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.target))
sp.axis[a]->settings.target = sp.axis[a]->settings.current;
}
move_axes(&sp, settings.last_time, dummy_factor);
}
}
//debug("do steps %f", factor);
uint32_t the_last_time = settings.last_current_time;
settings.last_current_time = current_time;
// Adjust start time if factor < 1.
bool have_steps = false;
if (factor < 1) {
for (uint8_t s = 0; !have_steps && s < 2; ++s) {
Space &sp = spaces[s];
for (uint8_t m = 0; m < sp.num_motors; ++m) {
Motor &mtr = *sp.motor[m];
double num = (mtr.settings.current_pos / mtr.steps_per_unit + mtr.settings.target_dist * factor) * mtr.steps_per_unit;
if (mtr.settings.current_pos != int(num + (num > 0 ? .49 : -.49))) {
//debug("have steps %d %f %f", mtr.settings.current_pos, mtr.settings.target_dist, factor);
have_steps = true;
break;
}
//debug("no steps yet %d %d", s, m);
}
}
// If there are no steps to take, wait until there are.
//static int streak = 0;
if (!have_steps) {
//movedebug("no steps");
// if (streak++ > 50)
// abort();
return false;
}
//streak = 0;
settings.start_time += (current_time - the_last_time) * ((1 - factor) * .99);
movedebug("correct: %f %d", factor, int(settings.start_time));
}
else
movedebug("no correct: %f %d", factor, int(settings.start_time));
settings.last_time = current_time;
// Move the motors.
//debug("start move");
for (uint8_t s = 0; s < 2; ++s) {
Space &sp = spaces[s];
for (uint8_t m = 0; m < sp.num_motors; ++m) {
Motor &mtr = *sp.motor[m];
if (isnan(mtr.settings.target_dist) || mtr.settings.target_dist == 0) {
//if (mtr.target_v == 0)
//mtr.last_v = 0;
continue;
}
double target = mtr.settings.current_pos / mtr.steps_per_unit + mtr.settings.target_dist * factor;
cpdebug(s, m, "ccp3 stopping %d target %f lastv %f spm %f tdist %f factor %f frag %d", stopping, target, mtr.settings.last_v, mtr.steps_per_unit, mtr.settings.target_dist, factor, current_fragment);
//if (fabs(mtr.settings.target_dist * factor) > .01) // XXX: This shouldn't ever happen on my printer, but shouldn't be a limitation.
//abort();
int new_cp = target * mtr.steps_per_unit + (target > 0 ? .49 : -.49);
if (mtr.settings.current_pos != new_cp) {
have_steps = true;
if (!mtr.active) {
mtr.active = true;
num_active_motors += 1;
}
DATA_SET(s, m, new_cp - mtr.settings.current_pos);
}
mtr.settings.current_pos = new_cp;
//cpdebug(s, m, "cp three %f", target);
mtr.settings.last_v = mtr.settings.target_v * factor;
}
}
current_fragment_pos += 1;
return have_steps;
} // }}}
static bool do_steps(double &factor, uint32_t current_time) { // {{{
//debug("steps");
if (factor <= 0) {
movedebug("end move");
return false;
}
for (int s = 0; s < NUM_SPACES; ++s) {
if (!settings.single && s == 2)
continue;
Space &sp = spaces[s];
for (int a = 0; a < sp.num_axes; ++a) {
if (!isnan(sp.axis[a]->settings.target)) {
//debug("add %d %d %f -> %f", s, a, (sp.axis[a]->settings.target - sp.axis[a]->settings.current) * factor, sp.axis[a]->settings.current);
sp.axis[a]->settings.current += (sp.axis[a]->settings.target - sp.axis[a]->settings.current) * factor;
}
}
}
if (factor < 1) {
// Recalculate steps; ignore resulting factor.
double dummy_factor = 1;
//debug("redo steps %d", current_fragment);
for (int s = 0; s < NUM_SPACES; ++s) {
if (!settings.single && s == 2)
continue;
Space &sp = spaces[s];
for (int a = 0; a < sp.num_axes; ++a) {
if (!isnan(sp.axis[a]->settings.target))
sp.axis[a]->settings.target = sp.axis[a]->settings.current;
}
move_axes(&sp, settings.last_time, dummy_factor);
}
}
//debug("do steps %f", factor);
uint32_t the_last_time = settings.last_current_time;
settings.last_current_time = current_time;
// Adjust start time if factor < 1.
bool have_steps = false;
if (factor < 1) {
for (int s = 0; !have_steps && s < NUM_SPACES; ++s) {
if (!settings.single && s == 2)
continue;
Space &sp = spaces[s];
for (int m = 0; m < sp.num_motors; ++m) {
Motor &mtr = *sp.motor[m];
double num = (mtr.settings.current_pos / mtr.steps_per_unit + mtr.settings.target_dist * factor) * mtr.steps_per_unit;
if (int(mtr.settings.current_pos) != int(num)) {
//debug("have steps %f %f %f", mtr.settings.current_pos, mtr.settings.target_dist, factor);
have_steps = true;
break;
}
//debug("no steps yet %d %d", s, m);
}
}
settings.start_time += (current_time - the_last_time) * ((1 - factor) * .99);
movedebug("correct: %f %d", factor, int(settings.start_time));
}
else
movedebug("no correct: %f %d", factor, int(settings.start_time));
settings.last_time = current_time;
#ifdef DEBUG_PATH
fprintf(stderr, "%d", current_time);
for (int a = 0; a < spaces[0].num_axes; ++a) {
if (isnan(spaces[0].axis[a]->settings.target))
fprintf(stderr, "\t%f", spaces[0].axis[a]->settings.source);
else
fprintf(stderr, "\t%f", spaces[0].axis[a]->settings.target);
}
fprintf(stderr, "\n");
#endif
// Move the motors.
//debug("start move");
for (int s = 0; s < NUM_SPACES; ++s) {
if (!settings.single && s == 2)
continue;
Space &sp = spaces[s];
for (int m = 0; m < sp.num_motors; ++m) {
Motor &mtr = *sp.motor[m];
if (isnan(mtr.settings.target_dist) || mtr.settings.target_dist == 0) {
//debug("no %d %d", s, m);
//if (mtr.target_v == 0)
//mtr.last_v = 0;
continue;
}
double target = mtr.settings.current_pos / mtr.steps_per_unit + mtr.settings.target_dist * factor;
cpdebug(s, m, "ccp3 stopping %d target %f lastv %f spm %f tdist %f factor %f frag %d", stopping, target, mtr.settings.last_v, mtr.steps_per_unit, mtr.settings.target_dist, factor, current_fragment);
//if (fabs(mtr.settings.target_dist * factor) > .01) // XXX: This shouldn't ever happen on my printer, but shouldn't be a limitation.
//abort();
double new_cp = target * mtr.steps_per_unit;
if (int(mtr.settings.current_pos) != int(new_cp)) {
have_steps = true;
if (!mtr.active) {
mtr.active = true;
num_active_motors += 1;
}
int diff = int(new_cp) - int(mtr.settings.current_pos);
DATA_SET(s, m, diff);
}
mtr.settings.current_pos = new_cp;
if (!settings.single) {
for (int mm = 0; mm < spaces[2].num_motors; ++mm) {
int fm = space_types[spaces[2].type].follow(&spaces[2], mm);
if (fm < 0)
continue;
int fs = fm >> 8;
fm &= 0xff;
if (fs != s || fm != m || (fs == 2 && fm >= mm))
continue;
spaces[2].motor[mm]->settings.current_pos += new_cp - mtr.settings.current_pos;
}
}
//cpdebug(s, m, "cp three %f", target);
mtr.settings.last_v = mtr.settings.target_v * factor;
}
}
current_fragment_pos += 1;
return have_steps;
} // }}}
static void handle_motors(unsigned long long current_time) { // {{{
// Check for move.
if (!computing_move) {
movedebug("handle motors not moving");
return;
}
movedebug("handling %d %d", computing_move, cbs_after_current_move);
double factor = 1;
double t = (current_time - settings.start_time) / 1e6;
if (t >= settings.t0 + settings.tp) { // Finish this move and prepare next. {{{
movedebug("finishing %f %f %f %ld %ld", t, settings.t0, settings.tp, long(current_time), long(settings.start_time));
//debug("finish steps");
for (int s = 0; s < NUM_SPACES; ++s) {
if (!settings.single && s == 2)
continue;
Space &sp = spaces[s];
bool new_move = false;
if (!isnan(sp.settings.dist[0])) {
for (int a = 0; a < sp.num_axes; ++a) {
if (!isnan(sp.axis[a]->settings.dist[0])) {
sp.axis[a]->settings.source += sp.axis[a]->settings.dist[0];
sp.axis[a]->settings.dist[0] = NAN;
//debug("new source %d %f", a, sp.axis[a]->settings.source);
}
}
sp.settings.dist[0] = NAN;
}
for (int a = 0; a < sp.num_axes; ++a)
sp.axis[a]->settings.target = sp.axis[a]->settings.source;
move_axes(&sp, current_time, factor);
//debug("f %f", factor);
}
//debug("f2 %f %ld %ld", factor, settings.last_time, current_time);
bool did_steps = do_steps(factor, current_time);
//debug("f3 %f", factor);
// Start time may have changed; recalculate t.
t = (current_time - settings.start_time) / 1e6;
if (t / (settings.t0 + settings.tp) >= done_factor) {
int had_cbs = cbs_after_current_move;
//debug("clearing %d cbs after current move for later inserting into history", cbs_after_current_move);
cbs_after_current_move = 0;
run_file_fill_queue();
if (settings.queue_start != settings.queue_end || settings.queue_full) {
had_cbs += next_move();
if (!aborting && had_cbs > 0) {
int fragment;
if (num_active_motors == 0)
fragment = (current_fragment + FRAGMENTS_PER_BUFFER - 1) % FRAGMENTS_PER_BUFFER;
else
fragment = current_fragment;
//debug("adding %d cbs to fragment %d", had_cbs, fragment);
history[fragment].cbs += had_cbs;
}
return;
}
cbs_after_current_move += had_cbs;
//debug("adding %d to cbs after current move making it %d", had_cbs, cbs_after_current_move);
if (factor == 1) {
//debug("queue done");
if (!did_steps) {
//debug("done move");
computing_move = false;
// Cut off final sample, which was no steps anyway.
current_fragment_pos -= 1;
}
for (int s = 0; s < NUM_SPACES; ++s) {
Space &sp = spaces[s];
for (int m = 0; m < sp.num_motors; ++m)
sp.motor[m]->settings.last_v = 0;
}
if (cbs_after_current_move > 0) {
if (!aborting) {
int fragment;
if (num_active_motors == 0)
if (running_fragment == current_fragment) {
//debug("sending movecbs immediately");
send_host(CMD_MOVECB, cbs_after_current_move);
cbs_after_current_move = 0;
return;
}
else
fragment = (current_fragment + FRAGMENTS_PER_BUFFER - 1) % FRAGMENTS_PER_BUFFER;
else
fragment = current_fragment;
//debug("adding %d cbs to final fragment %d", cbs_after_current_move, fragment);
history[fragment].cbs += cbs_after_current_move;
}
//debug("clearing %d cbs after current move in final", cbs_after_current_move);
cbs_after_current_move = 0;
}
}
}
return;
} // }}}
if (t < settings.t0) { // Main part. {{{
double t_fraction = t / settings.t0;
double current_f = (settings.f1 * (2 - t_fraction) + settings.f2 * t_fraction) * t_fraction;
movedebug("main t %f t0 %f tp %f tfrac %f f1 %f f2 %f cf %f", t, settings.t0, settings.tp, t_fraction, settings.f1, settings.f2, current_f);
//debug("main steps");
for (int s = 0; s < NUM_SPACES; ++s) {
if (!settings.single && s == 2)
continue;
Space &sp = spaces[s];
for (int a = 0; a < sp.num_axes; ++a) {
if (isnan(sp.axis[a]->settings.dist[0])) {
sp.axis[a]->settings.target = NAN;
continue;
}
sp.axis[a]->settings.target = sp.axis[a]->settings.source;
}
make_target(sp, current_f, false);
move_axes(&sp, current_time, factor);
}
} // }}}
else { // Connector part. {{{
movedebug("connector %f %f %f", t, settings.t0, settings.tp);
double tc = t - settings.t0;
double t_fraction = tc / settings.tp;
double current_f2 = settings.fp * (2 - t_fraction) * t_fraction;
double current_f3 = settings.fq * t_fraction * t_fraction;
//debug("connect steps");
for (int s = 0; s < NUM_SPACES; ++s) {
if (!settings.single && s == 2)
continue;
Space &sp = spaces[s];
for (int a = 0; a < sp.num_axes; ++a) {
if (isnan(sp.axis[a]->settings.dist[0]) && isnan(sp.axis[a]->settings.dist[1])) {
sp.axis[a]->settings.target = NAN;
continue;
}
sp.axis[a]->settings.target = sp.axis[a]->settings.source;
}
make_target(sp, (1 - settings.fp) + current_f2, false);
make_target(sp, current_f3, true);
move_axes(&sp, current_time, factor);
}
} // }}}
do_steps(factor, current_time);
} // }}}
