void Space::load_motor(uint8_t m, int32_t &addr) { // {{{
loaddebug("loading motor %d", m);
uint16_t enable = motor[m]->enable_pin.write();
double old_home_pos = motor[m]->home_pos;
double old_steps_per_unit = motor[m]->steps_per_unit;
motor[m]->step_pin.read(read_16(addr));
motor[m]->dir_pin.read(read_16(addr));
motor[m]->enable_pin.read(read_16(addr));
motor[m]->limit_min_pin.read(read_16(addr));
motor[m]->limit_max_pin.read(read_16(addr));
motor[m]->sense_pin.read(read_16(addr));
motor[m]->steps_per_unit = read_float(addr);
motor[m]->max_steps = read_8(addr);
motor[m]->home_pos = read_float(addr);
motor[m]->limit_v = read_float(addr);
motor[m]->limit_a = read_float(addr);
motor[m]->home_order = read_8(addr);
arch_motors_change();
SET_OUTPUT(motor[m]->enable_pin);
if (enable != motor[m]->enable_pin.write()) {
if (motors_busy)
SET(motor[m]->enable_pin);
else {
RESET(motor[m]->enable_pin);
}
}
RESET(motor[m]->step_pin);
SET_INPUT(motor[m]->limit_min_pin);
SET_INPUT(motor[m]->limit_max_pin);
SET_INPUT(motor[m]->sense_pin);
bool must_move = false;
if (!isnan(motor[m]->home_pos)) {
// Axes with a limit switch.
if (motors_busy && (old_home_pos != motor[m]->home_pos || old_steps_per_unit != motor[m]->steps_per_unit) && !isnan(old_home_pos)) {
int32_t hp = motor[m]->home_pos * motor[m]->steps_per_unit + (motor[m]->home_pos > 0 ? .49 : -.49);
int32_t ohp = old_home_pos * old_steps_per_unit + (old_home_pos > 0 ? .49 : -.49);
int32_t diff = hp - ohp;
motor[m]->settings.current_pos += diff;
cpdebug(id, m, "load motor new home add %d", diff);
arch_addpos(id, m, diff);
must_move = true;
}
}
else {
// Axes without a limit switch, including extruders.
if (motors_busy && old_steps_per_unit != motor[m]->steps_per_unit) {
int32_t cp = motor[m]->settings.current_pos;
double pos = cp / old_steps_per_unit;
cpdebug(id, m, "load motor new steps no home");
motor[m]->settings.current_pos = pos * motor[m]->steps_per_unit;
int diff = motor[m]->settings.current_pos - cp;
arch_addpos(id, m, diff);
}
}
if (must_move)
move_to_current();
} // }}}
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 void eload(Space *s, uint8_t old_type, int32_t &addr) {
uint8_t num = read_8(addr);
if (!s->setup_nums(num, num)) {
debug("Failed to set up extruder axes");
uint8_t n = min(s->num_axes, s->num_motors);
if (!s->setup_nums(n, n)) {
debug("Trouble! Failed to abort. Cancelling.");
s->cancel_update();
}
}
for (int a = EDATA(s).num_axes; a < s->num_axes; ++a) {
s->axis[a]->type_data = new ExtruderAxisData;
for (int i = 0; i < 3; ++i)
EADATA(s, a).offset[i] = 0;
}
EDATA(s).num_axes = s->num_axes;
bool move = false;
if (motors_busy && !computing_move && settings.queue_start == settings.queue_end && !settings.queue_full) {
move = true;
queue[settings.queue_end].probe = false;
queue[settings.queue_end].cb = false;
queue[settings.queue_end].f[0] = INFINITY;
queue[settings.queue_end].f[1] = INFINITY;
for (int i = 0; i < spaces[0].num_axes; ++i) {
queue[settings.queue_end].data[i] = spaces[0].axis[i]->settings.current;
for (int ss = 0; ss < 2; ++ss)
queue[settings.queue_end].data[i] = space_types[spaces[ss].type].unchange0(&spaces[ss], i, queue[settings.queue_end].data[i]);
if (i == 2)
queue[settings.queue_end].data[i] -= zoffset;
}
for (int i = spaces[0].num_axes; i < QUEUE_LENGTH; ++i) {
queue[settings.queue_end].data[i] = NAN;
}
cpdebug(0, 0, "eload end");
settings.queue_end = (settings.queue_end + 1) % QUEUE_LENGTH;
// This shouldn't happen and causes communication problems, but if you have a 1-item buffer it is correct.
if (settings.queue_end == settings.queue_start)
settings.queue_full = true;
}
for (int a = 0; a < s->num_axes; ++a) {
for (int o = 0; o < 3; ++o)
EADATA(s, a).offset[o] = read_float(addr);
}
if (move) {
next_move();
buffer_refill();
}
}
void move_to_current() { // {{{
if (computing_move || !motors_busy) {
if (moving_to_current == 0)
moving_to_current = 1;
return;
}
moving_to_current = 0;
debug("move to current");
settings.f0 = 0;
settings.fmain = 1;
settings.fp = 0;
settings.fq = 0;
settings.t0 = 0;
settings.tp = 0;
cbs_after_current_move = 0;
current_fragment_pos = 0;
first_fragment = current_fragment;
computing_move = true;
settings.cbs = 0;
settings.hwtime = 0;
settings.start_time = 0;
settings.last_time = 0;
settings.last_current_time = 0;
for (uint8_t s = 0; s < 2; ++s) {
Space &sp = spaces[s];
sp.settings.dist[0] = 0;
sp.settings.dist[1] = 0;
for (uint8_t a = 0; a < sp.num_axes; ++a) {
cpdebug(s, a, "using current %f", sp.axis[a]->settings.current);
sp.axis[a]->settings.source = sp.axis[a]->settings.current;
sp.axis[a]->settings.target = sp.axis[a]->settings.current;
sp.axis[a]->settings.dist[0] = 0;
sp.axis[a]->settings.dist[1] = 0;
sp.axis[a]->settings.main_dist = 0;
}
for (uint8_t m = 0; m < sp.num_motors; ++m)
sp.motor[m]->settings.last_v = 0;
}
#ifdef DEBUG_PATH
fprintf(stderr, "\n");
#endif
buffer_refill();
} // }}}
void move_to_current() { // {{{
if (computing_move || !motors_busy) {
if (moving_to_current == 0)
moving_to_current = 1;
return;
}
moving_to_current = 0;
//debug("move to current");
settings.f0 = 0;
settings.fmain = 1;
settings.fp = 0;
settings.fq = 0;
settings.t0 = 0;
settings.tp = 0;
//debug("clearing %d cbs after current move for move to current", cbs_after_current_move);
cbs_after_current_move = 0;
current_fragment_pos = 0;
first_fragment = current_fragment;
computing_move = true;
settings.cbs = 0;
settings.hwtime = 0;
settings.start_time = 0;
settings.last_time = 0;
settings.last_current_time = 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) {
cpdebug(s, a, "using current %f", sp.axis[a]->settings.current);
sp.axis[a]->settings.source = sp.axis[a]->settings.current;
sp.axis[a]->settings.target = sp.axis[a]->settings.current;
sp.axis[a]->settings.dist[0] = 0;
sp.axis[a]->settings.dist[1] = 0;
sp.axis[a]->settings.main_dist = 0;
}
for (int m = 0; m < sp.num_motors; ++m)
sp.motor[m]->settings.last_v = 0;
}
buffer_refill();
} // }}}
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;
} // }}}
void restore_settings() { // {{{
current_fragment_pos = 0;
num_active_motors = 0;
settings.t0 = history[current_fragment].t0;
settings.tp = history[current_fragment].tp;
settings.f0 = history[current_fragment].f0;
settings.f1 = history[current_fragment].f1;
settings.f2 = history[current_fragment].f2;
settings.fp = history[current_fragment].fp;
settings.fq = history[current_fragment].fq;
settings.fmain = history[current_fragment].fmain;
history[current_fragment].cbs = 0;
settings.hwtime = history[current_fragment].hwtime;
settings.start_time = history[current_fragment].start_time;
settings.last_time = history[current_fragment].last_time;
settings.last_current_time = history[current_fragment].last_current_time;
settings.queue_start = history[current_fragment].queue_start;
settings.queue_end = history[current_fragment].queue_end;
settings.queue_full = history[current_fragment].queue_full;
settings.run_file_current = history[current_fragment].run_file_current;
settings.run_time = history[current_fragment].run_time;
settings.run_dist = history[current_fragment].run_dist;
for (int s = 0; s < NUM_SPACES; ++s) {
Space &sp = spaces[s];
sp.settings.dist[0] = sp.history[current_fragment].dist[0];
sp.settings.dist[1] = sp.history[current_fragment].dist[1];
for (int i = 0; i < 2; ++i) {
sp.settings.arc[i] = sp.history[current_fragment].arc[i];
sp.settings.angle[i] = sp.history[current_fragment].angle[i];
sp.settings.helix[i] = sp.history[current_fragment].helix[i];
for (int t = 0; t < 2; ++t)
sp.settings.radius[i][t] = sp.history[current_fragment].radius[i][t];
for (int t = 0; t < 3; ++t) {
sp.settings.offset[i][t] = sp.history[current_fragment].offset[i][t];
sp.settings.e1[i][t] = sp.history[current_fragment].e1[i][t];
sp.settings.e2[i][t] = sp.history[current_fragment].e2[i][t];
sp.settings.normal[i][t] = sp.history[current_fragment].normal[i][t];
}
}
for (int m = 0; m < sp.num_motors; ++m) {
sp.motor[m]->active = false;
DATA_CLEAR(s, m);
sp.motor[m]->settings.last_v = sp.motor[m]->history[current_fragment].last_v;
sp.motor[m]->settings.target_v = sp.motor[m]->history[current_fragment].target_v;
sp.motor[m]->settings.target_dist = sp.motor[m]->history[current_fragment].target_dist;
sp.motor[m]->settings.current_pos = sp.motor[m]->history[current_fragment].current_pos;
sp.motor[m]->settings.endpos = sp.motor[m]->history[current_fragment].endpos;
cpdebug(s, m, "restore");
}
for (int a = 0; a < sp.num_axes; ++a) {
sp.axis[a]->settings.dist[0] = sp.axis[a]->history[current_fragment].dist[0];
sp.axis[a]->settings.dist[1] = sp.axis[a]->history[current_fragment].dist[1];
sp.axis[a]->settings.main_dist = sp.axis[a]->history[current_fragment].main_dist;
sp.axis[a]->settings.target = sp.axis[a]->history[current_fragment].target;
sp.axis[a]->settings.source = sp.axis[a]->history[current_fragment].source;
sp.axis[a]->settings.current = sp.axis[a]->history[current_fragment].current;
sp.axis[a]->settings.endpos[0] = sp.axis[a]->history[current_fragment].endpos[0];
sp.axis[a]->settings.endpos[1] = sp.axis[a]->history[current_fragment].endpos[1];
}
}
} // }}}
void restore_settings() { // {{{
current_fragment_pos = 0;
num_active_motors = 0;
if (FRAGMENTS_PER_BUFFER == 0)
return;
for (int i = 0; i < 3; ++i) {
settings.P[i] = history[current_fragment].P[i];
settings.A[i] = history[current_fragment].A[i];
settings.B[i] = history[current_fragment].B[i];
}
settings.v0 = history[current_fragment].v0;
settings.v1 = history[current_fragment].v1;
settings.dist = history[current_fragment].dist;
settings.alpha_max = history[current_fragment].alpha_max;
settings.hwtime = history[current_fragment].hwtime;
settings.hwtime_step = history[current_fragment].hwtime_step;
settings.end_time = history[current_fragment].end_time;
history[current_fragment].cbs = 0;
settings.queue_start = history[current_fragment].queue_start;
settings.queue_end = history[current_fragment].queue_end;
settings.queue_full = history[current_fragment].queue_full;
settings.run_file_current = history[current_fragment].run_file_current;
settings.probing = history[current_fragment].probing;
settings.single = history[current_fragment].single;
settings.run_time = history[current_fragment].run_time;
settings.run_dist = history[current_fragment].run_dist;
settings.factor = history[current_fragment].factor;
settings.pattern_size = history[current_fragment].pattern_size;
for (int i = 0; i < PATTERN_MAX; ++i)
settings.pattern[i] = history[current_fragment].pattern[i];
for (int s = 0; s < NUM_SPACES; ++s) {
Space &sp = spaces[s];
sp.settings.dist[0] = sp.history[current_fragment].dist[0];
sp.settings.dist[1] = sp.history[current_fragment].dist[1];
for (int i = 0; i < 2; ++i) {
sp.settings.arc[i] = sp.history[current_fragment].arc[i];
sp.settings.angle[i] = sp.history[current_fragment].angle[i];
sp.settings.helix[i] = sp.history[current_fragment].helix[i];
for (int t = 0; t < 2; ++t)
sp.settings.radius[i][t] = sp.history[current_fragment].radius[i][t];
for (int t = 0; t < 3; ++t) {
sp.settings.offset[i][t] = sp.history[current_fragment].offset[i][t];
sp.settings.e1[i][t] = sp.history[current_fragment].e1[i][t];
sp.settings.e2[i][t] = sp.history[current_fragment].e2[i][t];
sp.settings.normal[i][t] = sp.history[current_fragment].normal[i][t];
}
}
for (int m = 0; m < sp.num_motors; ++m) {
sp.motor[m]->active = false;
sp.motor[m]->settings.target_v = sp.motor[m]->history[current_fragment].target_v;
sp.motor[m]->settings.target_pos = sp.motor[m]->history[current_fragment].target_pos;
sp.motor[m]->settings.current_pos = sp.motor[m]->history[current_fragment].current_pos;
cpdebug(s, m, "restore");
}
for (int a = 0; a < sp.num_axes; ++a) {
sp.axis[a]->settings.dist[0] = sp.axis[a]->history[current_fragment].dist[0];
sp.axis[a]->settings.dist[1] = sp.axis[a]->history[current_fragment].dist[1];
sp.axis[a]->settings.main_dist = sp.axis[a]->history[current_fragment].main_dist;
sp.axis[a]->settings.target = sp.axis[a]->history[current_fragment].target;
sp.axis[a]->settings.source = sp.axis[a]->history[current_fragment].source;
sp.axis[a]->settings.current = sp.axis[a]->history[current_fragment].current;
sp.axis[a]->settings.endpos = sp.axis[a]->history[current_fragment].endpos;
}
}
pattern.active = false;
DATA_CLEAR();
} // }}}
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();
}
} // }}}