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);
} // }}}
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 void apply_tick() { // {{{
// Move motors to position for next time tick.
// If it exceeds limits, adjust hwtime so that it's acceptable.
//debug("tick");
if (current_fragment_pos >= SAMPLES_PER_FRAGMENT) {
// Fragment is already full. This shouldn't normally happen.
debug("aborting apply_tick, because fragment is already full.");
return;
}
// Check for move.
if (!computing_move) {
mdebug("apply tick called, but not moving");
return;
}
mdebug("handling %d %d", computing_move, cbs_after_current_move);
// This loop is normally only run once, but when a move is complete it is rerun for the next move.
while ((running_fragment - 1 - current_fragment + FRAGMENTS_PER_BUFFER) % FRAGMENTS_PER_BUFFER > (FRAGMENTS_PER_BUFFER > 4 ? 4 : FRAGMENTS_PER_BUFFER - 2)) {
settings.hwtime += settings.hwtime_step;
//debug("tick time %d step %d frag %d pos %d", settings.hwtime, settings.hwtime_step, current_fragment, current_fragment_pos);
double t = settings.hwtime / 1e6;
double target_factor; // Factor of current move that should be completed at this time.
if (settings.hwtime >= settings.end_time)
target_factor = 1;
else if (settings.hwtime <= 0)
target_factor = 0;
else {
target_factor = ((settings.v1 - settings.v0) / (settings.end_time / 1e6) * t * t / 2 + settings.v0 * t) / settings.dist;
if (target_factor > 1)
target_factor = 1;
if (target_factor < 0)
target_factor = 0;
}
//debug("target factor: %f (t=%f end=%f)", target_factor, t, settings.end_time / 1e6);
// Go straight to the next move if the distance was 0 (so target_factor is NaN).
if (!std::isnan(target_factor)) {
double f = set_targets(target_factor);
if (f < 1) {
//double old = target_factor;
if (settings.factor > 0 || settings.hwtime <= 0)
target_factor = settings.factor + f * (target_factor - settings.factor);
else {
// settings.factor == 0, so we're at the start of a move.
// settings.hwtime > 0, so this must be a continuation.
// Only the new part can be limited, so to limit the same distance, it needs to be a larger fraction.
// Example:
// hwtime_step = 10
// hwtime = 6
// -> newpart = 0.6
// f = 0.9
// target move = 100
// so (1-0.9)*100=10 of target move must be removed
// that is (1-0.9)/0.6 of new part.
double newpart = settings.hwtime * 1. / settings.hwtime_step;
f = 1 - (1 - f) / newpart;
target_factor = f * target_factor;
}
if (target_factor < 0)
target_factor = 0;
else if (target_factor > 1)
target_factor = 1;
//debug("adjust factor (%f) from %f to %f because f=%f", settings.factor, old, target_factor, f);
set_targets(target_factor);
// Adjust time.
settings.hwtime -= settings.hwtime_step * ((1 - f) * .99);
}
//debug("target factor %f time 0 -> %d -> %d v %f -> %f", target_factor, settings.hwtime, settings.end_time, settings.v0, settings.v1);
settings.factor = target_factor;
if (settings.factor < 1) {
do_steps();
return;
}
}
//debug("next segment");
// Set new sources for all axes.
for (int s = 0; s < NUM_SPACES; ++s) {
Space &sp = spaces[s];
for (int a = 0; a < sp.num_axes; ++a) {
auto ax = sp.axis[a];
ax->settings.source = ax->settings.target;
}
}
// start new move; adjust time.
history[current_fragment].cbs += cbs_after_current_move;
//debug("adding %d cbs because move is completed", cbs_after_current_move);
cbs_after_current_move = next_move(settings.end_time);
//debug("new pending cbs: %d", cbs_after_current_move);
mdebug("next move prepared");
if (!computing_move) {
// There is no next move.
continue_event = true;
do_steps();
return;
}
mdebug("try again");
// Next loop the time is incremented again, but in this case that shouldn't happen, so compensate.
settings.hwtime -= settings.hwtime_step;
}
//if (spaces[0].num_axes >= 2)
//debug("move z %d %d %f %f %f", current_fragment, current_fragment_pos, spaces[0].axis[2]->settings.current, spaces[0].motor[0]->settings.current_pos, spaces[0].motor[0]->settings.current_pos + avr_pos_offset[0]);
} // }}}
