Mostly Printed SCARA (MPSCARA) support

[gouache]

Signed-off-by: brian park gouache95@gmail.com

Requirements

I've done working for MP_SCARA something like below in thingiverse
https://www.thingiverse.com/thing:2487048
https://www.thingiverse.com/thing:1241491

there are some tasks (auto home, kinematics, calibration) but this is the first step to add MP_SCARA.

Description

• separate directory from MORGAN_SCARA
• use MP_SCARA LINKAGE length (98.41 mm, 100.66 mm)
• change SCARA_OFFSET_X and SCARA_OFFSET_Y unit to degree for G28

here's some demo clip
https://youtu.be/vdVfJpOJ__g

Benefits

mpscara support

Related Issues

auto home, kinematics, calibration

[thinkyhead]

I have this pile of SCARA kinematics code that is similar but not identical to the usual. I wonder if it is any better or any worse than what is there now…?

Alternative SCARA Kinematics

  void inverse_kinematics(const float cart[XYZ], const float probe_y_offset) {

    // Translate the Cartesian position to SCARA space
    const float sx = cart[X_AXIS] - SCARA_OFFSET_X,
                sy = cart[Y_AXIS] - SCARA_OFFSET_Y;

    // Square of the distance from center to target
    const float D2 = HYPOT2(sx, sy);

    // Calculate angles. Use simplified equations for equal length arms
    float angA, angB;
    if (probe_y_offset) {
      float p2 = L2 + probe_y_offset, p2_2 = sq(p2);
      angA = acos((D2 + p2_2 - L1_2) / (2 * p2 * SQRT(D2)));
      angB = acos((D2 - p2_2 - L1_2) / (2 * L1 * p2));
    }
    else if (L1 != L2) {
      angA = acos((D2 + L2_2 - L1_2) / (2 * L2 * SQRT(D2)));
      angB = acos((D2 - L2_2 - L1_2) / (2 * L1 * L2));
    }
    else {
      angA = acos(D2 / (2 * L1 * SQRT(D2)));
      angB = acos((D2 - L1_2_2) / L1_2_2);
    }

    delta[A_AXIS] = DEGREES(ATAN2(sy, sx) + (arm_orientation ? angA - angB : angB - angA));
    delta[B_AXIS] = DEGREES(arm_orientation ? angB : -angB);

    // Only allow A angles from -90 to +270
    if (delta[A_AXIS] < 0) delta[A_AXIS] += 360.0;
    if (delta[A_AXIS] > 270) delta[A_AXIS] -= 360.0;

    // Return the cartesian Z as-is
    delta[C_AXIS] = cart[Z_AXIS];
  }

  void forward_kinematics_SCARA(const float &a, const float &b) {

    // Sine for Y offsets, Cosine for X offsets
    float sin_a  = sin(RADIANS(a)),   cos_a  = cos(RADIANS(a)),
          sin_ab = sin(RADIANS(a+b)), cos_ab = cos(RADIANS(a+b));

    if (L1 == L2) {
      // x = L × (cos(ϕ) + cos(ϕ + θ))
      // y = L × (sin(ϕ) + sin(ϕ + θ))
      cartes[X_AXIS] = L1 * (cos_a + cos_ab) + SCARA_OFFSET_X;
      cartes[Y_AXIS] = L1 * (sin_a + sin_ab) + SCARA_OFFSET_Y;
    }
    else {
      // x = Ls × cos(ϕ) + Le × cos(ϕ + θ)
      // y = Ls × sin(ϕ) + Le × sin(ϕ + θ)
      cartes[X_AXIS] = L1 * cos_a + L2 * cos_ab + SCARA_OFFSET_X;
      cartes[Y_AXIS] = L1 * sin_a + L2 * sin_ab + SCARA_OFFSET_Y;
    }

    cartes[Z_AXIS] = current_position[Z_AXIS];
  }

[gouache]

I wrote my kinematics code referring to this document.
http://www.deltatau.com/Common/technotes/SCARA%20Robot%20Kinematics.pdf
and mpscara's convert code (https://www.thingiverse.com/thing:2487048/files)
https://github.com/gouache/mpscara/blob/master/Gcode_Translator.py

not only inverse but also forward kinematic is required. you can compare with my code.
I didn't add scara.cpp & .h files yet. coz configuration files should be merged first
and autohome code is more important and difficult than kinematics. I'll make another pull requests right after this review.

