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