static void move_axes(Space *s, uint32_t current_time, double &factor) { // {{{ #ifdef DEBUG_PATH fprintf(stderr, "%d\t%d", current_time, s->id); for (int a = 0; a < s->num_axes; ++a) { if (isnan(s->axis[a]->settings.target)) fprintf(stderr, "\t%f", s->axis[a]->settings.source); else fprintf(stderr, "\t%f", s->axis[a]->settings.target); } fprintf(stderr, "\n"); #endif double motors_target[s->num_motors]; bool ok = true; space_types[s->type].xyz2motors(s, motors_target, &ok); // Try again if it didn't work; it should have moved target to a better location. if (!ok) { space_types[s->type].xyz2motors(s, motors_target, &ok); movedebug("retried move"); } //movedebug("ok %d", ok); for (uint8_t m = 0; m < s->num_motors; ++m) { movedebug("move %d %f %f", m, motors_target[m], s->motor[m]->settings.current_pos / s->motor[m]->steps_per_unit); double distance = motors_target[m] - s->motor[m]->settings.current_pos / s->motor[m]->steps_per_unit; check_distance(s->motor[m], distance, (current_time - settings.last_time) / 1e6, factor); } } // }}}
static void move_axes(Space *s, uint32_t current_time, double &factor) { // {{{ double motors_target[s->num_motors]; bool ok = true; space_types[s->type].xyz2motors(s, motors_target, &ok); // Try again if it didn't work; it should have moved target to a better location. if (!ok) { space_types[s->type].xyz2motors(s, motors_target, &ok); movedebug("retried move"); } //movedebug("ok %d", ok); for (int m = 0; m < s->num_motors; ++m) { //if (s->id == 0 && m == 0) //debug("check move %d %d target %f current %f", s->id, m, motors_target[m], s->motor[m]->settings.current_pos / s->motor[m]->steps_per_unit); double distance = motors_target[m] - s->motor[m]->settings.current_pos / s->motor[m]->steps_per_unit; check_distance(s->motor[m], distance, (current_time - settings.last_time) / 1e6, factor); } } // }}}
void make_target(Space &sp, double f, bool next) { // {{{ int a = 0; if (sp.settings.arc[next]) { // Convert time-fraction into angle-fraction. f = (2 * sp.settings.radius[next][0] * f) / (sp.settings.radius[next][0] + sp.settings.radius[next][1] - (sp.settings.radius[next][1] - sp.settings.radius[next][0]) * f); double angle = sp.settings.angle[next] * f; double radius = sp.settings.radius[next][0] + (sp.settings.radius[next][1] - sp.settings.radius[next][0]) * f; double helix = sp.settings.helix[next] * f; double cosa = cos(angle); double sina = sin(angle); //debug("e1 %f %f %f e2 %f %f %f", sp.settings.e1[next][0], sp.settings.e1[next][1], sp.settings.e1[next][2], sp.settings.e2[next][0], sp.settings.e2[next][1], sp.settings.e2[next][2]); for (int i = 0; i < min(3, sp.num_axes); ++i) sp.axis[i]->settings.target += radius * cosa * sp.settings.e1[next][i] + radius * sina * sp.settings.e2[next][i] - sp.settings.offset[next][i] + helix * sp.settings.normal[next][i]; a = 3; // Fall through. } for (; a < sp.num_axes; ++a) { sp.axis[a]->settings.target += sp.axis[a]->settings.dist[next] * f; movedebug("do %d %d %f %f", sp.id, a, sp.axis[a]->settings.dist[0], sp.axis[a]->settings.target); } } // }}}
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; } // }}}
// Movement handling. {{{ static void check_distance(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); int8_t 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) { movedebug("!"); mtr->settings.last_v = 0; } // Limit v. if (v > mtr->limit_v) { movedebug("v %f %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) { movedebug("a+ %f %f %f %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) { movedebug("a- %f %f %f %d %d %f", mtr->settings.endpos, mtr->limit_a, max_dist, 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 = distance * mtr->steps_per_unit; //cpdebug(s, m, "cp %d hwcp %d cf %d value %d", mtr.settings.current_pos, mtr.settings.current_pos, current_fragment, value); if (settings.probing && steps) steps = s; else { if (abs(steps) > 8 * mtr->max_steps) // TODO: fix 8. steps = s * 8 * mtr->max_steps; if (abs(steps) > 0x7f) { debug("overflow %d", steps); steps = 0x7f * s; } } if (steps != int(distance * mtr->steps_per_unit)) { distance = steps / mtr->steps_per_unit; v = fabs(distance / dt); } //debug("move pos %d %d cf %d time %d cp %d hwcp %d diff %d value %d", s, m, current_fragment, settings.hwtime, sp.motor[m]->settings.current_pos + avr_pos_offset[m], sp.motor[m]->settings.current_pos + avr_pos_offset[m], sp.motor[m]->settings.current_pos - sp.motor[m]->settings.current_pos, value); //debug("============="); double f = distance / mtr->settings.target_dist; movedebug("checked %f %f", mtr->settings.target_dist, distance); if (f < factor) factor = f; } // }}}
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); } // }}}