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;
}
}
*/
} // }}}
