Merge pull request #8865 from thinkyhead/bf2_more_scara_scaling

[2.0.x] SCARA Feedrate Scaling for G2/G3 - using HYPOT

Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp

 
     #if ENABLED(DELTA)  // apply delta inverse_kinematics
 
-      DELTA_RAW_IK();
+      DELTA_IK(raw);
       planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], in_raw[E_AXIS], fr, active_extruder);
 
     #elif IS_SCARA  // apply scara inverse_kinematics (should be changed to save raw->logical->raw)

Marlin/src/gcode/motion/G2_G3.cpp

 #include "../../module/planner.h"
 #include "../../module/temperature.h"
 
+#if ENABLED(DELTA)
+  #include "../../module/delta.h"
+#elif ENABLED(SCARA)
+  #include "../../module/scara.h"
+#endif
+
 #if N_ARC_CORRECTION < 1
   #undef N_ARC_CORRECTION
   #define N_ARC_CORRECTION 1
    * This is important when there are successive arc motions.
    */
   // Vector rotation matrix values
-  float arc_target[XYZE];
+  float raw[XYZE];
   const float theta_per_segment = angular_travel / segments,
               linear_per_segment = linear_travel / segments,
               extruder_per_segment = extruder_travel / segments,
               cos_T = 1 - 0.5 * sq(theta_per_segment); // Small angle approximation
 
   // Initialize the linear axis
-  arc_target[l_axis] = current_position[l_axis];
+  raw[l_axis] = current_position[l_axis];
 
   // Initialize the extruder axis
-  arc_target[E_AXIS] = current_position[E_AXIS];
+  raw[E_AXIS] = current_position[E_AXIS];
 
   const float fr_mm_s = MMS_SCALED(feedrate_mm_s);
 
     int8_t arc_recalc_count = N_ARC_CORRECTION;
   #endif
 
+  #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 = stepper.get_axis_position_degrees(A_AXIS),
+          oldB = stepper.get_axis_position_degrees(B_AXIS);
+  #endif
+
   for (uint16_t i = 1; i < segments; i++) { // Iterate (segments-1) times
 
     thermalManager.manage_heater();
       r_Q = -offset[0] * sin_Ti - offset[1] * cos_Ti;
     }
 
-    // Update arc_target location
-    arc_target[p_axis] = center_P + r_P;
-    arc_target[q_axis] = center_Q + r_Q;
-    arc_target[l_axis] += linear_per_segment;
-    arc_target[E_AXIS] += extruder_per_segment;
-
-    clamp_to_software_endstops(arc_target);
-
-    planner.buffer_line_kinematic(arc_target, fr_mm_s, active_extruder);
+    // Update raw location
+    raw[p_axis] = center_P + r_P;
+    raw[q_axis] = center_Q + r_Q;
+    raw[l_axis] += linear_per_segment;
+    raw[E_AXIS] += extruder_per_segment;
+
+    clamp_to_software_endstops(raw);
+
+    #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);
+    planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], cart[Z_AXIS], cart[E_AXIS], HYPOT(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB) * 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

Marlin/src/module/delta.cpp

   }while(0)
 
 void inverse_kinematics(const float raw[XYZ]) {
-  DELTA_RAW_IK();
+  DELTA_IK(raw);
   // DELTA_DEBUG();
 }
 

Marlin/src/module/delta.h

 #endif
 
 // Macro to obtain the Z position of an individual tower
-#define DELTA_Z(T) raw[Z_AXIS] + _SQRT(     \
-  delta_diagonal_rod_2_tower[T] - HYPOT2(   \
-      delta_tower[T][X_AXIS] - raw[X_AXIS], \
-      delta_tower[T][Y_AXIS] - raw[Y_AXIS]  \
-    )                                       \
+#define DELTA_Z(V,T) V[Z_AXIS] + _SQRT(   \
+  delta_diagonal_rod_2_tower[T] - HYPOT2( \
+      delta_tower[T][X_AXIS] - V[X_AXIS], \
+      delta_tower[T][Y_AXIS] - V[Y_AXIS]  \
+    )                                     \
   )
 
-#define DELTA_RAW_IK() do {        \
-  delta[A_AXIS] = DELTA_Z(A_AXIS); \
-  delta[B_AXIS] = DELTA_Z(B_AXIS); \
-  delta[C_AXIS] = DELTA_Z(C_AXIS); \
+#define DELTA_IK(V) do {        \
+  delta[A_AXIS] = DELTA_Z(V, A_AXIS); \
+  delta[B_AXIS] = DELTA_Z(V, B_AXIS); \
+  delta[C_AXIS] = DELTA_Z(V, C_AXIS); \
 }while(0)
 
 void inverse_kinematics(const float raw[XYZ]);

Marlin/src/module/motion.cpp

     // SERIAL_ECHOPAIR(" seconds=", seconds);
     // SERIAL_ECHOLNPAIR(" segments=", segments);
 
-    #if IS_SCARA && ENABLED(SCARA_FEEDRATE_SCALING)
+    #if ENABLED(SCARA_FEEDRATE_SCALING)
       // SCARA needs to scale the feed rate from mm/s to degrees/s
       const float inv_segment_length = min(10.0, float(segments) / cartesian_mm), // 1/mm/segs
                   inverse_secs = inv_segment_length * _feedrate_mm_s;
       }
 
       LOOP_XYZE(i) raw[i] += segment_distance[i];
+
       #if ENABLED(DELTA)
-        DELTA_RAW_IK(); // Delta can inline its kinematics
+        DELTA_IK(raw); // Delta can inline its kinematics
       #else
         inverse_kinematics(raw);
       #endif
-
       ADJUST_DELTA(raw); // Adjust Z if bed leveling is enabled
 
-      #if IS_SCARA && ENABLED(SCARA_FEEDRATE_SCALING)
+      #if ENABLED(SCARA_FEEDRATE_SCALING)
         // For SCARA scale the feed rate from mm/s to degrees/s
-        // Use ratio between the length of the move and the larger angle change
-        const float adiff = abs(delta[A_AXIS] - oldA),
-                    bdiff = abs(delta[B_AXIS] - oldB);
-        planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], max(adiff, bdiff) * inverse_secs, active_extruder);
-        oldA = delta[A_AXIS];
-        oldB = delta[B_AXIS];
+        // 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);
+        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);
+        planner.buffer_line(delta[A_AXIS], delta[B_AXIS], raw[Z_AXIS], raw[E_AXIS], _feedrate_mm_s, active_extruder);
       #endif
     }
 
-    // Since segment_distance is only approximate,
-    // the final move must be to the exact destination.
-
-    #if IS_SCARA && ENABLED(SCARA_FEEDRATE_SCALING)
-      // For SCARA scale the feed rate from mm/s to degrees/s
-      // With segments > 1 length is 1 segment, otherwise total length
+    // Ensure last segment arrives at target location.
+    #if ENABLED(SCARA_FEEDRATE_SCALING)
       inverse_kinematics(rtarget);
       ADJUST_DELTA(rtarget);
-      const float adiff = abs(delta[A_AXIS] - oldA),
-                  bdiff = abs(delta[B_AXIS] - oldB);
-      planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], max(adiff, bdiff) * inverse_secs, active_extruder);
+      planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], rtarget[Z_AXIS], rtarget[E_AXIS], HYPOT(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB) * inverse_secs, active_extruder);
     #else
       planner.buffer_line_kinematic(rtarget, _feedrate_mm_s, active_extruder);
     #endif