void send_fragment() { // {{{ if (host_block) { current_fragment_pos = 0; return; } if (current_fragment_pos <= 0 || stopping || sending_fragment) { debug("no send fragment %d %d %d", current_fragment_pos, stopping, sending_fragment); return; } if (num_active_motors == 0) { if (current_fragment_pos < 2) { // TODO: find out why this is attempted and avoid it. debug("not sending short fragment for 0 motors; %d %d", current_fragment, running_fragment); if (history[current_fragment].cbs) { if (settings.queue_start == settings.queue_end && !settings.queue_full) { // Send cbs immediately. if (!host_block) { send_host(CMD_MOVECB, history[current_fragment].cbs); history[current_fragment].cbs = 0; } } } current_fragment_pos = 0; return; } else debug("sending fragment for 0 motors at position %d", current_fragment_pos); //abort(); } //debug("sending %d prevcbs %d", current_fragment, settings[(current_fragment - 1) % FRAGMENTS_PER_BUFFER].cbs); if (arch_send_fragment()) { current_fragment = (current_fragment + 1) % FRAGMENTS_PER_BUFFER; //debug("current send -> %x", current_fragment); store_settings(); if ((current_fragment - running_fragment + FRAGMENTS_PER_BUFFER) % FRAGMENTS_PER_BUFFER >= MIN_BUFFER_FILL && !stopping) arch_start_move(0); } } // }}}
void run_file_fill_queue() { static bool lock = false; if (lock) return; lock = true; rundebug("run queue, wait = %d tempwait = %d q = %d %d %d finish = %d", run_file_wait, run_file_wait_temp, settings.queue_end, settings.queue_start, settings.queue_full, run_file_finishing); if (run_file_audio >= 0) { while (true) { if (!run_file_map || run_file_wait || run_file_finishing) break; if (settings.run_file_current >= run_file_num_records) { run_file_finishing = true; //debug("done running audio"); break; } int16_t next = (current_fragment + 1) % FRAGMENTS_PER_BUFFER; if (next == running_fragment) break; settings.run_file_current = arch_send_audio(&reinterpret_cast <uint8_t *>(run_file_map)[sizeof(double)], settings.run_file_current, run_file_num_records, run_file_audio); current_fragment = next; store_settings(); if ((current_fragment - running_fragment + FRAGMENTS_PER_BUFFER) % FRAGMENTS_PER_BUFFER >= MIN_BUFFER_FILL && !stopping) arch_start_move(0); } lock = false; return; } while (run_file_map // There is a file to run. && (settings.queue_end - settings.queue_start + QUEUE_LENGTH) % QUEUE_LENGTH < 4 // There is space in the queue. && !settings.queue_full // Really, there is space in the queue. && settings.run_file_current < run_file_num_records // There are records to send. && !run_file_wait_temp // We are not waiting for a temp alarm. && !run_file_wait // We are not waiting for something else (pause or confirm). && !run_file_finishing) { // We are not waiting for underflow (should be impossible anyway, if there are commands in the queue). int t = run_file_map[settings.run_file_current].type; if (t != RUN_LINE && t != RUN_PRE_LINE && t != RUN_PRE_ARC && t != RUN_ARC && (arch_running() || settings.queue_end != settings.queue_start || computing_move)) break; Run_Record &r = run_file_map[settings.run_file_current]; rundebug("running %d: %d %d", settings.run_file_current, r.type, r.tool); switch (r.type) { case RUN_SYSTEM: { char const *cmd = strndupa(&reinterpret_cast<char const *>(run_file_map)[run_file_first_string + strings[r.tool].start], strings[r.tool].len); debug("Running system command: %ld %d %s", strings[r.tool].start, strings[r.tool].len, cmd); int ret = system(cmd); debug("Done running system command, return = %d", ret); break; } case RUN_PRE_ARC: { double x = r.X * run_file_cosa - r.Y * run_file_sina + run_file_refx; double y = r.Y * run_file_cosa + r.X * run_file_sina + run_file_refy; double z = r.Z; //debug("line %f %f %f", x, y, z); queue[settings.queue_end].center[0] = x; queue[settings.queue_end].center[1] = y; queue[settings.queue_end].center[2] = handle_probe(x, y, z); queue[settings.queue_end].normal[0] = r.E; queue[settings.queue_end].normal[1] = r.f; queue[settings.queue_end].normal[2] = r.F; break; } case RUN_PRE_LINE: { run_preline.X = r.X; run_preline.Y = r.Y; run_preline.Z = r.Z; run_preline.E = r.E; run_preline.tool = r.tool; break; } case RUN_LINE: case RUN_ARC: { queue[settings.queue_end].single = false; queue[settings.queue_end].probe = false; queue[settings.queue_end].arc = r.type == RUN_ARC; queue[settings.queue_end].f[0] = r.f; queue[settings.queue_end].f[1] = r.F; double x = r.X * run_file_cosa - r.Y * run_file_sina + run_file_refx; double y = r.Y * run_file_cosa + r.X * run_file_sina + run_file_refy; double z = r.Z; //debug("line/arc %f %f %f", x, y, z); int num0 = spaces[0].num_axes; if (num0 > 0) { queue[settings.queue_end].data[0] = x; if (num0 > 1) { queue[settings.queue_end].data[1] = y; if (num0 > 2) { queue[settings.queue_end].data[2] = handle_probe(x, y, z); if (num0 > 3) { queue[settings.queue_end].data[3] = run_preline.X; if (num0 > 4) { queue[settings.queue_end].data[4] = run_preline.Y; if (num0 > 5) { queue[settings.queue_end].data[5] = run_preline.Z; } } run_preline.X = NAN; run_preline.Y = NAN; run_preline.Z = NAN; } } } } for (int i = 6; i < num0; ++i) queue[settings.queue_end].data[i] = NAN; for (int i = 0; i < spaces[1].num_axes; ++i) { queue[settings.queue_end].data[num0 + i] = (i == r.tool ? r.E : i == run_preline.tool ? run_preline.E : NAN); //debug("queue %d + %d = %f", num0, i, queue[settings.queue_end].data[num0 + i]); } run_preline.E = NAN; num0 += spaces[1].num_axes; for (int s = 2; s < NUM_SPACES; ++s) { for (int i = 0; i < spaces[s].num_axes; ++i) queue[settings.queue_end].data[num0 + i] = NAN; num0 += spaces[s].num_axes; } queue[settings.queue_end].time = r.time; queue[settings.queue_end].dist = r.dist; queue[settings.queue_end].cb = false; settings.queue_end = (settings.queue_end + 1) % QUEUE_LENGTH; if (!computing_move) next_move(); else rundebug("no"); buffer_refill(); break; } case RUN_GPIO: { int tool = r.tool; if (tool == -2) tool = fan_id; else if (tool == -3) tool = spindle_id; if (tool < 0 || tool >= num_gpios) { if (tool != -1) debug("cannot set invalid gpio %d", tool); break; } if (r.X) { gpios[tool].state = 1; SET(gpios[tool].pin); } else { gpios[tool].state = 0; RESET(gpios[tool].pin); } send_host(CMD_UPDATE_PIN, tool, gpios[tool].state); break; } case RUN_SETTEMP: { int tool = r.tool; if (tool == -1) tool = bed_id; rundebug("settemp %d %f", tool, r.X); settemp(tool, r.X); send_host(CMD_UPDATE_TEMP, tool, 0, r.X); break; } case RUN_WAITTEMP: { int tool = r.tool; if (tool == -2) tool = bed_id; if (tool == -3) { for (int i = 0; i < num_temps; ++i) { if (temps[i].min_alarm >= 0 || temps[i].max_alarm < MAXINT) { run_file_wait_temp += 1; waittemp(i, temps[i].min_alarm, temps[i].max_alarm); } } break; } if (tool < 0 || tool >= num_temps) { if (tool != -1) debug("cannot wait for invalid temp %d", tool); break; } else rundebug("waittemp %d", tool); if (temps[tool].adctarget[0] >= 0 && temps[tool].adctarget[0] < MAXINT) { rundebug("waiting"); run_file_wait_temp += 1; waittemp(tool, temps[tool].target[0], temps[tool].max_alarm); } else rundebug("not waiting"); break; } case RUN_SETPOS: if (r.tool >= spaces[1].num_axes) { debug("Not setting position of invalid extruder %d", r.tool); break; } setpos(1, r.tool, r.X); break; case RUN_WAIT: if (r.X > 0) { run_file_timer.it_value.tv_sec = r.X; run_file_timer.it_value.tv_nsec = (r.X - run_file_timer.it_value.tv_sec) * 1e9; run_file_wait += 1; timerfd_settime(pollfds[0].fd, 0, &run_file_timer, NULL); } break; case RUN_CONFIRM: { int len = min(strings[r.tool].len, 250); memcpy(datastore, &reinterpret_cast<char const *>(run_file_map)[run_file_first_string + strings[r.tool].start], len); run_file_wait += 1; send_host(CMD_CONFIRM, r.X ? 1 : 0, 0, 0, 0, len); break; } case RUN_PARK: run_file_wait += 1; send_host(CMD_PARKWAIT); break; default: debug("Invalid record type %d in %s", r.type, run_file_name); break; } settings.run_file_current += 1; } rundebug("run queue done"); if (run_file_map && settings.run_file_current >= run_file_num_records && !run_file_wait_temp && !run_file_wait && !run_file_finishing) { // Done. //debug("done running file"); if (!computing_move && !sending_fragment && !arch_running()) { send_host(CMD_FILE_DONE); abort_run_file(); } else run_file_finishing = true; } lock = false; return; }
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); } // }}}
// 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; } // }}}
void handle_temp(int id, int temp) { // {{{ if (store_adc) fprintf(store_adc, "%d %d %f %d\n", millis(), id, temps[id].fromadc(temp), temp); if (requested_temp == id) { //debug("replying temp"); double result = temps[requested_temp].fromadc(temp); requested_temp = ~0; send_host(CMD_TEMP, 0, 0, result); } //debug("temp for %d: %d", id, temp); // If an alarm should be triggered, do so. Adc values are higher for lower temperatures. //debug("alarms: %d %d %d %d", id, temps[id].adcmin_alarm, temps[id].adcmax_alarm, temp); if (temps[id].adcmin_alarm >= temp || temps[id].adcmax_alarm <= temp) { //debug("alarm: %d %d %d %d", id, temps[id].adcmin_alarm, temps[id].adcmax_alarm, temp); temps[id].min_alarm = NAN; temps[id].max_alarm = NAN; temps[id].adcmin_alarm = -1; temps[id].adcmax_alarm = MAXINT; if (run_file_wait_temp) { run_file_wait_temp -= 1; run_file_fill_queue(); } else send_host(CMD_TEMPCB, id); } /* // TODO: Make this work and decide on units. // We have model settings. uint32_t dt = current_time - temps[id].last_temp_time; if (dt == 0) return; temps[id].last_temp_time = current_time; // Heater and core/shell transfer. if (temps[id].is_on) temps[id].core_T += temps[id].power / temps[id].core_C * dt; double Q = temps[id].transfer * (temps[id].core_T - temps[id].shell_T) * dt; temps[id].core_T -= Q / temps[id].core_C; temps[id].shell_T += Q / temps[id].shell_C; if (temps[id].is_on) temps[id].core_T += temps[id].power / temps[id].core_C * dt / 2; // Set shell to measured value. temps[id].shell_T = temp; // Add energy if required. double E = temps[id].core_T * temps[id].core_C + temps[id].shell_T * temps[id].shell_C; double T = E / (temps[id].core_C + temps[id].shell_C); // Set the pin to correct value. if (T < temps[id].target) { if (!temps[id].is_on) { SET(temps[id].power_pin); temps[id].is_on = true; ++temps_busy; } else temps[id].time_on += current_time - temps[id].last_temp_time; } else { if (temps[id].is_on) { RESET(temps[id].power_pin); temps[id].is_on = false; temps[id].time_on += current_time - temps[id].last_temp_time; --temps_busy; } } */ } // }}}