From 4e67a85a5dea4ae0db7bb233e5ec71847315ede3 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Sat, 23 Jul 2016 13:07:23 -0700 Subject: [PATCH 1/3] Macros to loop over axes --- Marlin/Marlin_main.cpp | 41 +++++++++++++++++----------------- Marlin/configuration_store.cpp | 2 +- Marlin/enum.h | 3 +++ Marlin/planner.cpp | 16 ++++++------- 4 files changed, 32 insertions(+), 30 deletions(-) diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index 4f2db6d6c..4286e67b3 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -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]); @@ -2597,7 +2596,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 +3899,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 +5146,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 +5338,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 +5349,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 +5360,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 +5420,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 +5462,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 +5954,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(); } @@ -6155,7 +6154,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 +6284,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 +6505,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 +6521,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 +6530,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 +6569,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 +6581,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; } @@ -8013,7 +8012,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 +8030,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); diff --git a/Marlin/configuration_store.cpp b/Marlin/configuration_store.cpp index b203bbb62..6a581ee1f 100644 --- a/Marlin/configuration_store.cpp +++ b/Marlin/configuration_store.cpp @@ -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]; diff --git a/Marlin/enum.h b/Marlin/enum.h index fe369c23f..cbdfd4a41 100644 --- a/Marlin/enum.h +++ b/Marlin/enum.h @@ -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 diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp index f7455253b..35e5d3bd9 100644 --- a/Marlin/planner.cpp +++ b/Marlin/planner.cpp @@ -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]; } From 96a162bedad94aa50f7e952545d2b0674161f4cd Mon Sep 17 00:00:00 2001 From: AnHardt Date: Sat, 23 Jul 2016 14:02:14 +0200 Subject: [PATCH 2/3] Use implications from #4370 in run_z_probe() See also https://github.com/MarlinFirmware/Marlin/pull/4370#issuecomment-234713680 --- Marlin/Marlin_main.cpp | 20 ++------------------ 1 file changed, 2 insertions(+), 18 deletions(-) diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index 4286e67b3..b1cfad4bb 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -2103,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() { @@ -2120,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(); 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(); SYNC_PLAN_POSITION_KINEMATIC(); #if ENABLED(DEBUG_LEVELING_FEATURE) From bb93b6822a0eb86acab8fcf97ae4eeacf3b0e4d4 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Sat, 23 Jul 2016 13:11:18 -0700 Subject: [PATCH 3/3] Use set_current_from_steppers_for_axis --- Marlin/Marlin_main.cpp | 29 ++++++++++------------------- 1 file changed, 10 insertions(+), 19 deletions(-) diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index b1cfad4bb..b92a22242 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -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); @@ -2116,7 +2116,7 @@ static void clean_up_after_endstop_or_probe_move() { do_blocking_move_to_z(-(Z_MAX_LENGTH + 10), Z_PROBE_SPEED_FAST); endstops.hit_on_purpose(); - set_current_from_steppers(); + set_current_from_steppers_for_axis(Z_AXIS); SYNC_PLAN_POSITION_KINEMATIC(); // move up the retract distance @@ -2125,7 +2125,7 @@ static void clean_up_after_endstop_or_probe_move() { // 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_current_from_steppers(); + set_current_from_steppers_for_axis(Z_AXIS); SYNC_PLAN_POSITION_KINEMATIC(); #if ENABLED(DEBUG_LEVELING_FEATURE) @@ -6074,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 } @@ -7912,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)