void Space::load_motor(int m, int32_t &addr) { // {{{ 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; motor[m]->step_pin.read(read_16(addr)); motor[m]->dir_pin.read(read_16(addr)); motor[m]->enable_pin.read(read_16(addr)); motor[m]->limit_min_pin.read(read_16(addr)); motor[m]->limit_max_pin.read(read_16(addr)); motor[m]->steps_per_unit = read_float(addr); motor[m]->home_pos = read_float(addr); motor[m]->limit_v = read_float(addr); motor[m]->limit_a = read_float(addr); motor[m]->home_order = read_8(addr); 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); SET_INPUT(motor[m]->limit_min_pin); SET_INPUT(motor[m]->limit_max_pin); bool must_move = false; if (!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) && !isnan(old_home_pos)) { double ohp = old_home_pos * old_steps_per_unit; double hp = motor[m]->home_pos * motor[m]->steps_per_unit; double diff = hp - ohp; motor[m]->settings.current_pos += diff; //debug("load motor %d %d new home %f add %f", id, m, motor[m]->home_pos, diff); arch_addpos(id, m, diff); must_move = true; } } else { // Axes without a limit switch, including extruders. if (motors_busy && old_steps_per_unit != motor[m]->steps_per_unit) { debug("load motor %d %d new steps no home", id, m); double oldpos = motor[m]->settings.current_pos; double pos = oldpos / old_steps_per_unit; motor[m]->settings.current_pos = pos * motor[m]->steps_per_unit; arch_addpos(id, m, motor[m]->settings.current_pos - oldpos); // Adjust current_pos in all history. for (int h = 0; h < FRAGMENTS_PER_BUFFER; ++h) { oldpos = motor[m]->history[h].current_pos; pos = oldpos / old_steps_per_unit; motor[m]->history[h].current_pos = pos * motor[m]->steps_per_unit; } } } if (must_move) move_to_current(); } // }}}
void Space::load_motor(uint8_t m, int32_t &addr) { // {{{ 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; motor[m]->step_pin.read(read_16(addr)); motor[m]->dir_pin.read(read_16(addr)); motor[m]->enable_pin.read(read_16(addr)); motor[m]->limit_min_pin.read(read_16(addr)); motor[m]->limit_max_pin.read(read_16(addr)); motor[m]->sense_pin.read(read_16(addr)); motor[m]->steps_per_unit = read_float(addr); motor[m]->max_steps = read_8(addr); motor[m]->home_pos = read_float(addr); motor[m]->limit_v = read_float(addr); motor[m]->limit_a = read_float(addr); motor[m]->home_order = read_8(addr); 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); SET_INPUT(motor[m]->limit_min_pin); SET_INPUT(motor[m]->limit_max_pin); SET_INPUT(motor[m]->sense_pin); bool must_move = false; if (!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) && !isnan(old_home_pos)) { int32_t hp = motor[m]->home_pos * motor[m]->steps_per_unit + (motor[m]->home_pos > 0 ? .49 : -.49); int32_t ohp = old_home_pos * old_steps_per_unit + (old_home_pos > 0 ? .49 : -.49); int32_t diff = hp - ohp; motor[m]->settings.current_pos += diff; cpdebug(id, m, "load motor new home add %d", diff); arch_addpos(id, m, diff); must_move = true; } } else { // Axes without a limit switch, including extruders. if (motors_busy && old_steps_per_unit != motor[m]->steps_per_unit) { int32_t cp = motor[m]->settings.current_pos; double pos = cp / old_steps_per_unit; cpdebug(id, m, "load motor new steps no home"); motor[m]->settings.current_pos = pos * motor[m]->steps_per_unit; int diff = motor[m]->settings.current_pos - cp; arch_addpos(id, m, diff); } } if (must_move) move_to_current(); } // }}}
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); } } // }}}
bool Space::setup_nums(int na, int nm) { // {{{ if (na == num_axes && nm == num_motors) return true; loaddebug("new space %d %d %d %d", na, nm, num_axes, num_motors); int old_nm = num_motors; int old_na = num_axes; num_motors = nm; num_axes = na; if (na != old_na) { Axis **new_axes = new Axis *[na]; loaddebug("new axes: %d", na); for (int a = 0; a < min(old_na, na); ++a) new_axes[a] = axis[a]; for (int a = old_na; a < na; ++a) { new_axes[a] = new Axis; new_axes[a]->park = NAN; new_axes[a]->park_order = 0; new_axes[a]->min_pos = -INFINITY; new_axes[a]->max_pos = INFINITY; new_axes[a]->type_data = NULL; new_axes[a]->settings.dist[0] = NAN; new_axes[a]->settings.dist[1] = NAN; new_axes[a]->settings.main_dist = NAN; new_axes[a]->settings.target = NAN; new_axes[a]->settings.source = NAN; new_axes[a]->settings.current = NAN; new_axes[a]->history = setup_axis_history(); } for (int a = na; a < old_na; ++a) { space_types[type].afree(this, a); delete[] axis[a]->history; delete axis[a]; } delete[] axis; axis = new_axes; } if (nm != old_nm) { Motor **new_motors = new Motor *[nm]; loaddebug("new motors: %d", nm); for (int m = 0; m < min(old_nm, nm); ++m) new_motors[m] = motor[m]; for (int m = old_nm; m < nm; ++m) { new_motors[m] = new Motor; new_motors[m]->step_pin.init(); new_motors[m]->dir_pin.init(); new_motors[m]->enable_pin.init(); new_motors[m]->limit_min_pin.init(); new_motors[m]->limit_max_pin.init(); new_motors[m]->steps_per_unit = 100; new_motors[m]->limit_v = INFINITY; new_motors[m]->limit_a = INFINITY; new_motors[m]->home_pos = NAN; new_motors[m]->home_order = 0; new_motors[m]->limit_v = INFINITY; new_motors[m]->limit_a = INFINITY; new_motors[m]->active = false; new_motors[m]->settings.last_v = 0; new_motors[m]->settings.current_pos = 0; new_motors[m]->settings.target_v = NAN; new_motors[m]->settings.target_pos = NAN; new_motors[m]->history = setup_motor_history(); ARCH_NEW_MOTOR(id, m, new_motors); } for (int m = nm; m < old_nm; ++m) { DATA_DELETE(id, m); delete[] motor[m]->history; delete motor[m]; } delete[] motor; motor = new_motors; arch_motors_change(); } return true; } // }}}
bool globals_load(int32_t &addr) { bool change_hw = false; uint8_t nt = read_8(addr); uint8_t ng = read_8(addr); // Free the old memory and initialize the new memory. if (nt != num_temps) { ldebug("new temp"); for (uint8_t t = nt; t < num_temps; ++t) temps[t].free(); Temp *new_temps = new Temp[nt]; for (uint8_t t = 0; t < min(nt, num_temps); ++t) temps[t].copy(new_temps[t]); for (uint8_t t = num_temps; t < nt; ++t) new_temps[t].init(); delete[] temps; temps = new_temps; num_temps = nt; } if (ng != num_gpios) { for (uint8_t g = ng; g < num_gpios; ++g) gpios[g].free(); Gpio *new_gpios = new Gpio[ng]; for (uint8_t g = 0; g < min(ng, num_gpios); ++g) gpios[g].copy(new_gpios[g]); for (uint8_t g = num_gpios; g < ng; ++g) new_gpios[g].init(); delete[] gpios; gpios = new_gpios; num_gpios = ng; } ldebug("new done"); int p = led_pin.write(); led_pin.read(read_16(addr)); if (p != led_pin.write()) change_hw = true; p = stop_pin.write(); stop_pin.read(read_16(addr)); if (p != stop_pin.write()) change_hw = true; p = probe_pin.write(); probe_pin.read(read_16(addr)); if (p != probe_pin.write()) change_hw = true; p = spiss_pin.write(); spiss_pin.read(read_16(addr)); if (p != spiss_pin.write()) change_hw = true; int t = timeout; timeout = read_16(addr); if (t != timeout) change_hw = true; bed_id = read_16(addr); fan_id = read_16(addr); spindle_id = read_16(addr); feedrate = read_float(addr); if (isnan(feedrate) || isinf(feedrate) || feedrate <= 0) feedrate = 1; max_deviation = read_float(addr); max_v = read_float(addr); int ce = read_8(addr); targetx = read_float(addr); targety = read_float(addr); double zo = read_float(addr); if (motors_busy && (current_extruder != ce || zoffset != zo) && settings.queue_start == settings.queue_end && !settings.queue_full && !computing_move) { queue[settings.queue_end].probe = false; queue[settings.queue_end].cb = false; queue[settings.queue_end].f[0] = INFINITY; queue[settings.queue_end].f[1] = INFINITY; for (int i = 0; i < spaces[0].num_axes; ++i) { queue[settings.queue_end].data[i] = spaces[0].axis[i]->settings.current - (i == 2 ? zoffset : 0); for (int s = 0; s < NUM_SPACES; ++s) queue[settings.queue_end].data[i] = space_types[spaces[s].type].unchange0(&spaces[s], i, queue[settings.queue_end].data[i]); } for (int i = spaces[0].num_axes; i < QUEUE_LENGTH; ++i) { queue[settings.queue_end].data[i] = NAN; } settings.queue_end = (settings.queue_end + 1) % QUEUE_LENGTH; // This shouldn't happen and causes communication problems, but if you have a 1-item buffer it is correct. if (settings.queue_end == settings.queue_start) settings.queue_full = true; current_extruder = ce; zoffset = zo; next_move(); buffer_refill(); } else { current_extruder = ce; zoffset = zo; } bool store = read_8(addr); if (store && !store_adc) { store_adc = fopen("/tmp/franklin-adc-dump", "a"); } else if (!store && store_adc) { fclose(store_adc); store_adc = NULL; } ldebug("all done"); if (change_hw) arch_motors_change(); return true; }