I think it's better to discuss about kinematics in depth at next pull requests.

Kinematic Code

#include "../inc/MarlinConfig.h"

#if IS_SCARA

#include "scara.h"
#include "motion.h"
#include "planner.h"

float delta_segments_per_second = SCARA_SEGMENTS_PER_SECOND;

#if ENABLED(MORGAN_SCARA)
void scara_set_axis_is_at_home(const AxisEnum axis) {
  if (axis == Z_AXIS)
    current_position[Z_AXIS] = Z_HOME_POS;
  else {

    /**
     * SCARA homes XY at the same time
     */
    float homeposition[XYZ];
    LOOP_XYZ(i) homeposition[i] = base_home_pos((AxisEnum)i);

    SERIAL_ECHOLNPAIR("homeposition X:", homeposition[X_AXIS], " Y:", homeposition[Y_AXIS]);

    /**
     * Get Home position SCARA arm angles using inverse kinematics,
     * and calculate homing offset using forward kinematics
     */
    inverse_kinematics(homeposition);
    forward_kinematics_SCARA(delta[A_AXIS], delta[B_AXIS]);

    SERIAL_ECHOLNPAIR("Cartesian X:", cartes[X_AXIS], " Y:", cartes[Y_AXIS]);

    current_position[axis] = cartes[axis];

    update_software_endstops(axis);
  }
}

/**
 * Morgan SCARA Forward Kinematics. Results in cartes[].
 * Maths and first version by QHARLEY.
 * Integrated into Marlin and slightly restructured by Joachim Cerny.
 */
void forward_kinematics_SCARA(const float &a, const float &b) {

  const float a_sin = sin(RADIANS(a)) * L1,
              a_cos = cos(RADIANS(a)) * L1,
              b_sin = sin(RADIANS(b)) * L2,
              b_cos = cos(RADIANS(b)) * L2;

  cartes[X_AXIS] = a_cos + b_cos + SCARA_OFFSET_X;  //theta
  cartes[Y_AXIS] = a_sin + b_sin + SCARA_OFFSET_Y;  //theta+phi

  /*
    SERIAL_ECHOLNPAIR(
      "SCARA FK Angle a=", a,
      " b=", b,
      " a_sin=", a_sin,
      " a_cos=", a_cos,
      " b_sin=", b_sin,
      " b_cos=", b_cos
    );
    SERIAL_ECHOLNPAIR(" cartes (X,Y) = "(cartes[X_AXIS], ", ", cartes[Y_AXIS], ")");
  //*/
}

/**
 * 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 float (&raw)[XYZ]) {

  static float C2, S2, SK1, SK2, THETA, PSI;

  float sx = raw[X_AXIS] - SCARA_OFFSET_X,  // Translate SCARA to standard X Y
        sy = raw[Y_AXIS] - SCARA_OFFSET_Y;  // With scaling factor.

  if (L1 == L2)
    C2 = HYPOT2(sx, sy) / L1_2_2 - 1;
  else
    C2 = (HYPOT2(sx, sy) - (L1_2 + L2_2)) / (2.0 * L1 * L2);

  S2 = SQRT(1 - sq(C2));

  // Unrotated Arm1 plus rotated Arm2 gives the distance from Center to End
  SK1 = L1 + L2 * C2;

  // Rotated Arm2 gives the distance from Arm1 to Arm2
  SK2 = L2 * S2;

  // Angle of Arm1 is the difference between Center-to-End angle and the Center-to-Elbow
  THETA = ATAN2(SK1, SK2) - ATAN2(sx, sy);

  // Angle of Arm2
  PSI = ATAN2(S2, C2);

  delta[A_AXIS] = DEGREES(THETA);        // theta is support arm angle
  delta[B_AXIS] = DEGREES(THETA + PSI);  // equal to sub arm angle (inverted motor)
  delta[C_AXIS] = raw[Z_AXIS];

  /*
    DEBUG_POS("SCARA IK", raw);
    DEBUG_POS("SCARA IK", delta);
    SERIAL_ECHOLNPAIR("  SCARA (x,y) ", sx, ",", sy, " C2=", C2, " S2=", S2, " Theta=", THETA, " Phi=", PHI);
  //*/
}

#elif ENABLED(MP_SCARA)

