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;
} // }}}
static bool do_steps(double &factor, int32_t current_time) { // {{{
//debug("steps");
if (factor <= 0) {
movedebug("end move");
return false;
}
// Check if more steps should be done, to detect end of move.
bool have_steps = false;
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];
// Check if there are steps to be done; ignore factor.
double num = (mtr.settings.current_pos / mtr.steps_per_unit + mtr.settings.target_dist);
//debug("num: %f ; current: %f", num, mtr.settings.current_pos);
if (arch_round_pos(s, m, mtr.settings.current_pos) != arch_round_pos(s, m, num * mtr.steps_per_unit)) {
//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 %f %f %f", s, m, mtr.settings.current_pos, mtr.settings.target_dist, factor);
}
}
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);
int32_t the_last_time = settings.last_current_time;
settings.last_current_time = current_time;
// Adjust start time if factor < 1.
if (factor < 1) {
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);
double new_cp = target * mtr.steps_per_unit;
if (arch_round_pos(s, m, mtr.settings.current_pos) != arch_round_pos(s, m, new_cp)) {
have_steps = true;
if (!mtr.active) {
mtr.active = true;
num_active_motors += 1;
}
int diff = arch_round_pos(s, m, new_cp) - arch_round_pos(s, m, mtr.settings.current_pos);
DATA_SET(s, m, diff);
}
//debug("new cp: %d %d %f %d", s, m, new_cp, current_fragment_pos);
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 &= 0x7f;
if (fs != s || fm != m || (fs == 2 && fm >= mm))
continue;
//debug("follow %d %d %d %d %d %f %f", s, m, fs, fm, mm, new_cp, mtr.settings.current_pos);
spaces[2].motor[mm]->settings.current_pos += 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;
} // }}}
// Movement handling. {{{
static void check_distance(int sp, int mt, Motor *mtr, double distance, double dt, double &factor) { // {{{
if (dt == 0) {
factor = 0;
return;
}
if (isnan(distance) || distance == 0) {
mtr->settings.target_dist = 0;
//mtr->last_v = 0;
return;
}
//debug("cd %f %f", distance, dt);
mtr->settings.target_dist = distance;
mtr->settings.target_v = distance / dt;
double v = fabs(mtr->settings.target_v);
int s = (mtr->settings.target_v < 0 ? -1 : 1);
// When turning around, ignore limits (they shouldn't have been violated anyway).
if (mtr->settings.last_v * s < 0) {
//debug("!");
mtr->settings.last_v = 0;
}
// Limit v.
if (v > mtr->limit_v) {
//debug("v %f limit %f", v, mtr->limit_v);
distance = (s * mtr->limit_v) * dt;
v = fabs(distance / dt);
}
//debug("cd2 %f %f", distance, dt);
// Limit a+.
double limit_dv = mtr->limit_a * dt;
if (v - mtr->settings.last_v * s > limit_dv) {
//debug("a+ target v %f limit dv %f last v %f s %d", mtr->settings.target_v, limit_dv, mtr->settings.last_v, s);
distance = (limit_dv * s + mtr->settings.last_v) * dt;
v = fabs(distance / dt);
}
//debug("cd3 %f %f", distance, dt);
// Limit a-.
// Distance to travel until end of segment or connection.
double max_dist = (mtr->settings.endpos - mtr->settings.current_pos / mtr->steps_per_unit) * s;
// Find distance traveled when slowing down at maximum a.
// x = 1/2 at²
// t = sqrt(2x/a)
// v = at
// v² = 2a²x/a = 2ax
// x = v²/2a
double limit_dist = v * v / 2 / mtr->limit_a;
//debug("max %f limit %f v %f a %f", max_dist, limit_dist, v, mtr->limit_a);
if (max_dist > 0 && limit_dist > max_dist) {
//debug("a- endpos %f limit a %f limit dist %f max dist %f v %f distance %f current pos %f s %d dt %f", mtr->settings.endpos, mtr->limit_a, limit_dist, max_dist, v, distance, mtr->settings.current_pos, s, dt);
v = sqrt(max_dist * 2 * mtr->limit_a);
distance = s * v * dt;
}
//debug("cd4 %f %f", distance, dt); */
int steps = arch_round_pos(sp, mt, mtr->settings.current_pos + distance * mtr->steps_per_unit) - round(mtr->settings.current_pos);
int targetsteps = steps;
//cpdebug(s, m, "cf %d value %d", current_fragment, value);
if (settings.probing && steps)
steps = s;
else {
// Maximum depends on number of subfragments. Be conservative and use 0x7e per subfragment; assume 128 subfragments (largest power of 2 smaller than 10000/75)
// TODO: 10000 and 75 should follow the actual values for step_time in cdriver and TIME_PER_ISR in firmware.
int max = 0x7e << 7;
if (abs(steps) > max) {
debug("overflow %d from cp %f dist %f steps/mm %f dt %f s %d max %d", steps, mtr->settings.current_pos, distance, mtr->steps_per_unit, dt, s, max);
steps = max * s;
}
}
if (abs(steps) < abs(targetsteps)) {
distance = (arch_round_pos(sp, mt, mtr->settings.current_pos) + steps + s * .5 - mtr->settings.current_pos) / mtr->steps_per_unit;
v = fabs(distance / dt);
}
//debug("=============");
double f = distance / mtr->settings.target_dist;
//debug("checked %f %f %f %f", mtr->settings.target_dist, distance, factor, f);
if (f < factor)
factor = f;
} // }}}
static void do_steps() { // {{{
// Do the steps to arrive at the correct position. Update axis and motor current positions.
// Set new current position.
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 (!std::isnan(sp.axis[a]->settings.target))
sp.axis[a]->settings.current = sp.axis[a]->settings.target;
}
}
// 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];
double target = mtr.settings.target_pos;
if (std::isnan(target))
continue;
cpdebug(s, m, "ccp3 stopping %d target %f frag %d", stopping, target, current_fragment);
double rounded_cp = arch_round_pos(s, m, mtr.settings.current_pos);
double rounded_new_cp = arch_round_pos(s, m, target);
if (rounded_cp != rounded_new_cp) {
if (!mtr.active) {
mtr.active = true;
num_active_motors += 1;
//debug("activating motor %d %d", s, m);
}
int diff = round((rounded_new_cp - rounded_cp) * mtr.steps_per_unit);
if (diff > 0x7f) {
debug("Error: trying to send more than 127 steps: %d", diff);
int adjust = diff - 0x7f;
settings.hwtime -= settings.hwtime_step * (adjust / diff);
diff = 0x7f;
target -= adjust;
mtr.settings.target_pos = target;
}
if (diff < -0x80) {
debug("Error: trying to send more than 128 steps: %d", -diff);
int adjust = diff + 0x80;
settings.hwtime -= settings.hwtime_step * (adjust / diff);
diff = -0x80;
target -= adjust;
mtr.settings.target_pos = target;
}
//debug("sending %d %d steps %d", s, m, diff);
DATA_SET(s, m, diff);
}
//debug("new cp: %d %d %f %d", s, m, target, current_fragment_pos);
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 &= 0x7f;
if (fs != s || fm != m || (fs == 2 && fm >= mm))
continue;
//debug("follow %d %d %d %d %d %f %f", s, m, fs, fm, mm, target, mtr.settings.current_pos);
spaces[2].motor[mm]->settings.current_pos += target - mtr.settings.current_pos;
}
}
mtr.settings.current_pos = target;
cpdebug(s, m, "cp three %f", target);
mtr.settings.last_v = mtr.settings.target_v;
}
}
int pattern_pos = (settings.hwtime - settings.hwtime_step / 2) / settings.hwtime_step;
//debug("time %d step %d pattern pos %d size %d", settings.hwtime, settings.hwtime_step, pattern_pos, settings.pattern_size);
if (pattern_pos < settings.pattern_size * 8) {
if (!pattern.active) {
pattern.active = true;
num_active_motors += 1;
//debug("activating pattern");
}
//debug("set pattern for %d at %d to %d", current_fragment, current_fragment_pos, settings.pattern[pattern_pos]);
PATTERN_SET(settings.pattern[pattern_pos]);
}
current_fragment_pos += 1;
if (current_fragment_pos >= SAMPLES_PER_FRAGMENT) {
//debug("sending because fragment full (normal) %d %d %d", computing_move, current_fragment_pos, BYTES_PER_FRAGMENT);
send_fragment();
}
} // }}}
static void check_distance(int sp, int mt, Motor *mtr, Motor *limit_mtr, double distance, double dt, double &factor) { // {{{
// Check motor limits for motor (sp,mt), which is mtr. For followers, limit_mtr is set to its leader unless settings.single is true.
// distance is the distance to travel; dt is the time, factor is lowered if needed.
//debug("check distance %d %d dist %f t %f current %f time step %d", sp, mt, distance, dt, mtr->settings.current_pos, settings.hwtime_step);
if (dt == 0) {
debug("check distance for 0 time");
factor = 0;
return;
}
if (std::isnan(distance) || distance == 0) {
mtr->settings.last_v = 0;
return;
}
double orig_distance = distance;
//debug("cd %f %f", distance, dt);
mtr->settings.target_v = distance / dt;
double v = std::fabs(mtr->settings.target_v);
int s = (mtr->settings.target_v < 0 ? -1 : 1);
// When turning around, ignore limits (they shouldn't have been violated anyway).
if (mtr->settings.last_v * s < 0) {
//debug("direction change");
mtr->settings.last_v = 0;
}
// Limit v.
if (v > limit_mtr->limit_v) {
debug("%d %d v %f limit %f dist %f t %f current %f factor %f", sp, mt, v, limit_mtr->limit_v, distance, dt, mtr->settings.current_pos, settings.factor);
distance = (s * limit_mtr->limit_v) * dt;
v = std::fabs(distance / dt);
}
// Limit a+.
double limit_dv = limit_mtr->limit_a * dt;
if (v - mtr->settings.last_v * s > limit_dv) {
debug("a+ %d %d target v %f limit dv %f last v %f s %d current %f factor %f", sp, mt, mtr->settings.target_v, limit_dv, mtr->settings.last_v, s, mtr->settings.current_pos, settings.factor);
distance = (limit_dv * s + mtr->settings.last_v) * dt;
v = std::fabs(distance / dt);
}
int steps = round((arch_round_pos(sp, mt, mtr->settings.current_pos + distance) - arch_round_pos(sp, mt, mtr->settings.current_pos)) * mtr->steps_per_unit);
int targetsteps = steps;
//cpdebug(s, m, "cf %d value %d", current_fragment, value);
if (settings.probing && steps)
steps = s;
else {
int max = 0x78; // Don't use 0x7f, because limiting isn't accurate.
if (abs(steps) > max) {
debug("overflow %d from cp %f dist %f steps/mm %f dt %f s %d max %d", steps, mtr->settings.current_pos, distance, mtr->steps_per_unit, dt, s, max);
steps = max * s;
}
}
if (abs(steps) < abs(targetsteps)) {
distance = arch_round_pos(sp, mt, mtr->settings.current_pos) + (steps + s * .5) / mtr->steps_per_unit - mtr->settings.current_pos;
v = std::fabs(distance / dt);
debug("new distance = %f, old = %f", distance, orig_distance);
}
//debug("=============");
double f = distance / orig_distance;
//debug("checked %d %d src %f target %f target dist %f new dist %f old factor %f new factor %f", sp, mt, mtr->settings.current_pos, mtr->settings.target_pos, orig_distance, distance, factor, f);
if (f < factor)
factor = f;
// Store adjusted value of v.
mtr->settings.target_v = s * v;
} // }}}
