void Space::load_info() { // {{{
loaddebug("loading space %d", id);
int t = type;
if (t < 0 || t >= NUM_SPACE_TYPES) {
debug("invalid current type?! using default type instead");
t = DEFAULT_TYPE;
}
type = shmem->ints[1];
loaddebug("requested type %d, current is %d", type, t);
if (type < 0 || type >= NUM_SPACE_TYPES || (id == 1 && type != TYPE_EXTRUDER) || (id == 2 && type != TYPE_FOLLOWER)) {
debug("request for type %d ignored", type);
type = t;
return; // The rest of the info is not meant for this type, so ignore it.
}
if (t != type) {
loaddebug("setting type to %d", type);
space_types[t].free(this);
if (!space_types[type].init(this)) {
type = DEFAULT_TYPE;
reset_pos(this);
for (int a = 0; a < num_axes; ++a)
axis[a]->settings.current = axis[a]->settings.source;
return; // The rest of the info is not meant for DEFAULT_TYPE, so ignore it.
}
}
space_types[type].load(this);
reset_pos(this);
loaddebug("done loading space");
} // }}}
void Space::load_motor(int m) { // {{{
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;
bool old_dir_invert = motor[m]->dir_pin.inverted();
motor[m]->step_pin.read(shmem->ints[2]);
motor[m]->dir_pin.read(shmem->ints[3]);
motor[m]->enable_pin.read(shmem->ints[4]);
motor[m]->limit_min_pin.read(shmem->ints[5]);
motor[m]->limit_max_pin.read(shmem->ints[6]);
motor[m]->home_order = shmem->ints[7];
motor[m]->steps_per_unit = shmem->floats[0];
if (std::isnan(motor[m]->steps_per_unit)) {
debug("Trying to set NaN steps per unit for motor %d %d", id, m);
motor[m]->steps_per_unit = old_steps_per_unit;
}
motor[m]->home_pos = shmem->floats[1];
motor[m]->limit_v = shmem->floats[2];
motor[m]->limit_a = shmem->floats[3];
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);
RESET(motor[m]->dir_pin);
SET_INPUT(motor[m]->limit_min_pin);
SET_INPUT(motor[m]->limit_max_pin);
if (motor[m]->dir_pin.inverted() != old_dir_invert)
arch_invertpos(id, m);
if (!std::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) && !std::isnan(old_home_pos)) {
double diff = motor[m]->home_pos - old_home_pos * old_steps_per_unit / motor[m]->steps_per_unit;
motor[m]->settings.current_pos += diff;
//debug("load motor %d %d new home %f add %f", id, m, motor[m]->home_pos, diff);
// adjusting the arch pos is not affected by steps/unit.
arch_addpos(id, m, motor[m]->home_pos - old_home_pos);
}
reset_pos(this);
}
else if (!std::isnan(motor[m]->settings.current_pos)) {
// Motors without a limit switch: adjust motor position to match axes.
for (int a = 0; a < num_axes; ++a)
axis[a]->settings.target = axis[a]->settings.current;
space_types[type].xyz2motors(this);
double diff = motor[m]->settings.target_pos - motor[m]->settings.current_pos;
motor[m]->settings.current_pos += diff;
arch_addpos(id, m, diff);
}
} // }}}
static int find_a_djikstra(t_infos *info,
t_int_list **int_list,
int i)
{
reset_pos(info);
if ((int_list[i] = load_the_djikstra(info)) == NULL)
return (FAILURE);
close_room(info);
return (SUCCESS);
}
void
EditorTilegroupMenu::menu_action(MenuItem* item)
{
if (item->id >= 0)
{
auto tileselect = &(Editor::current()->tileselect);
tileselect->active_tilegroup = Editor::current()->tileset->tilegroups[item->id].tiles;
tileselect->input_type = EditorInputGui::IP_TILE;
tileselect->reset_pos();
tileselect->update_mouse_icon();
}
MenuManager::instance().clear_menu_stack();
}
void nxtCanvasWidget::change_canvas( nxtCanvas* new_canvas, bool delete_old ){
//Delete old canvas if wanted
if( delete_old && canvas )
delete canvas;
canvas = new_canvas;
//reset shown position
if( is_moveable )
reset_pos();
update(); //Update the view
emit canvas_changed();
}
void CHARACTER::tick_defered()
{
// advance the dummy
{
WORLD_CORE tempworld;
reckoningcore.world = &tempworld;
reckoningcore.tick(false);
reckoningcore.move();
reckoningcore.quantize();
}
//lastsentcore;
/*if(!dead)
{*/
vec2 start_pos = core.pos;
vec2 start_vel = core.vel;
bool stuck_before = test_box(core.pos, vec2(28.0f, 28.0f));
core.move();
if(is_hitting_door())
{
reset_pos();
if(is_hitting_door() && !doorstuck)
{
game.send_emoticon(player->client_id, 11);
doorstuck = true;
}
}
else
doorstuck = false;
bool stuck_after_move = test_box(core.pos, vec2(28.0f, 28.0f));
core.quantize();
bool stuck_after_quant = test_box(core.pos, vec2(28.0f, 28.0f));
pos = core.pos;
if(!stuck_before && (stuck_after_move || stuck_after_quant))
{
dbg_msg("player", "STUCK!!! %d %d %d %f %f %f %f %x %x %x %x",
stuck_before,
stuck_after_move,
stuck_after_quant,
start_pos.x, start_pos.y,
start_vel.x, start_vel.y,
*((unsigned *)&start_pos.x), *((unsigned *)&start_pos.y),
*((unsigned *)&start_vel.x), *((unsigned *)&start_vel.y));
}
int events = core.triggered_events;
int mask = cmask_all_except_one(player->client_id);
if(events&COREEVENT_GROUND_JUMP) game.create_sound(pos, SOUND_PLAYER_JUMP, mask);
//if(events&COREEVENT_HOOK_LAUNCH) snd_play_random(CHN_WORLD, SOUND_HOOK_LOOP, 1.0f, pos);
if(events&COREEVENT_HOOK_ATTACH_PLAYER) game.create_sound(pos, SOUND_HOOK_ATTACH_PLAYER, cmask_all());
if(events&COREEVENT_HOOK_ATTACH_GROUND) game.create_sound(pos, SOUND_HOOK_ATTACH_GROUND, mask);
if(events&COREEVENT_HOOK_HIT_NOHOOK) game.create_sound(pos, SOUND_HOOK_NOATTACH, mask);
//if(events&COREEVENT_HOOK_RETRACT) snd_play_random(CHN_WORLD, SOUND_PLAYER_JUMP, 1.0f, pos);
//}
if(team == -1)
{
pos.x = input.target_x;
pos.y = input.target_y;
}
// update the sendcore if needed
{
NETOBJ_CHARACTER predicted;
NETOBJ_CHARACTER current;
mem_zero(&predicted, sizeof(predicted));
mem_zero(¤t, sizeof(current));
reckoningcore.write(&predicted);
core.write(¤t);
// only allow dead reackoning for a top of 3 seconds
if(reckoning_tick+server_tickspeed()*3 < server_tick() || mem_comp(&predicted, ¤t, sizeof(NETOBJ_CHARACTER)) != 0)
{
reckoning_tick = server_tick();
sendcore = core;
reckoningcore = core;
}
}
}
// For documentation about variables used here, see struct History in cdriver.h
int next_move(int32_t start_time) { // {{{
// Clean up state before starting the move.
if (current_fragment_pos > 0) {
//debug("sending because new move is started");
send_fragment();
}
settings.probing = false;
settings.factor = 0;
int num_cbs = 0;
run_file_fill_queue();
if (settings.queue_start == settings.queue_end && !settings.queue_full) {
//debug("no next move");
computing_move = false;
return num_cbs;
}
mdebug("Next move; queue start = %d, end = %d", settings.queue_start, settings.queue_end);
// Set everything up for running queue[settings.queue_start].
int q = settings.queue_start;
int n = (settings.queue_start + 1) % QUEUE_LENGTH;
settings.single = queue[q].single;
if (queue[q].cb)
++num_cbs;
// Make sure machine state is good. {{{
// If the source is unknown, determine it from current_pos.
for (int s = 0; s < NUM_SPACES; ++s) {
if (s == 1) {
// Extruders are handled later.
continue;
}
Space &sp = spaces[s];
for (int a = 0; a < sp.num_axes; ++a) {
if (std::isnan(sp.axis[a]->settings.source)) {
if (!std::isnan(sp.axis[a]->settings.current)) {
sp.axis[a]->settings.source = sp.axis[a]->settings.current;
continue;
}
reset_pos(&sp);
for (int aa = 0; aa < sp.num_axes; ++aa)
sp.axis[aa]->settings.current = sp.axis[aa]->settings.source;
break;
}
else
mdebug("non-nan: %d %d %f %f", s, a, sp.axis[a]->settings.source, sp.motor[a]->settings.current_pos);
}
// Followers don't have good motor data, so initialize them here.
for (int a = 0; a < sp.num_axes; ++a) {
if (s == 2)
sp.motor[a]->settings.current_pos = sp.axis[a]->settings.source;
}
}
// }}}
change0(q);
// Fill unspecified coordinates with previous values. {{{
Space &sp0 = spaces[0];
for (int a = 0; a < sp0.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 (!std::isnan(queue[q].X[a]) && std::isnan(queue[n].X[a])) {
queue[n].X[a] = queue[q].X[a];
mdebug("filling next %d with %f", a, queue[n].X[a]);
}
if (std::isnan(queue[q].X[a]) && !std::isnan(queue[n].X[a])) {
queue[q].X[a] = sp0.axis[a]->settings.source;
mdebug("filling %d with %f", a, queue[q].X[a]);
}
}
if ((!std::isnan(queue[q].X[a]) || (n != settings.queue_end && !std::isnan(queue[n].X[a]))) && std::isnan(sp0.axis[a]->settings.source)) {
debug("Motor position for axis %d is not known, so move cannot take place; aborting move and removing it from the queue: q1=%f q2=%f src=%f", a, queue[q].X[a], queue[n].X[a], sp0.axis[a]->settings.source);
// This possibly removes one move too many, but it shouldn't happen anyway.
settings.queue_start = n;
settings.queue_full = false;
abort_move(current_fragment_pos);
return num_cbs;
}
}
// }}}
// Reset time. {{{
if (computing_move) {
settings.hwtime -= start_time;
//debug("time -= %d, now %d", start_time, settings.hwtime);
}
else {
settings.hwtime = 0;
}
// }}}
if (!computing_move) { // Set up source if this is a new move. {{{
mdebug("starting new move");
//debug("current %d running %d", current_fragment, running_fragment);
for (int s = 0; s < NUM_SPACES; ++s) {
Space &sp = spaces[s];
for (int a = 0; a < sp.num_axes; ++a) {
if (!std::isnan(sp.axis[a]->settings.current)) {
mdebug("setting source for %d %d to current %f (was %f)", s, a, sp.axis[a]->settings.current, sp.axis[a]->settings.source);
sp.axis[a]->settings.source = sp.axis[a]->settings.current;
}
}
}
} // }}}
settings.v0 = queue[q].v0 * feedrate;
settings.v1 = queue[q].v1 * feedrate;
double dot = 0, norma = 0, normb = 0, normab = 0;
for (int i = 0; i < 3; ++i) {
bool use = i < spaces[0].num_axes;
double p = (use ? spaces[0].axis[i]->settings.source : 0);
settings.P[i] = (use ? (std::isnan(queue[q].X[i]) ? p : (queue[q].X[i] + (i == 2 ? zoffset : 0) + p) / 2) : 0);
settings.A[i] = settings.P[i] - p;
settings.B[i] = (use ? queue[q].B[i] : 0);
double ab = settings.A[i] + settings.B[i];
dot += settings.A[i] * ab;
norma += settings.A[i] * settings.A[i];
normb += settings.B[i] * settings.B[i];
normab += ab * ab;
}
norma = sqrt(norma);
normb = sqrt(normb);
normab = sqrt(normab);
settings.alpha_max = acos(dot / (norma * normab));
if (std::isnan(settings.alpha_max))
settings.alpha_max = 0;
settings.dist = (normb > 1e-5 ? norma * (normab / normb) * settings.alpha_max : norma) * 2;
if (std::isnan(settings.dist) || std::fabs(settings.dist) < 1e-10) {
//debug("no space dist, using other system. dist=%f a=%f ab=%f b=%f", settings.dist, norma, normab, normb);
if (queue[q].tool >= 0 && queue[q].tool < spaces[1].num_axes)
settings.dist = std::fabs(queue[q].e - spaces[1].axis[queue[q].tool]->settings.source);
else if (queue[q].single && queue[q].tool < 0 && ~queue[q].tool < spaces[2].num_axes)
settings.dist = std::fabs(queue[q].e - spaces[2].axis[~queue[q].tool]->settings.source);
}
double dt = settings.dist / ((settings.v0 + settings.v1) / 2);
settings.end_time = (std::isnan(dt) ? 0 : 1e6 * dt);
if (queue[q].tool >= 0 && queue[q].tool < spaces[1].num_axes && !std::isnan(queue[q].e)) {
spaces[1].axis[queue[q].tool]->settings.endpos = queue[q].e;
mdebug("move extruder to %f", queue[q].e);
}
else if (queue[q].single && queue[q].tool < 0 && ~queue[q].tool < spaces[2].num_axes && !std::isnan(queue[q].e)) {
spaces[2].axis[~queue[q].tool]->settings.endpos = queue[q].e;
mdebug("move follower to %f, current=%f source=%f current_pos=%f", queue[q].e, spaces[2].axis[~queue[q].tool]->settings.current, spaces[2].axis[~queue[q].tool]->settings.source, spaces[2].motor[~queue[q].tool]->settings.current_pos);
}
auto last_hwtime_step = settings.hwtime_step;
if (queue[q].pattern_size > 0) {
memcpy(settings.pattern, queue[q].pattern, queue[q].pattern_size);
settings.hwtime_step = dt * 1e6 / (queue[q].pattern_size * 8);
if (settings.hwtime_step < min_hwtime_step)
settings.hwtime_step = min_hwtime_step;
}
settings.pattern_size = queue[q].pattern_size;
if (settings.hwtime_step != last_hwtime_step)
arch_globals_change();
/*
if (spaces[0].num_axes > 2) {
debug("move prepared, from=(%f,%f,%f) Q=(%f,%f,%f) P=(%f,%f,%f), A=(%f,%f,%f), B=(%f,%f,%f), max alpha=%f, dist=%f, e=%f, v0=%f, v1=%f, end time=%f, single=%d, UVW=(%f,%f,%f)", spaces[0].axis[0]->settings.source, spaces[0].axis[1]->settings.source, spaces[0].axis[2]->settings.source, queue[q].X[0], queue[q].X[1], queue[q].X[2], settings.P[0], settings.P[1], settings.P[2], settings.A[0], settings.A[1], settings.A[2], settings.B[0], settings.B[1], settings.B[2], settings.alpha_max, settings.dist, queue[q].e, settings.v0, settings.v1, settings.end_time / 1e6, queue[q].single, spaces[0].motor[0]->settings.current_pos, spaces[0].motor[1]->settings.current_pos, spaces[0].motor[2]->settings.current_pos);
}
else if (spaces[0].num_axes > 1) {
debug("move prepared, from=(%f,%f) Q=(%f,%f,%f) P=(%f,%f,%f), A=(%f,%f,%f), B=(%f,%f,%f), max alpha=%f, dist=%f, e=%f, v0=%f, v1=%f, end time=%f, single=%d, UV=(%f,%f)", spaces[0].axis[0]->settings.source, spaces[0].axis[1]->settings.source, queue[q].X[0], queue[q].X[1], queue[q].X[2], settings.P[0], settings.P[1], settings.P[2], settings.A[0], settings.A[1], settings.A[2], settings.B[0], settings.B[1], settings.B[2], settings.alpha_max, settings.dist, queue[q].e, settings.v0, settings.v1, settings.end_time / 1e6, queue[q].single, spaces[0].motor[0]->settings.current_pos, spaces[0].motor[1]->settings.current_pos);
}
// */
//debug("times end %d current %d dist %f v0 %f v1 %f", settings.end_time, settings.hwtime, settings.dist, settings.v0, settings.v1);
settings.queue_start = n;
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.
if (!computing_move)
store_settings();
computing_move = true;
return num_cbs;
} // }}}