void scara_set_axis_is_at_home(const AxisEnum axis) {
  if (axis == Z_AXIS)
    current_position[Z_AXIS] = Z_HOME_POS;
  else {

    /**
     * SCARA homes XY at the same time
     */
    float homeposition[XYZ];
    LOOP_XYZ(i) homeposition[i] = base_home_pos((AxisEnum)i);

    SERIAL_ECHOLNPAIR("homeposition X:", homeposition[X_AXIS], " Y:", homeposition[Y_AXIS]);
    current_position[axis] = homeposition[axis];

    update_software_endstops(axis);
  }
}

void forward_kinematics_SCARA(const float &a, const float &b) {
  const float a_sin = sin(RADIANS(a)) * L1,
              a_cos = cos(RADIANS(a)) * L1,
              b_sin = sin(RADIANS(a + b)) * L2,
              b_cos = cos(RADIANS(a + b)) * L2;

  cartes[X_AXIS] = a_cos + b_cos;
  cartes[Y_AXIS] = a_sin + b_sin;

  SERIAL_ECHOLNPAIR(
    "SCARA FK Angle a=", a,
    " b=", b,
    " a_sin=", a_sin,
    " a_cos=", a_cos,
    " b_sin=", b_sin,
    " b_cos=", b_cos
  );
  SERIAL_ECHOLNPAIR(" cartes (X,Y) = ", cartes[X_AXIS], ", ", cartes[Y_AXIS], ")");
  SERIAL_EOL();
}

void inverse_kinematics(const float (&raw)[XYZ]) {
  static float THETA1, THETA2, THETA3;

  float x = raw[X_AXIS];
  float y = raw[Y_AXIS];
  float c = SQRT(POW(x, 2) + POW(y, 2));

  // Angle of Arm1 is the difference between Center-to-End angle and the Center-to-Elbow
  THETA3 = ATAN2(y, x);

  THETA1 = THETA3 + ACOS((POW(c, 2) + POW(L1, 2) - POW(L2, 2)) / (2 * c * L1));
  THETA2 = THETA3 - ACOS((POW(c, 2) + POW(L2, 2) - POW(L1, 2)) / (2 * c * L2));

  delta[A_AXIS] = DEGREES(THETA1);        // theta is support arm angle
  delta[B_AXIS] = DEGREES(THETA2);  // equal to sub arm angle (inverted motor)
  delta[C_AXIS] = raw[Z_AXIS];

  SERIAL_ECHOLNPAIR("  SCARA (x,y) ", x, ",", y," Theta1=", THETA1, " Theta2=", THETA2);
  SERIAL_EOL();

}
#endif

void scara_report_positions() {
  SERIAL_ECHOLNPAIR("SCARA Theta:", planner.get_axis_position_degrees(A_AXIS), "  Psi+Theta:", planner.get_axis_position_degrees(B_AXIS));
  SERIAL_EOL();
}

#endif // IS_SCARA

[gouache]

amend for adding scara.cpp file (kinematics)

[gouache]

I missed ACOS macro before. so amend it again.

[svsergo]

In the version Marlin-bugfix-2.0.x for SCARA does not work QUICK_HOME. Did you solve the problem?

[gouache]

In the version Marlin-bugfix-2.0.x for SCARA does not work QUICK_HOME. Did you solve the problem?

MP_SCARA does not use QUICK_HOME differently than MORGAN_SCARA. I'll continue to make another pull request for autohome patch for MP_SCARA after this review.

[svsergo]

I use an extra downshift to rotate the arm. But this requires compensation for the rotation of the hand and the simultaneous operation of the two engines. I wanted to use QUICK_HOME for this.

…

Вторник, 19 ноября 2019, 16:08 +04:00 от brian park ***@***.***>: >In the version Marlin-bugfix-2.0.x for SCARA does not work QUICK_HOME. Did you solve the problem? MP_SCARA does not use QUICK_HOME differently than MORGAN_SCARA. I'll continue to make another pull request for autohome patch for MP_SCARA after this review. — You are receiving this because you commented. Reply to this email directly, view it on GitHub , or unsubscribe .

[rwmech]

@gouache - I've been working on MPSCARA from a hardware perspective and noticed that 2.0 was not working with this. I would like to offer up my testing & coding skills on this if you guys need assistance.

[thinkyhead]

MP_SCARA does not use QUICK_HOME differently than MORGAN_SCARA. I'll continue to make another pull request for autohome patch for MP_SCARA after this review.

