Mostly Printed SCARA (MPSCARA) support (#15573)

Marlin/src/core/macros.h

@@ -251,6 +251,7 @@
 //
 // Maths macros that can be overridden by HAL
 //
+#define ACOS(x)     acosf(x)
 #define ATAN2(y, x) atan2f(y, x)
 #define POW(x, y)   powf(x, y)
 #define SQRT(x)     sqrtf(x)

Marlin/src/module/scara.cpp

@@ -45,19 +45,19 @@ void scara_set_axis_is_at_home(const AxisEnum axis) {
     xyz_pos_t homeposition;
     LOOP_XYZ(i) homeposition[i] = base_home_pos((AxisEnum)i);
 
-    // SERIAL_ECHOLNPAIR("homeposition X:", homeposition.x, " Y:", homeposition.y);
+    #if ENABLED(MORGAN_SCARA)
-
+      // MORGAN_SCARA uses arm angles for AB home position
-    /**
+      // SERIAL_ECHOLNPAIR("homeposition A:", homeposition.a, " B:", homeposition.b);
-     * Get Home position SCARA arm angles using inverse kinematics,
-     * and calculate homing offset using forward kinematics
-     */
       inverse_kinematics(homeposition);
       forward_kinematics_SCARA(delta.a, delta.b);
-
-    // SERIAL_ECHOLNPAIR("Cartesian X:", cartes.x, " Y:", cartes.y);
-
       current_position[axis] = cartes[axis];
+    #else
+      // MP_SCARA uses a Cartesian XY home position
+      // SERIAL_ECHOLNPAIR("homeposition X:", homeposition.x, " Y:", homeposition.y);
+      current_position[axis] = homeposition[axis];
+    #endif
 
+    // SERIAL_ECHOLNPAIR("Cartesian X:", current_position.x, " Y:", current_position.y);
     update_software_endstops(axis);
   }
 }
@@ -92,7 +92,11 @@ void forward_kinematics_SCARA(const float &a, const float &b) {
   //*/
 }
 
-/**
+void inverse_kinematics(const xyz_pos_t &raw) {
+
+  #if ENABLED(MORGAN_SCARA)
+  
+    /**
      * Morgan SCARA Inverse Kinematics. Results in 'delta'.
      *
      * See http://forums.reprap.org/read.php?185,283327
@@ -100,8 +104,6 @@ void forward_kinematics_SCARA(const float &a, const float &b) {
      * Maths and first version by QHARLEY.
      * Integrated into Marlin and slightly restructured by Joachim Cerny.
      */
-void inverse_kinematics(const xyz_pos_t &raw) {
-
     float C2, S2, SK1, SK2, THETA, PSI;
 
     // Translate SCARA to standard XY with scaling factor
@@ -111,7 +113,7 @@ void inverse_kinematics(const xyz_pos_t &raw) {
     if (L1 == L2)
       C2 = H2 / L1_2_2 - 1;
     else
-    C2 = (H2 - (L1_2 + L2_2)) / (2.0 * L1 * L2);
+      C2 = (H2 - (L1_2 + L2_2)) / (2.0f * L1 * L2);
 
     S2 = SQRT(1.0f - sq(C2));
 
@@ -134,6 +136,23 @@ void inverse_kinematics(const xyz_pos_t &raw) {
       DEBUG_POS("SCARA IK", delta);
       SERIAL_ECHOLNPAIR("  SCARA (x,y) ", sx, ",", sy, " C2=", C2, " S2=", S2, " Theta=", THETA, " Phi=", PHI);
     //*/
+
+  #else // MP_SCARA
+
+    const float x = raw.x, y = raw.y, c = HYPOT(x, y),
+                THETA3 = ATAN2(y, x),
+                THETA1 = THETA3 + ACOS((sq(c) + sq(L1) - sq(L2)) / (2.0f * c * L1)),
+                THETA2 = THETA3 - ACOS((sq(c) + sq(L2) - sq(L1)) / (2.0f * c * L2));
+
+    delta.set(DEGREES(THETA1), DEGREES(THETA2), raw.z);
+
+    /*
+      DEBUG_POS("SCARA IK", raw);
+      DEBUG_POS("SCARA IK", delta);
+      SERIAL_ECHOLNPAIR("  SCARA (x,y) ", x, ",", y," Theta1=", THETA1, " Theta2=", THETA2);
+    //*/
+
+  #endif // MP_SCARA
 }
 
 void scara_report_positions() {

config/examples/SCARA/Morgan/Configuration.h

@@ -62,32 +62,45 @@
  * MORGAN_SCARA was developed by QHARLEY in South Africa in 2012-2013.
  * Implemented and slightly reworked by JCERNY in June, 2014.
  */
-
-// Specify the specific SCARA model
 #define MORGAN_SCARA
 
-#if ENABLED(MORGAN_SCARA)
+/**
-
+ * Mostly Printed SCARA is an open source design by Tyler Williams. See:
-  //#define DEBUG_SCARA_KINEMATICS
+ *   https://www.thingiverse.com/thing:2487048
-  #define SCARA_FEEDRATE_SCALING // Convert XY feedrate from mm/s to degrees/s on the fly
+ *   https://www.thingiverse.com/thing:1241491
+ */
+//#define MP_SCARA
 
+#if EITHER(MORGAN_SCARA, MP_SCARA)
   // If movement is choppy try lowering this value
   #define SCARA_SEGMENTS_PER_SECOND 200
 
   // Length of inner and outer support arms. Measure arm lengths precisely.
-  #define SCARA_LINKAGE_1 150 //mm
+  #define SCARA_LINKAGE_1 150       // (mm)
-  #define SCARA_LINKAGE_2 150 //mm
+  #define SCARA_LINKAGE_2 150       // (mm)
 
   // SCARA tower offset (position of Tower relative to bed zero position)
   // This needs to be reasonably accurate as it defines the printbed position in the SCARA space.
-  #define SCARA_OFFSET_X 100 //mm
+  #define SCARA_OFFSET_X  100       // (mm)
-  #define SCARA_OFFSET_Y -56 //mm
+  #define SCARA_OFFSET_Y  -56       // (mm)
+
+  #if ENABLED(MORGAN_SCARA)
+
+    //#define DEBUG_SCARA_KINEMATICS
+    #define SCARA_FEEDRATE_SCALING  // Convert XY feedrate from mm/s to degrees/s on the fly
 
     // Radius around the center where the arm cannot reach
-  #define MIDDLE_DEAD_ZONE_R 0 //mm
+    #define MIDDLE_DEAD_ZONE_R   0  // (mm)
 
-  #define THETA_HOMING_OFFSET 0  //calculatated from Calibration Guide and command M360 / M114 see picture in http://reprap.harleystudio.co.za/?page_id=1073
+    #define THETA_HOMING_OFFSET  0  // Calculated from Calibration Guide and M360 / M114. See http://reprap.harleystudio.co.za/?page_id=1073
-  #define PSI_HOMING_OFFSET   0  //calculatated from Calibration Guide and command M364 / M114 see picture in http://reprap.harleystudio.co.za/?page_id=1073
+    #define PSI_HOMING_OFFSET    0  // Calculated from Calibration Guide and M364 / M114. See http://reprap.harleystudio.co.za/?page_id=1073
+
+  #elif ENABLED(MP_SCARA)
+
+    #define SCARA_OFFSET_THETA1  12 // degrees
+    #define SCARA_OFFSET_THETA2 131 // degrees
+
+  #endif
 
 #endif