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,
+      inverse_kinematics(homeposition);
-     * and calculate homing offset using forward kinematics
+      forward_kinematics_SCARA(delta.a, delta.b);
-     */
+      current_position[axis] = cartes[axis];
-    inverse_kinematics(homeposition);
+    #else
-    forward_kinematics_SCARA(delta.a, delta.b);
+      // MP_SCARA uses a Cartesian XY home position
-
+      // SERIAL_ECHOLNPAIR("homeposition X:", homeposition.x, " Y:", homeposition.y);
-    // SERIAL_ECHOLNPAIR("Cartesian X:", cartes.x, " Y:", cartes.y);
+      current_position[axis] = homeposition[axis];
-
+    #endif
-    current_position[axis] = cartes[axis];
+
-
+    // SERIAL_ECHOLNPAIR("Cartesian X:", current_position.x, " Y:", current_position.y);
     update_software_endstops(axis);
   }
 }
@@ -92,48 +92,67 @@ void forward_kinematics_SCARA(const float &a, const float &b) {
   //*/
 }
 
-/**
- * Morgan SCARA Inverse Kinematics. Results in 'delta'.
- *
- * See http://forums.reprap.org/read.php?185,283327
- *
- * 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;
+  #if ENABLED(MORGAN_SCARA)
+  
+    /**
+     * Morgan SCARA Inverse Kinematics. Results in 'delta'.
+     *
+     * See http://forums.reprap.org/read.php?185,283327
+     *
+     * Maths and first version by QHARLEY.
+     * Integrated into Marlin and slightly restructured by Joachim Cerny.
+     */
+    float C2, S2, SK1, SK2, THETA, PSI;
 
-  // Translate SCARA to standard XY with scaling factor
+    // Translate SCARA to standard XY with scaling factor
-  const xy_pos_t spos = raw - scara_offset;
+    const xy_pos_t spos = raw - scara_offset;
 
-  const float H2 = HYPOT2(spos.x, spos.y);
+    const float H2 = HYPOT2(spos.x, spos.y);
-  if (L1 == L2)
+    if (L1 == L2)
-    C2 = H2 / L1_2_2 - 1;
+      C2 = H2 / L1_2_2 - 1;
-  else
+    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));
+    S2 = SQRT(1.0f - sq(C2));
 
-  // Unrotated Arm1 plus rotated Arm2 gives the distance from Center to End
+    // Unrotated Arm1 plus rotated Arm2 gives the distance from Center to End
-  SK1 = L1 + L2 * C2;
+    SK1 = L1 + L2 * C2;
 
-  // Rotated Arm2 gives the distance from Arm1 to Arm2
+    // Rotated Arm2 gives the distance from Arm1 to Arm2
-  SK2 = L2 * S2;
+    SK2 = L2 * S2;
 
-  // Angle of Arm1 is the difference between Center-to-End angle and the Center-to-Elbow
+    // Angle of Arm1 is the difference between Center-to-End angle and the Center-to-Elbow
-  THETA = ATAN2(SK1, SK2) - ATAN2(spos.x, spos.y);
+    THETA = ATAN2(SK1, SK2) - ATAN2(spos.x, spos.y);
 
-  // Angle of Arm2
+    // Angle of Arm2
-  PSI = ATAN2(S2, C2);
+    PSI = ATAN2(S2, C2);
 
-  delta.set(DEGREES(THETA), DEGREES(THETA + PSI), raw.z);
+    delta.set(DEGREES(THETA), DEGREES(THETA + PSI), raw.z);
 
-  /*
+    /*
-    DEBUG_POS("SCARA IK", raw);
+      DEBUG_POS("SCARA IK", raw);
-    DEBUG_POS("SCARA IK", delta);
+      DEBUG_POS("SCARA IK", delta);
-    SERIAL_ECHOLNPAIR("  SCARA (x,y) ", sx, ",", sy, " C2=", C2, " S2=", S2, " Theta=", THETA, " Phi=", PHI);
+      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/Configuration.h → 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)
 
-  // Radius around the center where the arm cannot reach
+    //#define DEBUG_SCARA_KINEMATICS
-  #define MIDDLE_DEAD_ZONE_R 0 //mm
+    #define SCARA_FEEDRATE_SCALING  // Convert XY feedrate from mm/s to degrees/s on the fly
 
-  #define THETA_HOMING_OFFSET 0  //calculatated from Calibration Guide and command M360 / M114 see picture in http://reprap.harleystudio.co.za/?page_id=1073
+    // Radius around the center where the arm cannot reach
-  #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 MIDDLE_DEAD_ZONE_R   0  // (mm)
+
+    #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  // 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