SCARA should home Z first, then it should do a "quick home" on the AB axes. Here's some slightly older SCARA homing code that I suggest using as a template. The base angles and motion range might seem unusual, but they make sense for the nearly 360° motion of the machine it was written for….

SCARA Home AB Code

+#elif IS_SCARA

  /**
   * A SCARA homes each arm segment
   * Endstops should already be set to interrupt the move.
   */

  constexpr float SCARA_HOME_ANGLE_A = 180.0,
                  SCARA_HOME_ANGLE_B = 147.5;

  inline bool home_scara_ab(const bool homeA, const bool homeB) {
    #if ENABLED(DEBUG_LEVELING_FEATURE)
      if (DEBUGGING(LEVELING)) DEBUG_POS(">>> home_scara_ab", current_position);
    #endif

    // A and B home in the negative

    // Start the angles at A-180 B0
    if (homeA) stepper.set_position(A_AXIS, -130 * X_HOME_DIR);
    if (homeB) stepper.set_position(B_AXIS, -155 * Y_HOME_DIR);

    // Move A and B as far as they can go
    float target[ABCE] = {
      homeA ? 260 * X_HOME_DIR : planner.get_axis_position_degrees(A_AXIS),
      homeB ? 155 * Y_HOME_DIR : planner.get_axis_position_degrees(B_AXIS),
      current_position[Z_AXIS], current_position[E_AXIS]
    };

    // Move A and/or B until an endstop is hit.
    feedrate_mm_s = homing_feedrate(A_AXIS);
    plan_direct_stepper_move(target);
    planner.synchronize();

    // Get cartesian from steppers and tell the planner
    set_current_from_steppers_for_axis(ALL_AXES);
    SYNC_PLAN_POSITION_KINEMATIC();

    // After a move interruption we need to reset the
    //target[A_AXIS] = planner.get_axis_position_degrees(A_AXIS);
    //target[B_AXIS] = planner.get_axis_position_degrees(B_AXIS);

    // If only one was triggered, finish the other
    const uint8_t hits = endstops.trigger_state();
    if (homeA && homeB && TEST(hits, X_MAX) != TEST(hits, Y_MAX)) {
      const AxisEnum axis = TEST(hits, X_MAX) ? A_AXIS : B_AXIS;
      target[axis] = planner.get_axis_position_degrees(axis); // hit axis stays still
      plan_direct_stepper_move(target);
    }

    endstops.hit_on_purpose();

    if (homeA) {
      axis_homed[A_AXIS] = axis_known_position[A_AXIS] = true;
      stepper.set_position(A_AXIS, SCARA_HOME_ANGLE_A * planner.axis_steps_per_mm[A_AXIS]);
    }
    if (homeB) {
      axis_homed[B_AXIS] = axis_known_position[B_AXIS] = true;
      stepper.set_position(B_AXIS, SCARA_HOME_ANGLE_B * planner.axis_steps_per_mm[B_AXIS]);
    }

    // Work backward from known home angles to Cartesian XYZ
    arm_orientation = Y_HOME_DIR > 0 ? RIGHT_ARM : LEFT_ARM;
    set_current_from_steppers_for_axis(ALL_AXES);
    SYNC_PLAN_POSITION_KINEMATIC();

    #if ENABLED(DEBUG_LEVELING_FEATURE)
      if (DEBUGGING(LEVELING)) DEBUG_POS("<<< home_scara_ab", current_position);
    #endif

    return true;
  }

#endif

…which is called from G28 sort of like this…

G28 SCARA Code

  #elif ENABLED(MP_SCARA)

    const bool homeA = always_home_all || parser.seen('X') || parser.seen('A'),
               homeB = always_home_all || parser.seen('Y') || parser.seen('B'),
               homeZ = always_home_all || parser.seen('Z'),
               home_all = (!homeA && !homeB && !homeZ) || (homeA && homeB && homeZ);

    // SCARA should always home Z first
    if (home_all || homeZ) {
      #if ENABLED(Z_SAFE_HOMING)
        home_z_safely();
      #else
        HOMEAXIS(Z);
      #endif
    }

    #if (HAS_X_MIN || HAS_X_MAX) && (HAS_Y_MIN || HAS_Y_MAX)
      home_scara_ab(home_all || homeA, home_all || homeB);
    #endif