void abort_move(int pos) { // {{{
aborting = true;
//debug("abort pos %d", pos);
//debug("abort; cf %d rf %d first %d computing_move %d fragments, regenerating %d ticks", current_fragment, running_fragment, first_fragment, computing_move, pos);
//debug("try aborting move");
current_fragment = running_fragment;
//debug("current abort -> %x", current_fragment);
restore_settings();
#ifdef DEBUG_MOVE
debug("move no longer prepared");
#endif
//debug("free abort reset");
current_fragment_pos = 0;
//if (spaces[0].num_axes > 0)
// fcpdebug(0, 0, "starting hwpos %x", spaces[0].motor[0]->settings.current_pos + avr_pos_offset[0]);
computing_move = true;
//debug("restoring position for fragment %d to position %d", current_fragment, pos);
while (computing_move && current_fragment_pos < pos) {
//debug("tick %d %d %d", current_fragment_pos, settings.hwtime, current_fragment);
apply_tick();
//if (spaces[0].num_axes > 0)
// fcpdebug(0, 0, "current hwpos %x time %d", spaces[0].motor[0]->settings.current_pos + avr_pos_offset[0], settings.hwtime);
}
//if (spaces[0].num_axes > 0)
// fcpdebug(0, 0, "ending hwpos %x", spaces[0].motor[0]->settings.current_pos + avr_pos_offset[0]);
//debug("done restoring position");
// Copy settings back to previous fragment.
store_settings();
computing_move = false;
prepared = false;
current_fragment_pos = 0;
//if (spaces[0].num_axes > 0)
// fcpdebug(0, 0, "final hwpos %x", spaces[0].motor[0]->settings.current_pos + avr_pos_offset[0]);
//debug("curf3 %d", current_fragment);
for (uint8_t s = 0; s < 2; ++s) {
Space &sp = spaces[s];
sp.settings.dist[0] = NAN;
sp.settings.dist[1] = NAN;
for (uint8_t a = 0; a < sp.num_axes; ++a) {
//debug("setting axis %d source to %f", a, sp.axis[a]->settings.current);
sp.axis[a]->settings.source = sp.axis[a]->settings.current;
sp.axis[a]->settings.dist[0] = NAN;
sp.axis[a]->settings.dist[1] = NAN;
}
for (uint8_t m = 0; m < sp.num_motors; ++m) {
sp.motor[m]->settings.last_v = 0;
//debug("setting motor %d pos to %d", m, sp.motor[m]->settings.current_pos);
}
}
//debug("aborted move");
aborting = false;
} // }}}
void abort_move(int pos) { // {{{
aborting = true;
//debug("abort pos %d", pos);
//debug("abort; cf %d rf %d first %d computing_move %d fragments, regenerating %d ticks", current_fragment, running_fragment, first_fragment, computing_move, pos);
//debug("try aborting move");
current_fragment = running_fragment;
//debug("current_fragment = running_fragment; %d", current_fragment);
//debug("current abort -> %x", current_fragment);
while (pos < 0) {
if (current_fragment == first_fragment) {
pos = 0;
}
else {
current_fragment = (current_fragment + FRAGMENTS_PER_BUFFER - 1) % FRAGMENTS_PER_BUFFER;
//debug("current_fragment = (current_fragment + FRAGMENTS_PER_BUFFER - 1) %% FRAGMENTS_PER_BUFFER; %d", current_fragment);
pos += SAMPLES_PER_FRAGMENT;
running_fragment = current_fragment;
}
}
restore_settings();
//debug("free abort reset");
current_fragment_pos = 0;
computing_move = true;
while (computing_move && current_fragment_pos < unsigned(pos)) {
//debug("abort reconstruct %d %d", current_fragment_pos, pos);
apply_tick();
}
if (spaces[0].num_axes > 0)
cpdebug(0, 0, "ending hwpos %f", arch_round_pos(0, 0, spaces[0].motor[0]->settings.current_pos) + avr_pos_offset[0]);
// Flush queue.
settings.queue_start = 0;
settings.queue_end = 0;
settings.queue_full = false;
// Copy settings back to previous fragment.
store_settings();
computing_move = false;
current_fragment_pos = 0;
for (int s = 0; s < NUM_SPACES; ++s) {
Space &sp = spaces[s];
sp.settings.dist[0] = 0;
sp.settings.dist[1] = 0;
for (int a = 0; a < sp.num_axes; ++a) {
//debug("setting axis %d source to %f", a, sp.axis[a]->settings.current);
if (!std::isnan(sp.axis[a]->settings.current))
sp.axis[a]->settings.source = sp.axis[a]->settings.current;
sp.axis[a]->settings.dist[0] = NAN;
sp.axis[a]->settings.dist[1] = NAN;
}
}
mdebug("aborted move");
aborting = false;
} // }}}
void buffer_refill() { // {{{
if (preparing) {
//debug("no refill because prepare");
return;
}
if (moving_to_current == 2)
move_to_current();
if (!computing_move || refilling || stopping || discard_pending || discarding) {
//debug("refill block %d %d %d %d %d", computing_move, refilling, stopping, discard_pending, discarding);
return;
}
refilling = true;
// send_fragment in the previous refill may have failed; try it again.
if (current_fragment_pos > 0)
send_fragment();
//debug("refill start %d %d %d", running_fragment, current_fragment, sending_fragment);
// Keep one free fragment, because we want to be able to rewind and use the buffer before the one currently active.
while (computing_move && !stopping && !discard_pending && !discarding && (running_fragment - 1 - current_fragment + FRAGMENTS_PER_BUFFER) % FRAGMENTS_PER_BUFFER > 4 && !sending_fragment) {
//debug("refill %d %d %f", current_fragment, current_fragment_pos, spaces[0].motor[0]->settings.current_pos);
// fill fragment until full.
apply_tick();
//debug("refill2 %d %f", current_fragment, spaces[0].motor[0]->settings.current_pos);
if (current_fragment_pos >= SAMPLES_PER_FRAGMENT) {
//debug("fragment full %d %d %d", computing_move, current_fragment_pos, BYTES_PER_FRAGMENT);
send_fragment();
}
// Check for commands from host; in case of many short buffers, this loop may not end in a reasonable time.
//serial(0);
}
if (stopping || discard_pending) {
//debug("aborting refill for stopping");
refilling = false;
return;
}
if (!computing_move && current_fragment_pos > 0) {
//debug("finalize");
send_fragment();
}
refilling = false;
arch_start_move(0);
} // }}}
void buffer_refill() { // {{{
// Try to fill the buffer. This is called at any time that the buffer may be refillable.
//debug("refill");
if (aborting || preparing || FRAGMENTS_PER_BUFFER == 0) {
//debug("no refill because prepare or no buffer yet");
return;
}
if (!computing_move || refilling || stopping || discard_pending || discarding) {
//debug("no refill due to block: %d %d %d %d %d", !computing_move, refilling, stopping, discard_pending, discarding);
return;
}
refilling = true;
// send_fragment in the previous refill may have failed; try it again.
/*if (current_fragment_pos > 0) {
debug("sending because data pending frag=%d pos=%d", current_fragment, current_fragment_pos);
send_fragment();
}*/
//debug("refill start %d %d %d", running_fragment, current_fragment, sending_fragment);
// Keep one free fragment, because we want to be able to rewind and use the buffer before the one currently active.
while (computing_move && !aborting && !stopping && !discard_pending && !discarding && (running_fragment - 1 - current_fragment + FRAGMENTS_PER_BUFFER) % FRAGMENTS_PER_BUFFER > (FRAGMENTS_PER_BUFFER > 4 ? 4 : FRAGMENTS_PER_BUFFER - 2) && !sending_fragment) {
//debug("refill %d %d %f", current_fragment, current_fragment_pos, spaces[0].motor[0]->settings.current_pos);
// fill fragment until full.
apply_tick();
//debug("refill2 %d %f", current_fragment, spaces[0].motor[0]->settings.current_pos);
if (current_fragment_pos >= SAMPLES_PER_FRAGMENT) {
//debug("sending because fragment full (weird) %d %d %d", computing_move, current_fragment_pos, BYTES_PER_FRAGMENT);
send_fragment();
}
}
if (aborting || stopping || discard_pending) {
//debug("aborting refill for stopping");
refilling = false;
return;
}
if (!computing_move && current_fragment_pos > 0) {
//debug("sending because move ended");
send_fragment();
}
refilling = false;
arch_start_move(0);
} // }}}
