From 6cfd190107e5a4aa3ad07dc1049b0bae1a101ce5 Mon Sep 17 00:00:00 2001
From: Tanguy Pruvot <tpruvot@users.noreply.github.com>
Date: Fri, 5 Mar 2021 00:34:38 +0100
Subject: [PATCH] Followup to MP_SCARA/TPARA patches (#21248)

---
 .../src/feature/bedlevel/ubl/ubl_motion.cpp   |   2 +-
 Marlin/src/gcode/calibrate/M665.cpp           |   4 +-
 Marlin/src/module/delta.cpp                   |   2 +-
 Marlin/src/module/delta.h                     |   2 +-
 Marlin/src/module/motion.cpp                  |   2 +-
 Marlin/src/module/scara.cpp                   | 101 +++++++++++-------
 Marlin/src/module/scara.h                     |   2 +-
 Marlin/src/module/settings.cpp                |  12 +--
 8 files changed, 74 insertions(+), 53 deletions(-)

diff --git a/Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp b/Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp
index 8b7cd15a3c6..33b4f03ac29 100644
--- a/Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp
+++ b/Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp
@@ -335,7 +335,7 @@
 
     #if IS_KINEMATIC
       const float seconds = cart_xy_mm / scaled_fr_mm_s;                             // Duration of XY move at requested rate
-      uint16_t segments = LROUND(delta_segments_per_second * seconds),               // Preferred number of segments for distance @ feedrate
+      uint16_t segments = LROUND(segments_per_second * seconds),                     // Preferred number of segments for distance @ feedrate
                seglimit = LROUND(cart_xy_mm * RECIPROCAL(DELTA_SEGMENT_MIN_LENGTH)); // Number of segments at minimum segment length
       NOMORE(segments, seglimit);                                                    // Limit to minimum segment length (fewer segments)
     #else
diff --git a/Marlin/src/gcode/calibrate/M665.cpp b/Marlin/src/gcode/calibrate/M665.cpp
index 3eac54f266c..0d0c4146d93 100644
--- a/Marlin/src/gcode/calibrate/M665.cpp
+++ b/Marlin/src/gcode/calibrate/M665.cpp
@@ -48,7 +48,7 @@
     if (parser.seenval('H')) delta_height              = parser.value_linear_units();
     if (parser.seenval('L')) delta_diagonal_rod        = parser.value_linear_units();
     if (parser.seenval('R')) delta_radius              = parser.value_linear_units();
-    if (parser.seenval('S')) delta_segments_per_second = parser.value_float();
+    if (parser.seenval('S')) segments_per_second       = parser.value_float();
     if (parser.seenval('X')) delta_tower_angle_trim.a  = parser.value_float();
     if (parser.seenval('Y')) delta_tower_angle_trim.b  = parser.value_float();
     if (parser.seenval('Z')) delta_tower_angle_trim.c  = parser.value_float();
@@ -76,7 +76,7 @@
    *   B, T, and Y are all aliases for the elbow angle
    */
   void GcodeSuite::M665() {
-    if (parser.seenval('S')) delta_segments_per_second = parser.value_float();
+    if (parser.seenval('S')) segments_per_second = parser.value_float();
 
     #if HAS_SCARA_OFFSET
 
diff --git a/Marlin/src/module/delta.cpp b/Marlin/src/module/delta.cpp
index 9857b89bf3b..a1676b3ba34 100644
--- a/Marlin/src/module/delta.cpp
+++ b/Marlin/src/module/delta.cpp
@@ -54,7 +54,7 @@ float delta_height;
 abc_float_t delta_endstop_adj{0};
 float delta_radius,
       delta_diagonal_rod,
-      delta_segments_per_second;
+      segments_per_second;
 abc_float_t delta_tower_angle_trim;
 xy_float_t delta_tower[ABC];
 abc_float_t delta_diagonal_rod_2_tower;
diff --git a/Marlin/src/module/delta.h b/Marlin/src/module/delta.h
index a974da97c8b..9caec530288 100644
--- a/Marlin/src/module/delta.h
+++ b/Marlin/src/module/delta.h
@@ -32,7 +32,7 @@ extern float delta_height;
 extern abc_float_t delta_endstop_adj;
 extern float delta_radius,
              delta_diagonal_rod,
-             delta_segments_per_second;
+             segments_per_second;
 extern abc_float_t delta_tower_angle_trim;
 extern xy_float_t delta_tower[ABC];
 extern abc_float_t delta_diagonal_rod_2_tower;
diff --git a/Marlin/src/module/motion.cpp b/Marlin/src/module/motion.cpp
index 024f28dc9fe..2f4f5e283a3 100644
--- a/Marlin/src/module/motion.cpp
+++ b/Marlin/src/module/motion.cpp
@@ -763,7 +763,7 @@ FORCE_INLINE void segment_idle(millis_t &next_idle_ms) {
 
     // The number of segments-per-second times the duration
     // gives the number of segments
-    uint16_t segments = delta_segments_per_second * seconds;
+    uint16_t segments = segments_per_second * seconds;
 
     // For SCARA enforce a minimum segment size
     #if IS_SCARA
diff --git a/Marlin/src/module/scara.cpp b/Marlin/src/module/scara.cpp
index 7586b8136fb..1767e230c15 100644
--- a/Marlin/src/module/scara.cpp
+++ b/Marlin/src/module/scara.cpp
@@ -37,46 +37,7 @@
   #include "../MarlinCore.h"
 #endif
 
-float delta_segments_per_second = TERN(AXEL_TPARA, TPARA_SEGMENTS_PER_SECOND, SCARA_SEGMENTS_PER_SECOND);
-
-void scara_set_axis_is_at_home(const AxisEnum axis) {
-  if (axis == Z_AXIS)
-    current_position.z = Z_HOME_POS;
-  else {
-    #if ENABLED(MORGAN_SCARA)
-      // MORGAN_SCARA uses arm angles for AB home position
-      ab_float_t homeposition = { SCARA_OFFSET_THETA1, SCARA_OFFSET_THETA2 };
-      //DEBUG_ECHOLNPAIR("homeposition A:", homeposition.a, " B:", homeposition.b);
-    #elif ENABLED(MP_SCARA)
-      // MP_SCARA uses a Cartesian XY home position
-      xyz_pos_t homeposition = { X_HOME_POS, Y_HOME_POS, Z_HOME_POS };
-      //DEBUG_ECHOPGM("homeposition");
-      //DEBUG_ECHOLNPAIR_P(SP_X_LBL, homeposition.x, SP_Y_LBL, homeposition.y);
-    #elif ENABLED(AXEL_TPARA)
-      xyz_pos_t homeposition = { X_HOME_POS, Y_HOME_POS, Z_HOME_POS };
-      //DEBUG_ECHOPGM("homeposition");
-      //DEBUG_ECHOLNPAIR_P(SP_X_LBL, homeposition.x, SP_Y_LBL, homeposition.y, SP_Z_LBL, homeposition.z);
-    #endif
-
-    #if ENABLED(MORGAN_SCARA)
-      delta = homeposition;
-    #else
-      inverse_kinematics(homeposition);
-    #endif
-
-    #if EITHER(MORGAN_SCARA, MP_SCARA)
-      forward_kinematics(delta.a, delta.b);
-    #elif ENABLED(AXEL_TPARA)
-      forward_kinematics(delta.a, delta.b, delta.c);
-    #endif
-
-    current_position[axis] = cartes[axis];
-
-    //DEBUG_ECHOPGM("Cartesian");
-    //DEBUG_ECHOLNPAIR_P(SP_X_LBL, current_position.x, SP_Y_LBL, current_position.y);
-    update_software_endstops(axis);
-  }
-}
+float segments_per_second = TERN(AXEL_TPARA, TPARA_SEGMENTS_PER_SECOND, SCARA_SEGMENTS_PER_SECOND);
 
 #if EITHER(MORGAN_SCARA, MP_SCARA)
 
@@ -109,6 +70,27 @@ void scara_set_axis_is_at_home(const AxisEnum axis) {
     //*/
   }
 
+#endif
+
+#if ENABLED(MORGAN_SCARA)
+
+  void scara_set_axis_is_at_home(const AxisEnum axis) {
+    if (axis == Z_AXIS)
+      current_position.z = Z_HOME_POS;
+    else {
+      // MORGAN_SCARA uses a Cartesian XY home position
+      xyz_pos_t homeposition = { X_HOME_POS, Y_HOME_POS, Z_HOME_POS };
+      //DEBUG_ECHOLNPAIR_P(PSTR("homeposition X"), homeposition.x, SP_Y_LBL, homeposition.y);
+
+      delta = homeposition;
+      forward_kinematics(delta.a, delta.b);
+      current_position[axis] = cartes[axis];
+
+      //DEBUG_ECHOLNPAIR_P(PSTR("Cartesian X"), current_position.x, SP_Y_LBL, current_position.y);
+      update_software_endstops(axis);
+    }
+  }
+
   /**
    * Morgan SCARA Inverse Kinematics. Results are stored in 'delta'.
    *
@@ -156,6 +138,29 @@ void scara_set_axis_is_at_home(const AxisEnum axis) {
 
 #elif ENABLED(MP_SCARA)
 
+  void scara_set_axis_is_at_home(const AxisEnum axis) {
+    if (axis == Z_AXIS)
+      current_position.z = Z_HOME_POS;
+    else {
+      // MP_SCARA uses arm angles for AB home position
+      #ifndef SCARA_OFFSET_THETA1
+        #define SCARA_OFFSET_THETA1  12 // degrees
+      #endif
+      #ifndef SCARA_OFFSET_THETA2
+        #define SCARA_OFFSET_THETA2 131 // degrees
+      #endif
+      ab_float_t homeposition = { SCARA_OFFSET_THETA1, SCARA_OFFSET_THETA2 };
+      //DEBUG_ECHOLNPAIR("homeposition A:", homeposition.a, " B:", homeposition.b);
+
+      inverse_kinematics(homeposition);
+      forward_kinematics(delta.a, delta.b);
+      current_position[axis] = cartes[axis];
+
+      //DEBUG_ECHOLNPAIR_P(PSTR("Cartesian X"), current_position.x, SP_Y_LBL, current_position.y);
+      update_software_endstops(axis);
+    }
+  }
+
   void inverse_kinematics(const xyz_pos_t &raw) {
     const float x = raw.x, y = raw.y, c = HYPOT(x, y),
                 THETA3 = ATAN2(y, x),
@@ -175,6 +180,22 @@ void scara_set_axis_is_at_home(const AxisEnum axis) {
 
   static constexpr xyz_pos_t robot_offset = { TPARA_OFFSET_X, TPARA_OFFSET_Y, TPARA_OFFSET_Z };
 
+  void scara_set_axis_is_at_home(const AxisEnum axis) {
+    if (axis == Z_AXIS)
+      current_position.z = Z_HOME_POS;
+    else {
+      xyz_pos_t homeposition = { X_HOME_POS, Y_HOME_POS, Z_HOME_POS };
+      //DEBUG_ECHOLNPAIR_P(PSTR("homeposition X"), homeposition.x, SP_Y_LBL, homeposition.y, SP_Z_LBL, homeposition.z);
+
+      inverse_kinematics(homeposition);
+      forward_kinematics(delta.a, delta.b, delta.c);
+      current_position[axis] = cartes[axis];
+
+      //DEBUG_ECHOLNPAIR_P(PSTR("Cartesian X"), current_position.x, SP_Y_LBL, current_position.y);
+      update_software_endstops(axis);
+    }
+  }
+
   // Convert ABC inputs in degrees to XYZ outputs in mm
   void forward_kinematics(const float &a, const float &b, const float &c) {
     const float w = c - b,
diff --git a/Marlin/src/module/scara.h b/Marlin/src/module/scara.h
index 5549e506b06..d24a4110fdc 100644
--- a/Marlin/src/module/scara.h
+++ b/Marlin/src/module/scara.h
@@ -27,7 +27,7 @@
 
 #include "../core/macros.h"
 
-extern float delta_segments_per_second;
+extern float segments_per_second;
 
 #if ENABLED(AXEL_TPARA)
 
diff --git a/Marlin/src/module/settings.cpp b/Marlin/src/module/settings.cpp
index d78d97e688e..c82ed0eb8f3 100644
--- a/Marlin/src/module/settings.cpp
+++ b/Marlin/src/module/settings.cpp
@@ -279,7 +279,7 @@ typedef struct SettingsDataStruct {
     abc_float_t delta_endstop_adj;                      // M666 X Y Z
     float delta_radius,                                 // M665 R
           delta_diagonal_rod,                           // M665 L
-          delta_segments_per_second;                    // M665 S
+          segments_per_second;                          // M665 S
     abc_float_t delta_tower_angle_trim,                 // M665 X Y Z
                 delta_diagonal_rod_trim;                // M665 A B C
   #elif HAS_EXTRA_ENDSTOPS
@@ -840,7 +840,7 @@ void MarlinSettings::postprocess() {
         EEPROM_WRITE(delta_endstop_adj);         // 3 floats
         EEPROM_WRITE(delta_radius);              // 1 float
         EEPROM_WRITE(delta_diagonal_rod);        // 1 float
-        EEPROM_WRITE(delta_segments_per_second); // 1 float
+        EEPROM_WRITE(segments_per_second);       // 1 float
         EEPROM_WRITE(delta_tower_angle_trim);    // 3 floats
         EEPROM_WRITE(delta_diagonal_rod_trim);   // 3 floats
 
@@ -1721,7 +1721,7 @@ void MarlinSettings::postprocess() {
           EEPROM_READ(delta_endstop_adj);         // 3 floats
           EEPROM_READ(delta_radius);              // 1 float
           EEPROM_READ(delta_diagonal_rod);        // 1 float
-          EEPROM_READ(delta_segments_per_second); // 1 float
+          EEPROM_READ(segments_per_second);       // 1 float
           EEPROM_READ(delta_tower_angle_trim);    // 3 floats
           EEPROM_READ(delta_diagonal_rod_trim);   // 3 floats
 
@@ -2711,7 +2711,7 @@ void MarlinSettings::reset() {
     delta_endstop_adj = adj;
     delta_radius = DELTA_RADIUS;
     delta_diagonal_rod = DELTA_DIAGONAL_ROD;
-    delta_segments_per_second = DELTA_SEGMENTS_PER_SECOND;
+    segments_per_second = DELTA_SEGMENTS_PER_SECOND;
     delta_tower_angle_trim = dta;
     delta_diagonal_rod_trim = ddr;
   #endif
@@ -3320,7 +3320,7 @@ void MarlinSettings::reset() {
       CONFIG_ECHO_HEADING("SCARA settings: S<seg-per-sec> P<theta-psi-offset> T<theta-offset>");
       CONFIG_ECHO_START();
       SERIAL_ECHOLNPAIR_P(
-          PSTR("  M665 S"), delta_segments_per_second
+          PSTR("  M665 S"), segments_per_second
         , SP_P_STR, scara_home_offset.a
         , SP_T_STR, scara_home_offset.b
         , SP_Z_STR, LINEAR_UNIT(scara_home_offset.z)
@@ -3342,7 +3342,7 @@ void MarlinSettings::reset() {
           PSTR("  M665 L"), LINEAR_UNIT(delta_diagonal_rod)
         , PSTR(" R"), LINEAR_UNIT(delta_radius)
         , PSTR(" H"), LINEAR_UNIT(delta_height)
-        , PSTR(" S"), delta_segments_per_second
+        , PSTR(" S"), segments_per_second
         , SP_X_STR, LINEAR_UNIT(delta_tower_angle_trim.a)
         , SP_Y_STR, LINEAR_UNIT(delta_tower_angle_trim.b)
         , SP_Z_STR, LINEAR_UNIT(delta_tower_angle_trim.c)