diff --git a/.travis.yml b/.travis.yml index cb1c650a72..c4a9f1b300 100644 --- a/.travis.yml +++ b/.travis.yml @@ -315,7 +315,7 @@ script: # SCARA with TMC2130 # - use_example_configs SCARA - - opt_enable AUTO_BED_LEVELING_BILINEAR FIX_MOUNTED_PROBE USE_ZMIN_PLUG EEPROM_SETTINGS EEPROM_CHITCHAT ULTIMAKERCONTROLLER + - opt_enable AUTO_BED_LEVELING_BILINEAR FIX_MOUNTED_PROBE USE_ZMIN_PLUG EEPROM_SETTINGS EEPROM_CHITCHAT ULTIMAKERCONTROLLER SCARA_FEEDRATE_SCALING - opt_enable_adv HAVE_TMC2130 X_IS_TMC2130 Y_IS_TMC2130 Z_IS_TMC2130 - opt_enable_adv MONITOR_DRIVER_STATUS STEALTHCHOP HYBRID_THRESHOLD SENSORLESS_HOMING - build_marlin_pio ${TRAVIS_BUILD_DIR} ${TEST_PLATFORM} diff --git a/Marlin/src/config/examples/SCARA/Configuration.h b/Marlin/src/config/examples/SCARA/Configuration.h index bc335fa948..9dca9a6da6 100644 --- a/Marlin/src/config/examples/SCARA/Configuration.h +++ b/Marlin/src/config/examples/SCARA/Configuration.h @@ -72,7 +72,9 @@ //#define MAKERARM_SCARA #if ENABLED(MORGAN_SCARA) || ENABLED(MAKERARM_SCARA) + //#define DEBUG_SCARA_KINEMATICS + #define SCARA_FEEDRATE_SCALING // Convert XY feedrate from mm/s to degrees/s on the fly // If movement is choppy try lowering this value #define SCARA_SEGMENTS_PER_SECOND 200 diff --git a/Marlin/src/gcode/motion/G2_G3.cpp b/Marlin/src/gcode/motion/G2_G3.cpp index 95ebba41eb..046f8db862 100644 --- a/Marlin/src/gcode/motion/G2_G3.cpp +++ b/Marlin/src/gcode/motion/G2_G3.cpp @@ -35,6 +35,10 @@ #include "../../module/scara.h" #endif +#if ENABLED(SCARA_FEEDRATE_SCALING) && ENABLED(AUTO_BED_LEVELING_BILINEAR) + #include "../../feature/bedlevel/abl/abl.h" +#endif + #if N_ARC_CORRECTION < 1 #undef N_ARC_CORRECTION #define N_ARC_CORRECTION 1 @@ -137,6 +141,14 @@ void plan_arc( millis_t next_idle_ms = millis() + 200UL; + #if ENABLED(SCARA_FEEDRATE_SCALING) + // SCARA needs to scale the feed rate from mm/s to degrees/s + const float inv_segment_length = 1.0 / (MM_PER_ARC_SEGMENT), + inverse_secs = inv_segment_length * fr_mm_s; + float oldA = planner.position_float[A_AXIS], + oldB = planner.position_float[B_AXIS]; + #endif + #if N_ARC_CORRECTION > 1 int8_t arc_recalc_count = N_ARC_CORRECTION; #endif @@ -180,11 +192,28 @@ void plan_arc( clamp_to_software_endstops(raw); - planner.buffer_line_kinematic(raw, fr_mm_s, active_extruder); + #if ENABLED(SCARA_FEEDRATE_SCALING) + // For SCARA scale the feed rate from mm/s to degrees/s + // i.e., Complete the angular vector in the given time. + inverse_kinematics(raw); + ADJUST_DELTA(raw); + planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], raw[Z_AXIS], raw[E_AXIS], HYPOT(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB) * inverse_secs, active_extruder); + oldA = delta[A_AXIS]; oldB = delta[B_AXIS]; + #else + planner.buffer_line_kinematic(raw, fr_mm_s, active_extruder); + #endif } // Ensure last segment arrives at target location. - planner.buffer_line_kinematic(cart, fr_mm_s, active_extruder); + #if ENABLED(SCARA_FEEDRATE_SCALING) + inverse_kinematics(cart); + ADJUST_DELTA(cart); + const float diff2 = HYPOT2(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB); + if (diff2) + planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], cart[Z_AXIS], cart[E_AXIS], SQRT(diff2) * inverse_secs, active_extruder); + #else + planner.buffer_line_kinematic(cart, fr_mm_s, active_extruder); + #endif // As far as the parser is concerned, the position is now == target. In reality the // motion control system might still be processing the action and the real tool position diff --git a/Marlin/src/module/motion.cpp b/Marlin/src/module/motion.cpp index 17097acead..76e5f3483e 100644 --- a/Marlin/src/module/motion.cpp +++ b/Marlin/src/module/motion.cpp @@ -498,20 +498,18 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS }, #if !UBL_SEGMENTED #if IS_KINEMATIC - #if ENABLED(AUTO_BED_LEVELING_BILINEAR) - #if ENABLED(DELTA) - #define ADJUST_DELTA(V) \ - if (planner.leveling_active) { \ - const float zadj = bilinear_z_offset(V); \ - delta[A_AXIS] += zadj; \ - delta[B_AXIS] += zadj; \ - delta[C_AXIS] += zadj; \ - } - #else - #define ADJUST_DELTA(V) if (planner.leveling_active) { delta[Z_AXIS] += bilinear_z_offset(V); } - #endif - #else - #define ADJUST_DELTA(V) NOOP + #if IS_SCARA + /** + * Before raising this value, use M665 S[seg_per_sec] to decrease + * the number of segments-per-second. Default is 200. Some deltas + * do better with 160 or lower. It would be good to know how many + * segments-per-second are actually possible for SCARA on AVR. + * + * Longer segments result in less kinematic overhead + * but may produce jagged lines. Try 0.5mm, 1.0mm, and 2.0mm + * and compare the difference. + */ + #define SCARA_MIN_SEGMENT_LENGTH 0.5 #endif /** @@ -566,9 +564,9 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS }, // gives the number of segments uint16_t segments = delta_segments_per_second * seconds; - // For SCARA minimum segment size is 0.25mm + // For SCARA enforce a minimum segment size #if IS_SCARA - NOMORE(segments, cartesian_mm * 4); + NOMORE(segments, cartesian_mm * (1.0 / SCARA_MIN_SEGMENT_LENGTH)); #endif // At least one segment is required @@ -576,7 +574,6 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS }, // The approximate length of each segment const float inv_segments = 1.0 / float(segments), - cartesian_segment_mm = cartesian_mm * inv_segments, segment_distance[XYZE] = { xdiff * inv_segments, ydiff * inv_segments, @@ -584,16 +581,47 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS }, ediff * inv_segments }; - // SERIAL_ECHOPAIR("mm=", cartesian_mm); - // SERIAL_ECHOPAIR(" seconds=", seconds); - // SERIAL_ECHOLNPAIR(" segments=", segments); - // SERIAL_ECHOLNPAIR(" segment_mm=", cartesian_segment_mm); + #if DISABLED(SCARA_FEEDRATE_SCALING) + const float cartesian_segment_mm = cartesian_mm * inv_segments; + #endif - // Get the current position as starting point + /* + SERIAL_ECHOPAIR("mm=", cartesian_mm); + SERIAL_ECHOPAIR(" seconds=", seconds); + SERIAL_ECHOPAIR(" segments=", segments); + #if DISABLED(SCARA_FEEDRATE_SCALING) + SERIAL_ECHOLNPAIR(" segment_mm=", cartesian_segment_mm); + #else + SERIAL_EOL(); + #endif + //*/ + + #if ENABLED(SCARA_FEEDRATE_SCALING) + // SCARA needs to scale the feed rate from mm/s to degrees/s + // i.e., Complete the angular vector in the given time. + const float segment_length = cartesian_mm * inv_segments, + inv_segment_length = 1.0 / segment_length, // 1/mm/segs + inverse_secs = inv_segment_length * _feedrate_mm_s; + + float oldA = planner.position_float[A_AXIS], + oldB = planner.position_float[B_AXIS]; + + /* + SERIAL_ECHOPGM("Scaled kinematic move: "); + SERIAL_ECHOPAIR(" segment_length (inv)=", segment_length); + SERIAL_ECHOPAIR(" (", inv_segment_length); + SERIAL_ECHOPAIR(") _feedrate_mm_s=", _feedrate_mm_s); + SERIAL_ECHOPAIR(" inverse_secs=", inverse_secs); + SERIAL_ECHOPAIR(" oldA=", oldA); + SERIAL_ECHOLNPAIR(" oldB=", oldB); + safe_delay(5); + //*/ + #endif + + // Get the current position as starting point float raw[XYZE]; COPY(raw, current_position); - // Calculate and execute the segments while (--segments) { @@ -613,11 +641,41 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS }, #endif ADJUST_DELTA(raw); // Adjust Z if bed leveling is enabled - planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], _feedrate_mm_s, active_extruder, cartesian_segment_mm); + #if ENABLED(SCARA_FEEDRATE_SCALING) + // For SCARA scale the feed rate from mm/s to degrees/s + // i.e., Complete the angular vector in the given time. + planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], raw[Z_AXIS], raw[E_AXIS], HYPOT(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB) * inverse_secs, active_extruder); + /* + SERIAL_ECHO(segments); + SERIAL_ECHOPAIR(": X=", raw[X_AXIS]); SERIAL_ECHOPAIR(" Y=", raw[Y_AXIS]); + SERIAL_ECHOPAIR(" A=", delta[A_AXIS]); SERIAL_ECHOPAIR(" B=", delta[B_AXIS]); + SERIAL_ECHOLNPAIR(" F", HYPOT(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB) * inverse_secs * 60); + safe_delay(5); + //*/ + oldA = delta[A_AXIS]; oldB = delta[B_AXIS]; + #else + planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], _feedrate_mm_s, active_extruder, cartesian_segment_mm); + #endif } // Ensure last segment arrives at target location. - planner.buffer_line_kinematic(rtarget, _feedrate_mm_s, active_extruder, cartesian_segment_mm); + #if ENABLED(SCARA_FEEDRATE_SCALING) + inverse_kinematics(rtarget); + ADJUST_DELTA(rtarget); + const float diff2 = HYPOT2(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB); + if (diff2) { + planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], rtarget[Z_AXIS], rtarget[E_AXIS], SQRT(diff2) * inverse_secs, active_extruder); + /* + SERIAL_ECHOPAIR("final: A=", delta[A_AXIS]); SERIAL_ECHOPAIR(" B=", delta[B_AXIS]); + SERIAL_ECHOPAIR(" adiff=", delta[A_AXIS] - oldA); SERIAL_ECHOPAIR(" bdiff=", delta[B_AXIS] - oldB); + SERIAL_ECHOLNPAIR(" F", (SQRT(diff2) * inverse_secs) * 60); + SERIAL_EOL(); + safe_delay(5); + //*/ + } + #else + planner.buffer_line_kinematic(rtarget, _feedrate_mm_s, active_extruder, cartesian_segment_mm); + #endif return false; // caller will update current_position } diff --git a/Marlin/src/module/motion.h b/Marlin/src/module/motion.h index 25b5a431fc..486677c4d9 100644 --- a/Marlin/src/module/motion.h +++ b/Marlin/src/module/motion.h @@ -340,4 +340,20 @@ void homeaxis(const AxisEnum axis); void set_home_offset(const AxisEnum axis, const float v); #endif +#if ENABLED(AUTO_BED_LEVELING_BILINEAR) + #if ENABLED(DELTA) + #define ADJUST_DELTA(V) \ + if (planner.leveling_active) { \ + const float zadj = bilinear_z_offset(V); \ + delta[A_AXIS] += zadj; \ + delta[B_AXIS] += zadj; \ + delta[C_AXIS] += zadj; \ + } + #else + #define ADJUST_DELTA(V) if (planner.leveling_active) { delta[Z_AXIS] += bilinear_z_offset(V); } + #endif +#else + #define ADJUST_DELTA(V) NOOP +#endif + #endif // MOTION_H diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index 04cfc2811e..3d0e871344 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -185,8 +185,11 @@ float Planner::previous_speed[NUM_AXIS], #endif #if ENABLED(LIN_ADVANCE) - float Planner::extruder_advance_K, // Initialized by settings.load() - Planner::position_float[XYZE]; // Needed for accurate maths. Steps cannot be used! + float Planner::extruder_advance_K; // Initialized by settings.load() +#endif + +#if HAS_POSITION_FLOAT + float Planner::position_float[XYZE]; // Needed for accurate maths. Steps cannot be used! #endif #if ENABLED(ULTRA_LCD) @@ -202,7 +205,7 @@ Planner::Planner() { init(); } void Planner::init() { block_buffer_head = block_buffer_tail = 0; ZERO(position); - #if ENABLED(LIN_ADVANCE) + #if HAS_POSITION_FLOAT ZERO(position_float); #endif ZERO(previous_speed); @@ -745,7 +748,7 @@ void Planner::check_axes_activity() { * extruder - target extruder */ void Planner::_buffer_steps(const int32_t (&target)[XYZE] - #if ENABLED(LIN_ADVANCE) + #if HAS_POSITION_FLOAT , const float (&target_float)[XYZE] #endif , float fr_mm_s, const uint8_t extruder, const float &millimeters/*=0.0*/ @@ -775,7 +778,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE] #if ENABLED(PREVENT_COLD_EXTRUSION) if (thermalManager.tooColdToExtrude(extruder)) { position[E_AXIS] = target[E_AXIS]; // Behave as if the move really took place, but ignore E part - #if ENABLED(LIN_ADVANCE) + #if HAS_POSITION_FLOAT position_float[E_AXIS] = target_float[E_AXIS]; #endif de = 0; // no difference @@ -786,7 +789,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE] #if ENABLED(PREVENT_LENGTHY_EXTRUDE) if (labs(de * e_factor[extruder]) > (int32_t)axis_steps_per_mm[E_AXIS_N] * (EXTRUDE_MAXLENGTH)) { // It's not important to get max. extrusion length in a precision < 1mm, so save some cycles and cast to int position[E_AXIS] = target[E_AXIS]; // Behave as if the move really took place, but ignore E part - #if ENABLED(LIN_ADVANCE) + #if HAS_POSITION_FLOAT position_float[E_AXIS] = target_float[E_AXIS]; #endif de = 0; // no difference @@ -857,6 +860,10 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE] block->steps[X_AXIS] = labs(da); block->steps[B_AXIS] = labs(db + dc); block->steps[C_AXIS] = labs(db - dc); + #elif IS_SCARA + block->steps[A_AXIS] = labs(da); + block->steps[B_AXIS] = labs(db); + block->steps[Z_AXIS] = labs(dc); #else // default non-h-bot planning block->steps[A_AXIS] = labs(da); @@ -892,7 +899,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE] powerManager.power_on(); #endif - //enable active axes + // Enable active axes #if CORE_IS_XY if (block->steps[A_AXIS] || block->steps[B_AXIS]) { enable_X(); @@ -1463,7 +1470,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE] // Update the position (only when a move was queued) static_assert(COUNT(target) > 1, "Parameter to _buffer_steps must be (&target)[XYZE]!"); COPY(position, target); - #if ENABLED(LIN_ADVANCE) + #if HAS_POSITION_FLOAT COPY(position_float, target_float); #endif @@ -1501,14 +1508,14 @@ void Planner::buffer_segment(const float &a, const float &b, const float &c, con LROUND(e * axis_steps_per_mm[E_AXIS_N]) }; - #if ENABLED(LIN_ADVANCE) + #if HAS_POSITION_FLOAT const float target_float[XYZE] = { a, b, c, e }; #endif // DRYRUN prevents E moves from taking place if (DEBUGGING(DRYRUN)) { position[E_AXIS] = target[E_AXIS]; - #if ENABLED(LIN_ADVANCE) + #if HAS_POSITION_FLOAT position_float[E_AXIS] = e; #endif } @@ -1547,7 +1554,7 @@ void Planner::buffer_segment(const float &a, const float &b, const float &c, con #define _BETWEEN(A) (position[A##_AXIS] + target[A##_AXIS]) >> 1 const int32_t between[ABCE] = { _BETWEEN(A), _BETWEEN(B), _BETWEEN(C), _BETWEEN(E) }; - #if ENABLED(LIN_ADVANCE) + #if HAS_POSITION_FLOAT #define _BETWEEN_F(A) (position_float[A##_AXIS] + target_float[A##_AXIS]) * 0.5 const float between_float[ABCE] = { _BETWEEN_F(A), _BETWEEN_F(B), _BETWEEN_F(C), _BETWEEN_F(E) }; #endif @@ -1555,7 +1562,7 @@ void Planner::buffer_segment(const float &a, const float &b, const float &c, con DISABLE_STEPPER_DRIVER_INTERRUPT(); _buffer_steps(between - #if ENABLED(LIN_ADVANCE) + #if HAS_POSITION_FLOAT , between_float #endif , fr_mm_s, extruder, millimeters * 0.5 @@ -1564,7 +1571,7 @@ void Planner::buffer_segment(const float &a, const float &b, const float &c, con const uint8_t next = block_buffer_head; _buffer_steps(target - #if ENABLED(LIN_ADVANCE) + #if HAS_POSITION_FLOAT , target_float #endif , fr_mm_s, extruder, millimeters * 0.5 @@ -1575,7 +1582,7 @@ void Planner::buffer_segment(const float &a, const float &b, const float &c, con } else _buffer_steps(target - #if ENABLED(LIN_ADVANCE) + #if HAS_POSITION_FLOAT , target_float #endif , fr_mm_s, extruder, millimeters @@ -1603,7 +1610,7 @@ void Planner::_set_position_mm(const float &a, const float &b, const float &c, c nb = position[B_AXIS] = LROUND(b * axis_steps_per_mm[B_AXIS]), nc = position[C_AXIS] = LROUND(c * axis_steps_per_mm[C_AXIS]), ne = position[E_AXIS] = LROUND(e * axis_steps_per_mm[_EINDEX]); - #if ENABLED(LIN_ADVANCE) + #if HAS_POSITION_FLOAT position_float[X_AXIS] = a; position_float[Y_AXIS] = b; position_float[Z_AXIS] = c; @@ -1635,7 +1642,7 @@ void Planner::set_position_mm_kinematic(const float (&cart)[XYZE]) { void Planner::sync_from_steppers() { LOOP_XYZE(i) { position[i] = stepper.position((AxisEnum)i); - #if ENABLED(LIN_ADVANCE) + #if HAS_POSITION_FLOAT position_float[i] = position[i] * steps_to_mm[i #if ENABLED(DISTINCT_E_FACTORS) + (i == E_AXIS ? active_extruder : 0) @@ -1656,7 +1663,7 @@ void Planner::set_position_mm(const AxisEnum axis, const float &v) { const uint8_t axis_index = axis; #endif position[axis] = LROUND(v * axis_steps_per_mm[axis_index]); - #if ENABLED(LIN_ADVANCE) + #if HAS_POSITION_FLOAT position_float[axis] = v; #endif stepper.set_position(axis, v); diff --git a/Marlin/src/module/planner.h b/Marlin/src/module/planner.h index da2cf36009..e2fedc4445 100644 --- a/Marlin/src/module/planner.h +++ b/Marlin/src/module/planner.h @@ -130,6 +130,8 @@ typedef struct { } block_t; +#define HAS_POSITION_FLOAT (ENABLED(LIN_ADVANCE) || ENABLED(SCARA_FEEDRATE_SCALING)) + #define BLOCK_MOD(n) ((n)&(BLOCK_BUFFER_SIZE-1)) class Planner { @@ -194,8 +196,11 @@ class Planner { #endif #if ENABLED(LIN_ADVANCE) - static float extruder_advance_K, - position_float[XYZE]; + static float extruder_advance_K; + #endif + + #if HAS_POSITION_FLOAT + static float position_float[XYZE]; #endif #if ENABLED(SKEW_CORRECTION) @@ -417,7 +422,7 @@ class Planner { * millimeters - the length of the movement, if known */ static void _buffer_steps(const int32_t (&target)[XYZE] - #if ENABLED(LIN_ADVANCE) + #if HAS_POSITION_FLOAT , const float (&target_float)[XYZE] #endif , float fr_mm_s, const uint8_t extruder, const float &millimeters=0.0