static void copy_ordinary_char_or_ESC_sequence (const char **original, char **copy) { if (**original != ESCAPE || ! matches ("[]", *(*original + 1))) { copy_next(1, original, copy); return; /* not an ESC[ sequence */ } *(*copy)++ = RL_PROMPT_START_IGNORE; copy_next(2, original, copy); match_and_copy(";0123456789", original, copy); match_and_copy("m", original, copy); *(*copy)++ = RL_PROMPT_END_IGNORE; }
// Used from previous segment (if prepared): tp, vq. uint8_t next_move() { // {{{ settings.probing = false; moving_to_current = 0; uint8_t num_cbs = 0; uint8_t a0; run_file_fill_queue(); if (settings.queue_start == settings.queue_end && !settings.queue_full) { //debug("no next move"); computing_move = false; prepared = false; return num_cbs; } #ifdef DEBUG_MOVE debug("Next move; queue start = %d, end = %d", settings.queue_start, settings.queue_end); #endif // Set everything up for running queue[settings.queue_start]. uint8_t n = (settings.queue_start + 1) % QUEUE_LENGTH; // Make sure printer state is good. {{{ // If the source is unknown, determine it from current_pos. //for (uint8_t a = 0; a < num_axes; ++a) // debug("target %d %f", a, queue[settings.queue_start].data[a]); 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.source)) { space_types[sp.type].reset_pos(&sp); for (uint8_t aa = 0; aa < sp.num_axes; ++aa) sp.axis[aa]->settings.current = sp.axis[aa]->settings.source; break; } #ifdef DEBUG_MOVE else debug("non-nan: %d %d %f %d", s, a, sp.axis[a]->settings.source, sp.motor[a]->settings.current_pos); #endif } } // }}} settings.f0 = settings.fq; // If no move is prepared, set dist[1] from the queue; it will be used as dist[0] below. {{{ if (!prepared) { #ifdef DEBUG_MOVE debug("No move prepared."); #endif settings.f0 = 0; a0 = 0; change0(settings.queue_start); for (uint8_t s = 0; s < 2; ++s) { Space &sp = spaces[s]; space_types[sp.type].check_position(&sp, &queue[settings.queue_start].data[a0]); sp.settings.dist[0] = 0; for (int a = 0; a < sp.num_axes; ++a) { sp.axis[a]->settings.dist[0] = 0; sp.axis[a]->settings.endpos[0] = sp.axis[a]->settings.source; } set_from_queue(s, settings.queue_start, a0, false); a0 += sp.num_axes; } } // }}} // Fill unspecified coordinates with previous values. {{{ a0 = 0; for (uint8_t s = 0; s < 2; ++s) { Space &sp = spaces[s]; for (uint8_t a = 0; a < sp.num_axes; ++a) { if (n != settings.queue_end) { // If only one of them is set, set the other one as well to make the rounded corner work. if (!isnan(queue[settings.queue_start].data[a0 + a]) && isnan(queue[n].data[a0 + a])) { queue[n].data[a0 + a] = sp.axis[a]->settings.source + sp.axis[a]->settings.dist[1] - (s == 0 && a == 2 ? zoffset : 0); #ifdef DEBUG_MOVE debug("filling next %d with %f", a0 + a, queue[n].data[a0 + a]); #endif } if (isnan(queue[settings.queue_start].data[a]) && !isnan(queue[n].data[a])) { queue[settings.queue_start].data[a0 + a] = sp.axis[a]->settings.source; #ifdef DEBUG_MOVE debug("filling %d with %f", a0 + a, queue[settings.queue_start].data[a0 + a]); #endif } } if ((!isnan(queue[settings.queue_start].data[a0 + a]) || (n != settings.queue_end && !isnan(queue[n].data[a0 + a]))) && isnan(sp.axis[a]->settings.source)) { debug("Motor positions are not known, so move cannot take place; aborting move and removing it from the queue: %f %f %f", queue[settings.queue_start].data[a0 + a], queue[n].data[a0 + a], sp.axis[a]->settings.source); // This possibly removes one move too many, but it shouldn't happen anyway. if (queue[settings.queue_start].cb) ++num_cbs; if (settings.queue_end == settings.queue_start) send_host(CMD_CONTINUE, 0); settings.queue_start = n; settings.queue_full = false; abort_move(current_fragment_pos); return num_cbs; } } a0 += sp.num_axes; } // }}} // We are prepared and can start the segment. bool action = false; double vq; if (n == settings.queue_end) { // There is no next segment; we should stop at the end. {{{ prepared = false; #ifdef DEBUG_MOVE debug("Building final segment."); #endif for (uint8_t s = 0; s < 2; ++s) { Space &sp = spaces[s]; copy_next(s); if (sp.settings.dist[0] != 0) action = true; } vq = 0; } // }}} else { // There is a next segment; we should connect to it. {{{ prepared = true; #ifdef DEBUG_MOVE debug("Building a connecting segment."); #endif a0 = 0; change0(n); for (uint8_t s = 0; s < 2; ++s) { Space &sp = spaces[s]; space_types[sp.type].check_position(&sp, &queue[n].data[a0]); copy_next(s); set_from_queue(s, n, a0, true); if (sp.settings.dist[1] != 0 || sp.settings.dist[0] != 0) action = true; a0 += sp.num_axes; } vq = queue[n].f[0] * feedrate; } // }}} double v0 = queue[settings.queue_start].f[0] * feedrate; double vp = queue[settings.queue_start].f[1] * feedrate; settings.probing = queue[settings.queue_start].probe; settings.run_time = queue[settings.queue_start].time; settings.run_dist = queue[settings.queue_start].dist; if (queue[settings.queue_start].cb) cbs_after_current_move += 1; //debug("add cb to current starting at %d", current_fragment); if (settings.queue_end == settings.queue_start) send_host(CMD_CONTINUE, 0); settings.queue_full = false; settings.queue_start = n; if (!action) { // Skip zero-distance move. {{{ #ifdef DEBUG_MOVE debug("Skipping zero-distance prepared move"); #endif num_cbs += cbs_after_current_move; cbs_after_current_move = 0; for (uint8_t s = 0; s < 2; ++s) { Space &sp = spaces[s]; sp.settings.dist[0] = NAN; for (uint8_t a = 0; a < sp.num_axes; ++a) sp.axis[a]->settings.dist[0] = NAN; } settings.fq = 0; return num_cbs + next_move(); } // }}} // Currently set up: // f0: fraction of move already done by connection. // v0: this move's requested starting speed. // vp: this move's requested ending speed. // vq: next move's requested starting speed. // cbs_after_current_move: number of cbs that should be fired after this segment is complete. // dist[0]: total distance of this segment (mm). // dist[1]: total distance of next segment (mm). // mtr->dist[0]: motor distance of this segment (mm). // mtr->dist[1]: motor distance of next segment (mm). #ifdef DEBUG_MOVE debug("Set up: v0 = %f /s, vp = %f /s, vq = %f /s", v0, vp, vq); #endif // Limit v0, vp, vq. {{{ for (uint8_t s = 0; s < 2; ++s) { Space &sp = spaces[s]; double limit; if (s == 0) limit = max_v; else if (current_extruder < sp.num_motors) limit = sp.motor[current_extruder]->limit_v; else continue; if (isnan(limit) || isinf(limit) || limit <= 0) continue; // max_mm is the maximum speed in mm/s. double max_mm = settings.probing ? space_types[sp.type].probe_speed(&sp) : limit; double max = max_mm / sp.settings.dist[0]; if (v0 < 0) v0 = -v0 / sp.settings.dist[0]; if (vp < 0) vp = -vp / sp.settings.dist[0]; if (vq < 0) vq = -vq / sp.settings.dist[1]; if (v0 > max) v0 = max; if (vp > max) vp = max; max = max_mm / sp.settings.dist[1]; if (vq > max) vq = max; } #ifdef DEBUG_MOVE debug("After limiting, v0 = %f /s, vp = %f /s and vq = %f /s", v0, vp, vq); #endif // }}} // Already set up: f0, v0, vp, vq, dist[0], dist[1], mtr->dist[0], mtr->dist[1]. // To do: start_time, t0, tp, fmain, fp, fq, mtr->main_dist #ifdef DEBUG_MOVE debug("Preparation did f0 = %f", settings.f0); #endif // Use maximum deviation to find fraction where to start rounded corner. {{{ double factor = vq / vp; done_factor = NAN; if (vq == 0) { settings.fp = 0; settings.fq = 0; } else { settings.fp = factor > 1 ? .5 / factor : .5; for (uint8_t s = 0; s < 2; ++s) { Space &sp = spaces[s]; if (sp.num_axes < 2) continue; if (s != 0 || max_deviation == 0) { settings.fp = 0; break; } double nd = sp.settings.dist[1] * factor; double d = sp.settings.dist[0] - sp.settings.dist[1]; // Calculate distances and ignore spaces which don't have two segments. if (nd <= 0) continue; if (sp.settings.dist[0] <= 0) continue; double done = 1 - max_deviation / sp.settings.dist[0]; // Set it also if done_factor is NaN. if (!(done <= done_factor)) done_factor = done; double new_fp = max_deviation / sqrt(nd / (sp.settings.dist[0] + nd) * d); #ifdef DEBUG_MOVE debug("Space %d fp %f dev %f", s, settings.fp, max_deviation); #endif if (new_fp < settings.fp) settings.fp = new_fp; } settings.fq = settings.fp * factor; } if (isnan(done_factor)) done_factor = 1; // }}} settings.t0 = (1 - settings.fp) / (fabs(v0 + vp) / 2); settings.tp = settings.fp / (fabs(vp) / 2); settings.f1 = .5 * fabs(v0) * settings.t0; settings.f2 = 1 - settings.fp - settings.f1; // Set up endpos. {{{ for (uint8_t s = 0; s < 2; ++s) { Space &sp = spaces[s]; for (uint8_t a = 0; a < sp.num_axes; ++a) { sp.axis[a]->settings.main_dist = sp.axis[a]->settings.dist[0] * (1 - settings.fp); // Fill target for filling endpos below. if ((sp.axis[a]->settings.dist[0] > 0 && sp.axis[a]->settings.dist[1] < 0) || (sp.axis[a]->settings.dist[0] < 0 && sp.axis[a]->settings.dist[1] > 0)) sp.axis[a]->settings.target = sp.axis[a]->settings.source + sp.axis[a]->settings.dist[0]; else sp.axis[a]->settings.target = sp.axis[a]->settings.source + sp.axis[a]->settings.dist[0] + sp.axis[a]->settings.dist[1] * settings.fq; #ifdef DEBUG_MOVE debug("Axis %d %d dist %f main dist = %f, next dist = %f currentpos = %d current = %f", s, a, sp.axis[a]->settings.dist[0], sp.axis[a]->settings.main_dist, sp.axis[a]->settings.dist[1], sp.motor[a]->settings.current_pos, sp.axis[a]->settings.current); #endif } bool ok = true; // Using NULL as target fills endpos. space_types[sp.type].xyz2motors(&sp, NULL, &ok); } // }}} // Enable motors if they weren't. {{{ if (!motors_busy) { for (uint8_t s = 0; s < 2; ++s) { Space &sp = spaces[s]; for (uint8_t m = 0; m < sp.num_motors; ++m) SET(sp.motor[m]->enable_pin); } motors_busy = true; } // }}} #ifdef DEBUG_MOVE debug("Segment has been set up: f0=%f fp=%f fq=%f v0=%f /s vp=%f /s vq=%f /s t0=%f s tp=%f s", settings.f0, settings.fp, settings.fq, v0, vp, vq, settings.t0, settings.tp); #endif // Reset time. {{{ settings.hwtime = 0; settings.last_time = 0; settings.last_current_time = 0; settings.start_time = settings.last_time - uint32_t(settings.f0 / fabs(vp) * 1e6); // }}} if (!computing_move) { // Set up source if this is a new move. {{{ #ifdef DEBUG_MOVE debug("starting new move"); #endif //debug("current %d running %d", current_fragment, running_fragment); for (uint8_t s = 0; s < 2; ++s) { Space &sp = spaces[s]; for (uint8_t a = 0; a < sp.num_axes; ++a) sp.axis[a]->settings.source = sp.axis[a]->settings.current; } store_settings(); #ifdef DEBUG_PATH fprintf(stderr, "\n"); #endif } // }}} first_fragment = current_fragment; // Do this every time, because otherwise the queue must be regenerated. TODO: send partial fragment to make sure this hack actually works, or fix it properly. computing_move = true; return num_cbs; } // }}}