andrey
/
Marlin_FB4S
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Merge pull request #4387 from thinkyhead/rc_anhardt_more_4370 

Update Z in a unified way in run_z_probe
pull/1/head
Scott Lahteine 9 years ago
committed by GitHub
parent
22d0654fe6 bb93b6822a
commit
c287846f46
4 changed files with 42 additions and 65 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 86
      Marlin/Marlin_main.cpp
  2. 2
      Marlin/configuration_store.cpp
  3. 3
      Marlin/enum.h
  4. 16
      Marlin/planner.cpp

86
Marlin/Marlin_main.cpp
View File

@ -564,7 +564,7 @@ void stop();
void get_available_commands();
void process_next_command();
void prepare_move_to_destination();
void set_current_from_steppers();
void set_current_from_steppers_for_axis(AxisEnum axis);
#if ENABLED(ARC_SUPPORT)
void plan_arc(float target[NUM_AXIS], float* offset, uint8_t clockwise);
@ -1524,8 +1524,7 @@ static void set_axis_is_at_home(AxisEnum axis) {
if (axis == X_AXIS || axis == Y_AXIS) {
float homeposition[3];
for (uint8_t i = X_AXIS; i <= Z_AXIS; i++)
homeposition[i] = LOGICAL_POSITION(base_home_pos(i), i);
LOOP_XYZ(i) homeposition[i] = LOGICAL_POSITION(base_home_pos(i), i);
// SERIAL_ECHOPGM("homeposition[x]= "); SERIAL_ECHO(homeposition[0]);
// SERIAL_ECHOPGM("homeposition[y]= "); SERIAL_ECHOLN(homeposition[1]);
@ -2104,12 +2103,6 @@ static void clean_up_after_endstop_or_probe_move() {
return false;
}
#if ENABLED(DELTA)
#define SET_Z_FROM_STEPPERS() set_current_from_steppers()
#else
#define SET_Z_FROM_STEPPERS() current_position[Z_AXIS] = LOGICAL_POSITION(stepper.get_axis_position_mm(Z_AXIS), Z_AXIS)
#endif
// Do a single Z probe and return with current_position[Z_AXIS]
// at the height where the probe triggered.
static float run_z_probe() {
@ -2121,28 +2114,18 @@ static void clean_up_after_endstop_or_probe_move() {
planner.bed_level_matrix.set_to_identity();
#endif
#if ENABLED(DELTA)
float z_before = current_position[Z_AXIS], // Current Z
z_mm = stepper.get_axis_position_mm(Z_AXIS); // Some tower's current position
#endif
do_blocking_move_to_z(-(Z_MAX_LENGTH + 10), Z_PROBE_SPEED_FAST);
endstops.hit_on_purpose();
SET_Z_FROM_STEPPERS();
set_current_from_steppers_for_axis(Z_AXIS);
SYNC_PLAN_POSITION_KINEMATIC();
// move up the retract distance
do_blocking_move_to_z(current_position[Z_AXIS] + home_bump_mm(Z_AXIS), Z_PROBE_SPEED_FAST);
#if ENABLED(DELTA)
z_before = current_position[Z_AXIS];
z_mm = stepper.get_axis_position_mm(Z_AXIS);
#endif
// move back down slowly to find bed
do_blocking_move_to_z(current_position[Z_AXIS] - home_bump_mm(Z_AXIS) * 2, Z_PROBE_SPEED_SLOW);
endstops.hit_on_purpose();
SET_Z_FROM_STEPPERS();
set_current_from_steppers_for_axis(Z_AXIS);
SYNC_PLAN_POSITION_KINEMATIC();
#if ENABLED(DEBUG_LEVELING_FEATURE)
@ -2597,7 +2580,7 @@ static void homeaxis(AxisEnum axis) {
* - Set the feedrate, if included
*/
void gcode_get_destination() {
for (int i = 0; i < NUM_AXIS; i++) {
LOOP_XYZE(i) {
if (code_seen(axis_codes[i]))
destination[i] = code_value_axis_units(i) + (axis_relative_modes[i] || relative_mode ? current_position[i] : 0);
else
@ -3900,7 +3883,7 @@ inline void gcode_G92() {
if (!didE) stepper.synchronize();
bool didXYZ = false;
for (int i = 0; i < NUM_AXIS; i++) {
LOOP_XYZE(i) {
if (code_seen(axis_codes[i])) {
float p = current_position[i],
v = code_value_axis_units(i);
@ -5147,7 +5130,7 @@ inline void gcode_M85() {
* (Follows the same syntax as G92)
*/
inline void gcode_M92() {
for (int8_t i = 0; i < NUM_AXIS; i++) {
LOOP_XYZE(i) {
if (code_seen(axis_codes[i])) {
if (i == E_AXIS) {
float value = code_value_per_axis_unit(i);
@ -5339,7 +5322,7 @@ inline void gcode_M200() {
* M201: Set max acceleration in units/s^2 for print moves (M201 X1000 Y1000)
*/
inline void gcode_M201() {
for (int8_t i = 0; i < NUM_AXIS; i++) {
LOOP_XYZE(i) {
if (code_seen(axis_codes[i])) {
planner.max_acceleration_mm_per_s2[i] = code_value_axis_units(i);
}
@ -5350,7 +5333,7 @@ inline void gcode_M201() {
#if 0 // Not used for Sprinter/grbl gen6
inline void gcode_M202() {
for (int8_t i = 0; i < NUM_AXIS; i++) {
LOOP_XYZE(i) {
if (code_seen(axis_codes[i])) axis_travel_steps_per_sqr_second[i] = code_value_axis_units(i) * planner.axis_steps_per_mm[i];
}
}
@ -5361,7 +5344,7 @@ inline void gcode_M201() {
* M203: Set maximum feedrate that your machine can sustain (M203 X200 Y200 Z300 E10000) in units/sec
*/
inline void gcode_M203() {
for (int8_t i = 0; i < NUM_AXIS; i++)
LOOP_XYZE(i)
if (code_seen(axis_codes[i]))
planner.max_feedrate_mm_s[i] = code_value_axis_units(i);
}
@ -5421,7 +5404,7 @@ inline void gcode_M205() {
* M206: Set Additional Homing Offset (X Y Z). SCARA aliases T=X, P=Y
*/
inline void gcode_M206() {
for (int8_t i = X_AXIS; i <= Z_AXIS; i++)
LOOP_XYZ(i)
if (code_seen(axis_codes[i]))
set_home_offset((AxisEnum)i, code_value_axis_units(i));
@ -5463,7 +5446,7 @@ inline void gcode_M206() {
SERIAL_ECHOLNPGM(">>> gcode_M666");
}
#endif
for (int8_t i = X_AXIS; i <= Z_AXIS; i++) {
LOOP_XYZ(i) {
if (code_seen(axis_codes[i])) {
endstop_adj[i] = code_value_axis_units(i);
#if ENABLED(DEBUG_LEVELING_FEATURE)
@ -5955,7 +5938,7 @@ inline void gcode_M303() {
* M365: SCARA calibration: Scaling factor, X, Y, Z axis
*/
inline void gcode_M365() {
for (int8_t i = X_AXIS; i <= Z_AXIS; i++)
LOOP_XYZ(i)
if (code_seen(axis_codes[i]))
axis_scaling[i] = code_value_float();
}
@ -6091,8 +6074,8 @@ void quickstop_stepper() {
stepper.quick_stop();
#if DISABLED(SCARA)
stepper.synchronize();
set_current_from_steppers();
sync_plan_position(); // ...re-apply to planner position
LOOP_XYZ(i) set_current_from_steppers_for_axis((AxisEnum)i);
SYNC_PLAN_POSITION_KINEMATIC();
#endif
}
@ -6155,7 +6138,7 @@ void quickstop_stepper() {
*/
inline void gcode_M428() {
bool err = false;
for (int8_t i = X_AXIS; i <= Z_AXIS; i++) {
LOOP_XYZ(i) {
if (axis_homed[i]) {
float base = (current_position[i] > (sw_endstop_min[i] + sw_endstop_max[i]) / 2) ? base_home_pos(i) : 0,
diff = current_position[i] - LOGICAL_POSITION(base, i);
@ -6285,7 +6268,7 @@ inline void gcode_M503() {
float lastpos[NUM_AXIS];
// Save current position of all axes
for (uint8_t i = 0; i < NUM_AXIS; i++)
LOOP_XYZE(i)
lastpos[i] = destination[i] = current_position[i];
// Define runplan for move axes
@ -6506,7 +6489,7 @@ inline void gcode_M503() {
*/
inline void gcode_M907() {
#if HAS_DIGIPOTSS
for (int i = 0; i < NUM_AXIS; i++)
LOOP_XYZE(i)
if (code_seen(axis_codes[i])) stepper.digipot_current(i, code_value_int());
if (code_seen('B')) stepper.digipot_current(4, code_value_int());
if (code_seen('S')) for (int i = 0; i <= 4; i++) stepper.digipot_current(i, code_value_int());
@ -6522,7 +6505,7 @@ inline void gcode_M907() {
#endif
#if ENABLED(DIGIPOT_I2C)
// this one uses actual amps in floating point
for (int i = 0; i < NUM_AXIS; i++) if (code_seen(axis_codes[i])) digipot_i2c_set_current(i, code_value_float());
LOOP_XYZE(i) if (code_seen(axis_codes[i])) digipot_i2c_set_current(i, code_value_float());
// for each additional extruder (named B,C,D,E..., channels 4,5,6,7...)
for (int i = NUM_AXIS; i < DIGIPOT_I2C_NUM_CHANNELS; i++) if (code_seen('B' + i - (NUM_AXIS))) digipot_i2c_set_current(i, code_value_float());
#endif
@ -6531,7 +6514,7 @@ inline void gcode_M907() {
float dac_percent = code_value_float();
for (uint8_t i = 0; i <= 4; i++) dac_current_percent(i, dac_percent);
}
for (uint8_t i = 0; i < NUM_AXIS; i++) if (code_seen(axis_codes[i])) dac_current_percent(i, code_value_float());
LOOP_XYZE(i) if (code_seen(axis_codes[i])) dac_current_percent(i, code_value_float());
#endif
}
@ -6570,7 +6553,7 @@ inline void gcode_M907() {
// M350 Set microstepping mode. Warning: Steps per unit remains unchanged. S code sets stepping mode for all drivers.
inline void gcode_M350() {
if (code_seen('S')) for (int i = 0; i <= 4; i++) stepper.microstep_mode(i, code_value_byte());
for (int i = 0; i < NUM_AXIS; i++) if (code_seen(axis_codes[i])) stepper.microstep_mode(i, code_value_byte());
LOOP_XYZE(i) if (code_seen(axis_codes[i])) stepper.microstep_mode(i, code_value_byte());
if (code_seen('B')) stepper.microstep_mode(4, code_value_byte());
stepper.microstep_readings();
}
@ -6582,11 +6565,11 @@ inline void gcode_M907() {
inline void gcode_M351() {
if (code_seen('S')) switch (code_value_byte()) {
case 1:
for (int i = 0; i < NUM_AXIS; i++) if (code_seen(axis_codes[i])) stepper.microstep_ms(i, code_value_byte(), -1);
LOOP_XYZE(i) if (code_seen(axis_codes[i])) stepper.microstep_ms(i, code_value_byte(), -1);
if (code_seen('B')) stepper.microstep_ms(4, code_value_byte(), -1);
break;
case 2:
for (int i = 0; i < NUM_AXIS; i++) if (code_seen(axis_codes[i])) stepper.microstep_ms(i, -1, code_value_byte());
LOOP_XYZE(i) if (code_seen(axis_codes[i])) stepper.microstep_ms(i, -1, code_value_byte());
if (code_seen('B')) stepper.microstep_ms(4, -1, code_value_byte());
break;
}
@ -7929,25 +7912,16 @@ void clamp_to_software_endstops(float target[3]) {
#endif // DELTA
void set_current_from_steppers() {
void set_current_from_steppers_for_axis(AxisEnum axis) {
#if ENABLED(DELTA)
set_cartesian_from_steppers();
current_position[X_AXIS] = cartesian_position[X_AXIS];
current_position[Y_AXIS] = cartesian_position[Y_AXIS];
current_position[Z_AXIS] = cartesian_position[Z_AXIS];
current_position[axis] = LOGICAL_POSITION(cartesian_position[axis], axis);
#elif ENABLED(AUTO_BED_LEVELING_FEATURE)
vector_3 pos = planner.adjusted_position(); // values directly from steppers...
current_position[X_AXIS] = pos.x;
current_position[Y_AXIS] = pos.y;
current_position[Z_AXIS] = pos.z;
vector_3 pos = planner.adjusted_position();
current_position[axis] = LOGICAL_POSITION(axis == X_AXIS ? pos.x : axis == Y_AXIS ? pos.y : pos.z, axis);
#else
current_position[X_AXIS] = stepper.get_axis_position_mm(X_AXIS); // CORE handled transparently
current_position[Y_AXIS] = stepper.get_axis_position_mm(Y_AXIS);
current_position[Z_AXIS] = stepper.get_axis_position_mm(Z_AXIS);
current_position[axis] = LOGICAL_POSITION(stepper.get_axis_position_mm(axis), axis); // CORE handled transparently
#endif
for (uint8_t i = X_AXIS; i <= Z_AXIS; i++)
current_position[i] += LOGICAL_POSITION(0, i);
}
#if ENABLED(MESH_BED_LEVELING)
@ -8013,7 +7987,7 @@ void mesh_line_to_destination(float fr_mm_m, uint8_t x_splits = 0xff, uint8_t y_
inline bool prepare_kinematic_move_to(float target[NUM_AXIS]) {
float difference[NUM_AXIS];
for (int8_t i = 0; i < NUM_AXIS; i++) difference[i] = target[i] - current_position[i];
LOOP_XYZE(i) difference[i] = target[i] - current_position[i];
float cartesian_mm = sqrt(sq(difference[X_AXIS]) + sq(difference[Y_AXIS]) + sq(difference[Z_AXIS]));
if (cartesian_mm < 0.000001) cartesian_mm = abs(difference[E_AXIS]);
@ -8031,7 +8005,7 @@ void mesh_line_to_destination(float fr_mm_m, uint8_t x_splits = 0xff, uint8_t y_
float fraction = float(s) * inv_steps;
for (int8_t i = 0; i < NUM_AXIS; i++)
LOOP_XYZE(i)
target[i] = current_position[i] + difference[i] * fraction;
inverse_kinematics(target);

2
Marlin/configuration_store.cpp
View File

@ -563,7 +563,7 @@ void Config_ResetDefault() {
float tmp1[] = DEFAULT_AXIS_STEPS_PER_UNIT;
float tmp2[] = DEFAULT_MAX_FEEDRATE;
long tmp3[] = DEFAULT_MAX_ACCELERATION;
for (uint8_t i = 0; i < NUM_AXIS; i++) {
LOOP_XYZE(i) {
planner.axis_steps_per_mm[i] = tmp1[i];
planner.max_feedrate_mm_s[i] = tmp2[i];
planner.max_acceleration_mm_per_s2[i] = tmp3[i];

3
Marlin/enum.h
View File

@ -45,6 +45,9 @@ enum AxisEnum {
Z_HEAD = 5
};
#define LOOP_XYZ(VAR) for (uint8_t VAR=X_AXIS; VAR<=Z_AXIS; VAR++)
#define LOOP_XYZE(VAR) for (uint8_t VAR=X_AXIS; VAR<=E_AXIS; VAR++)
typedef enum {
LINEARUNIT_MM,
LINEARUNIT_INCH

16
Marlin/planner.cpp
View File

@ -134,7 +134,7 @@ Planner::Planner() { init(); }
void Planner::init() {
block_buffer_head = block_buffer_tail = 0;
memset(position, 0, sizeof(position)); // clear position
for (int i = 0; i < NUM_AXIS; i++) previous_speed[i] = 0.0;
LOOP_XYZE(i) previous_speed[i] = 0.0;
previous_nominal_speed = 0.0;
#if ENABLED(AUTO_BED_LEVELING_FEATURE)
bed_level_matrix.set_to_identity();
@ -423,7 +423,7 @@ void Planner::check_axes_activity() {
for (uint8_t b = block_buffer_tail; b != block_buffer_head; b = next_block_index(b)) {
block = &block_buffer[b];
for (int i = 0; i < NUM_AXIS; i++) if (block->steps[i]) axis_active[i]++;
LOOP_XYZE(i) if (block->steps[i]) axis_active[i]++;
}
}
#if ENABLED(DISABLE_X)
@ -893,7 +893,7 @@ void Planner::check_axes_activity() {
// Calculate and limit speed in mm/sec for each axis
float current_speed[NUM_AXIS];
float speed_factor = 1.0; //factor <=1 do decrease speed
for (int i = 0; i < NUM_AXIS; i++) {
LOOP_XYZE(i) {
current_speed[i] = delta_mm[i] * inverse_second;
float cs = fabs(current_speed[i]), mf = max_feedrate_mm_s[i];
if (cs > mf) speed_factor = min(speed_factor, mf / cs);
@ -939,7 +939,7 @@ void Planner::check_axes_activity() {
// Correct the speed
if (speed_factor < 1.0) {
for (unsigned char i = 0; i < NUM_AXIS; i++) current_speed[i] *= speed_factor;
LOOP_XYZE(i) current_speed[i] *= speed_factor;
block->nominal_speed *= speed_factor;
block->nominal_rate *= speed_factor;
}
@ -1051,7 +1051,7 @@ void Planner::check_axes_activity() {
block->recalculate_flag = true; // Always calculate trapezoid for new block
// Update previous path unit_vector and nominal speed
for (int i = 0; i < NUM_AXIS; i++) previous_speed[i] = current_speed[i];
LOOP_XYZE(i) previous_speed[i] = current_speed[i];
previous_nominal_speed = block->nominal_speed;
#if ENABLED(LIN_ADVANCE)
@ -1098,7 +1098,7 @@ void Planner::check_axes_activity() {
block_buffer_head = next_buffer_head;
// Update position
for (int i = 0; i < NUM_AXIS; i++) position[i] = target[i];
LOOP_XYZE(i) position[i] = target[i];
recalculate();
@ -1155,7 +1155,7 @@ void Planner::check_axes_activity() {
stepper.set_position(nx, ny, nz, ne);
previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest.
for (int i = 0; i < NUM_AXIS; i++) previous_speed[i] = 0.0;
LOOP_XYZE(i) previous_speed[i] = 0.0;
}
/**
@ -1168,7 +1168,7 @@ void Planner::set_e_position_mm(const float& e) {
// Recalculate the steps/s^2 acceleration rates, based on the mm/s^2
void Planner::reset_acceleration_rates() {
for (int i = 0; i < NUM_AXIS; i++)
LOOP_XYZE(i)
max_acceleration_steps_per_s2[i] = max_acceleration_mm_per_s2[i] * axis_steps_per_mm[i];
}

Write Preview
Loading…
Cancel
Save