From 864d27d46079bb002a5ead776e056b48cf843f63 Mon Sep 17 00:00:00 2001 From: Axel Date: Wed, 3 Mar 2021 20:46:32 -0300 Subject: [PATCH] TPARA - 3DOF robot arm IK (#21005) Co-authored-by: Scott Lahteine --- Marlin/Configuration.h | 13 +- Marlin/src/core/macros.h | 1 + Marlin/src/gcode/calibrate/G28.cpp | 10 +- Marlin/src/gcode/calibrate/M665.cpp | 26 +-- Marlin/src/inc/Conditionals_LCD.h | 2 +- Marlin/src/inc/Conditionals_post.h | 4 +- Marlin/src/inc/SanityCheck.h | 4 +- Marlin/src/module/delta.cpp | 12 +- Marlin/src/module/motion.cpp | 19 +- Marlin/src/module/motion.h | 14 ++ Marlin/src/module/scara.cpp | 272 +++++++++++++++++++++------- Marlin/src/module/scara.h | 25 ++- 12 files changed, 285 insertions(+), 117 deletions(-) diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h index 46b70eb72..018c17166 100644 --- a/Marlin/Configuration.h +++ b/Marlin/Configuration.h @@ -58,18 +58,13 @@ */ //=========================================================================== -//============================= DELTA Printer =============================== +//========================== DELTA / SCARA / TPARA ========================== //=========================================================================== -// For a Delta printer, start with one of the configuration files in the config/examples/delta directory -// from https://github.com/MarlinFirmware/Configurations/branches/all and customize for your machine. // - -//=========================================================================== -//============================= SCARA Printer =============================== -//=========================================================================== -// For a SCARA printer, start with one of the configuration files in the config/examples/SCARA directory -// from https://github.com/MarlinFirmware/Configurations/branches/all and customize for your machine. +// Download configurations from the link above and customize for your machine. +// Examples are located in config/examples/delta, .../SCARA, and .../TPARA. // +//=========================================================================== // @section info diff --git a/Marlin/src/core/macros.h b/Marlin/src/core/macros.h index 9b07af361..d7043ba52 100644 --- a/Marlin/src/core/macros.h +++ b/Marlin/src/core/macros.h @@ -104,6 +104,7 @@ #define RADIANS(d) ((d)*float(M_PI)/180.0f) #define DEGREES(r) ((r)*180.0f/float(M_PI)) #define HYPOT2(x,y) (sq(x)+sq(y)) +#define NORMSQ(x,y,z) (sq(x)+sq(y)+sq(z)) #define CIRCLE_AREA(R) (float(M_PI) * sq(float(R))) #define CIRCLE_CIRC(R) (2 * float(M_PI) * float(R)) diff --git a/Marlin/src/gcode/calibrate/G28.cpp b/Marlin/src/gcode/calibrate/G28.cpp index 430daf6ba..947067887 100644 --- a/Marlin/src/gcode/calibrate/G28.cpp +++ b/Marlin/src/gcode/calibrate/G28.cpp @@ -311,7 +311,13 @@ void GcodeSuite::G28() { TERN_(IMPROVE_HOMING_RELIABILITY, end_slow_homing(slow_homing)); - #else // NOT DELTA + #elif ENABLED(AXEL_TPARA) + + constexpr bool doZ = true; // for NANODLP_Z_SYNC if your DLP is on a TPARA + + home_TPARA(); + + #else const bool homeZ = parser.seen('Z'), needX = homeZ && TERN0(Z_SAFE_HOMING, axes_should_home(_BV(X_AXIS))), @@ -392,7 +398,7 @@ void GcodeSuite::G28() { sync_plan_position(); - #endif // !DELTA (G28) + #endif /** * Preserve DXC mode across a G28 for IDEX printers in DXC_DUPLICATION_MODE. diff --git a/Marlin/src/gcode/calibrate/M665.cpp b/Marlin/src/gcode/calibrate/M665.cpp index 557204cc1..3eac54f26 100644 --- a/Marlin/src/gcode/calibrate/M665.cpp +++ b/Marlin/src/gcode/calibrate/M665.cpp @@ -40,21 +40,21 @@ * X = Alpha (Tower 1) angle trim * Y = Beta (Tower 2) angle trim * Z = Gamma (Tower 3) angle trim - * A = Alpha (Tower 1) digonal rod trim - * B = Beta (Tower 2) digonal rod trim - * C = Gamma (Tower 3) digonal rod trim + * A = Alpha (Tower 1) diagonal rod trim + * B = Beta (Tower 2) diagonal rod trim + * C = Gamma (Tower 3) diagonal rod trim */ void GcodeSuite::M665() { - if (parser.seen('H')) delta_height = parser.value_linear_units(); - if (parser.seen('L')) delta_diagonal_rod = parser.value_linear_units(); - if (parser.seen('R')) delta_radius = parser.value_linear_units(); - if (parser.seen('S')) delta_segments_per_second = parser.value_float(); - if (parser.seen('X')) delta_tower_angle_trim.a = parser.value_float(); - if (parser.seen('Y')) delta_tower_angle_trim.b = parser.value_float(); - if (parser.seen('Z')) delta_tower_angle_trim.c = parser.value_float(); - if (parser.seen('A')) delta_diagonal_rod_trim.a = parser.value_float(); - if (parser.seen('B')) delta_diagonal_rod_trim.b = parser.value_float(); - if (parser.seen('C')) delta_diagonal_rod_trim.c = parser.value_float(); + 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('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(); + if (parser.seenval('A')) delta_diagonal_rod_trim.a = parser.value_float(); + if (parser.seenval('B')) delta_diagonal_rod_trim.b = parser.value_float(); + if (parser.seenval('C')) delta_diagonal_rod_trim.c = parser.value_float(); recalc_delta_settings(); } diff --git a/Marlin/src/inc/Conditionals_LCD.h b/Marlin/src/inc/Conditionals_LCD.h index c8b683ecf..4931c767b 100644 --- a/Marlin/src/inc/Conditionals_LCD.h +++ b/Marlin/src/inc/Conditionals_LCD.h @@ -924,7 +924,7 @@ #define NORMAL_AXIS Z_AXIS #endif -#if ENABLED(MORGAN_SCARA) +#if EITHER(MORGAN_SCARA, AXEL_TPARA) #define IS_SCARA 1 #define IS_KINEMATIC 1 #elif ENABLED(DELTA) diff --git a/Marlin/src/inc/Conditionals_post.h b/Marlin/src/inc/Conditionals_post.h index c800a52fb..be0434120 100644 --- a/Marlin/src/inc/Conditionals_post.h +++ b/Marlin/src/inc/Conditionals_post.h @@ -143,7 +143,9 @@ */ #if IS_SCARA #undef SLOWDOWN - #define QUICK_HOME + #if DISABLED(AXEL_TPARA) + #define QUICK_HOME + #endif #define SCARA_PRINTABLE_RADIUS (SCARA_LINKAGE_1 + SCARA_LINKAGE_2) #endif diff --git a/Marlin/src/inc/SanityCheck.h b/Marlin/src/inc/SanityCheck.h index 3f7931e0b..ca09fad05 100644 --- a/Marlin/src/inc/SanityCheck.h +++ b/Marlin/src/inc/SanityCheck.h @@ -1227,8 +1227,8 @@ static_assert(Y_MAX_LENGTH >= Y_BED_SIZE, "Movement bounds (Y_MIN_POS, Y_MAX_POS /** * Allow only one kinematic type to be defined */ -#if MANY(DELTA, MORGAN_SCARA, COREXY, COREXZ, COREYZ, COREYX, COREZX, COREZY, MARKFORGED_XY) - #error "Please enable only one of DELTA, MORGAN_SCARA, COREXY, COREYX, COREXZ, COREZX, COREYZ, COREZY, or MARKFORGED_XY." +#if MANY(DELTA, MORGAN_SCARA, AXEL_TPARA, COREXY, COREXZ, COREYZ, COREYX, COREZX, COREZY, MARKFORGED_XY) + #error "Please enable only one of DELTA, MORGAN_SCARA, AXEL_TPARA, COREXY, COREYX, COREXZ, COREZX, COREYZ, COREZY, or MARKFORGED_XY." #endif /** diff --git a/Marlin/src/module/delta.cpp b/Marlin/src/module/delta.cpp index 93238a69e..565c67682 100644 --- a/Marlin/src/module/delta.cpp +++ b/Marlin/src/module/delta.cpp @@ -242,9 +242,9 @@ void home_delta() { // Disable stealthChop if used. Enable diag1 pin on driver. #if ENABLED(SENSORLESS_HOMING) - TERN_(X_SENSORLESS, sensorless_t stealth_states_x = start_sensorless_homing_per_axis(X_AXIS)); - TERN_(Y_SENSORLESS, sensorless_t stealth_states_y = start_sensorless_homing_per_axis(Y_AXIS)); - TERN_(Z_SENSORLESS, sensorless_t stealth_states_z = start_sensorless_homing_per_axis(Z_AXIS)); + TERN_(X_SENSORLESS, sensorless_t stealth_states_x = start_sensorless_homing_per_axis(X_AXIS)); + TERN_(Y_SENSORLESS, sensorless_t stealth_states_y = start_sensorless_homing_per_axis(Y_AXIS)); + TERN_(Z_SENSORLESS, sensorless_t stealth_states_z = start_sensorless_homing_per_axis(Z_AXIS)); #endif // Move all carriages together linearly until an endstop is hit. @@ -254,9 +254,9 @@ void home_delta() { // Re-enable stealthChop if used. Disable diag1 pin on driver. #if ENABLED(SENSORLESS_HOMING) - TERN_(X_SENSORLESS, end_sensorless_homing_per_axis(X_AXIS, stealth_states_x)); - TERN_(Y_SENSORLESS, end_sensorless_homing_per_axis(Y_AXIS, stealth_states_y)); - TERN_(Z_SENSORLESS, end_sensorless_homing_per_axis(Z_AXIS, stealth_states_z)); + TERN_(X_SENSORLESS, end_sensorless_homing_per_axis(X_AXIS, stealth_states_x)); + TERN_(Y_SENSORLESS, end_sensorless_homing_per_axis(Y_AXIS, stealth_states_y)); + TERN_(Z_SENSORLESS, end_sensorless_homing_per_axis(Z_AXIS, stealth_states_z)); #endif endstops.validate_homing_move(); diff --git a/Marlin/src/module/motion.cpp b/Marlin/src/module/motion.cpp index 6fff80ff7..7b4d1e36b 100644 --- a/Marlin/src/module/motion.cpp +++ b/Marlin/src/module/motion.cpp @@ -267,11 +267,15 @@ void get_cartesian_from_steppers() { #else #if IS_SCARA forward_kinematics_SCARA( - planner.get_axis_position_degrees(A_AXIS), - planner.get_axis_position_degrees(B_AXIS) + planner.get_axis_position_degrees(A_AXIS) + , planner.get_axis_position_degrees(B_AXIS) + #if ENABLED(AXEL_TPARA) + , planner.get_axis_position_degrees(C_AXIS) + #endif ); #else - cartes.set(planner.get_axis_position_mm(X_AXIS), planner.get_axis_position_mm(Y_AXIS)); + cartes.x = planner.get_axis_position_mm(X_AXIS); + cartes.y = planner.get_axis_position_mm(Y_AXIS); #endif cartes.z = planner.get_axis_position_mm(Z_AXIS); #endif @@ -1340,7 +1344,7 @@ void prepare_line_to_destination() { TERN_(SENSORLESS_HOMING, stealth_states = start_sensorless_homing_per_axis(axis)); } - #if IS_SCARA + #if EITHER(MORGAN_SCARA, MP_SCARA) // Tell the planner the axis is at 0 current_position[axis] = 0; sync_plan_position(); @@ -1490,7 +1494,7 @@ void prepare_line_to_destination() { void homeaxis(const AxisEnum axis) { - #if IS_SCARA + #if EITHER(MORGAN_SCARA, MP_SCARA) // Only Z homing (with probe) is permitted if (axis != Z_AXIS) { BUZZ(100, 880); return; } #else @@ -1732,7 +1736,8 @@ void prepare_line_to_destination() { TERN_(Z_MULTI_ENDSTOPS, case Z_AXIS:) stepper.set_separate_multi_axis(false); } - #endif + + #endif // HAS_EXTRA_ENDSTOPS #ifdef TMC_HOME_PHASE // move back to homing phase if configured and capable @@ -1839,7 +1844,7 @@ void set_axis_is_at_home(const AxisEnum axis) { } #endif - #if ENABLED(MORGAN_SCARA) + #if EITHER(MORGAN_SCARA, AXEL_TPARA) scara_set_axis_is_at_home(axis); #elif ENABLED(DELTA) current_position[axis] = (axis == Z_AXIS) ? delta_height - TERN0(HAS_BED_PROBE, probe.offset.z) : base_home_pos(axis); diff --git a/Marlin/src/module/motion.h b/Marlin/src/module/motion.h index f784c7bf8..0328d015a 100644 --- a/Marlin/src/module/motion.h +++ b/Marlin/src/module/motion.h @@ -395,8 +395,21 @@ FORCE_INLINE bool all_axes_trusted() { return xyz_bits == // Return true if the given point is within the printable area inline bool position_is_reachable(const float &rx, const float &ry, const float inset=0) { #if ENABLED(DELTA) + return HYPOT2(rx, ry) <= sq(DELTA_PRINTABLE_RADIUS - inset + fslop); + + #elif ENABLED(AXEL_TPARA) + + const float R2 = HYPOT2(rx - TPARA_OFFSET_X, ry - TPARA_OFFSET_Y); + return ( + R2 <= sq(L1 + L2) - inset + #if MIDDLE_DEAD_ZONE_R > 0 + && R2 >= sq(float(MIDDLE_DEAD_ZONE_R)) + #endif + ); + #elif IS_SCARA + const float R2 = HYPOT2(rx - SCARA_OFFSET_X, ry - SCARA_OFFSET_Y); return ( R2 <= sq(L1 + L2) - inset @@ -404,6 +417,7 @@ FORCE_INLINE bool all_axes_trusted() { return xyz_bits == && R2 >= sq(float(MIDDLE_DEAD_ZONE_R)) #endif ); + #endif } diff --git a/Marlin/src/module/scara.cpp b/Marlin/src/module/scara.cpp index 565a502d5..a8a3acf0f 100644 --- a/Marlin/src/module/scara.cpp +++ b/Marlin/src/module/scara.cpp @@ -32,26 +32,29 @@ #include "motion.h" #include "planner.h" -float delta_segments_per_second = SCARA_SEGMENTS_PER_SECOND; +#if ENABLED(AXEL_TPARA) + // For homing, as in delta + #include "planner.h" + #include "endstops.h" + #include "../lcd/marlinui.h" + #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 { - - /** - * SCARA homes XY at the same time - */ xyz_pos_t homeposition; LOOP_XYZ(i) homeposition[i] = base_home_pos((AxisEnum)i); - #if ENABLED(MORGAN_SCARA) // MORGAN_SCARA uses arm angles for AB home position //DEBUG_ECHOLNPAIR("homeposition A:", homeposition.a, " B:", homeposition.b); inverse_kinematics(homeposition); forward_kinematics_SCARA(delta.a, delta.b); current_position[axis] = cartes[axis]; - #else + #elif ENABLED(MP_SCARA) // MP_SCARA uses a Cartesian XY home position //DEBUG_ECHOPGM("homeposition"); //DEBUG_ECHOLNPAIR_P(SP_X_LBL, homeposition.x, SP_Y_LBL, homeposition.y); @@ -59,6 +62,12 @@ void scara_set_axis_is_at_home(const AxisEnum axis) { delta.b = SCARA_OFFSET_THETA2; forward_kinematics_SCARA(delta.a, delta.b); current_position[axis] = cartes[axis]; + #elif ENABLED(AXEL_TPARA) + //DEBUG_ECHOPGM("homeposition"); + //DEBUG_ECHOLNPAIR_P(SP_X_LBL, homeposition.x, SP_Y_LBL, homeposition.y, SP_Z_LBL, homeposition.z); + inverse_kinematics(homeposition); + forward_kinematics_TPARA(delta.a, delta.b, delta.c); + current_position[axis] = cartes[axis]; #endif //DEBUG_ECHOPGM("Cartesian"); @@ -67,85 +76,210 @@ void scara_set_axis_is_at_home(const AxisEnum axis) { } } -static constexpr xy_pos_t scara_offset = { SCARA_OFFSET_X, SCARA_OFFSET_Y }; +#if EITHER(MORGAN_SCARA, MP_SCARA) -/** - * 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) { + static constexpr xy_pos_t scara_offset = { SCARA_OFFSET_X, SCARA_OFFSET_Y }; - const float a_sin = sin(RADIANS(a)) * L1, - a_cos = cos(RADIANS(a)) * L1, - b_sin = sin(RADIANS(b + TERN0(MP_SCARA, a))) * L2, - b_cos = cos(RADIANS(b + TERN0(MP_SCARA, a))) * L2; + /** + * 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 + TERN0(MP_SCARA, a))) * L2, + b_cos = cos(RADIANS(b + TERN0(MP_SCARA, a))) * L2; - cartes.set(a_cos + b_cos + scara_offset.x, // theta - a_sin + b_sin + scara_offset.y); // phi + cartes.x = a_cos + b_cos + scara_offset.x; // theta + cartes.y = a_sin + b_sin + scara_offset.y; // phi - /* - DEBUG_ECHOLNPAIR( - "SCARA FK Angle a=", a, - " b=", b, - " a_sin=", a_sin, - " a_cos=", a_cos, - " b_sin=", b_sin, - " b_cos=", b_cos - ); - DEBUG_ECHOLNPAIR(" cartes (X,Y) = "(cartes.x, ", ", cartes.y, ")"); - //*/ -} + /* + DEBUG_ECHOLNPAIR( + "SCARA FK Angle a=", a, + " b=", b, + " a_sin=", a_sin, + " a_cos=", a_cos, + " b_sin=", b_sin, + " b_cos=", b_cos + ); + DEBUG_ECHOLNPAIR(" cartes (X,Y) = "(cartes.x, ", ", cartes.y, ")"); + //*/ + } -/** - * SCARA Inverse Kinematics. Results in 'delta'. - * - * See https://reprap.org/forum/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; + /** + * Morgan SCARA Inverse Kinematics. Results are stored in 'delta'. + * + * See https://reprap.org/forum/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; - // Translate SCARA to standard XY with scaling factor - const xy_pos_t spos = raw - scara_offset; + // Translate SCARA to standard XY with scaling factor + const xy_pos_t spos = raw - scara_offset; - const float H2 = HYPOT2(spos.x, spos.y); - if (L1 == L2) - C2 = H2 / L1_2_2 - 1; - else - C2 = (H2 - (L1_2 + L2_2)) / (2.0f * L1 * L2); + const float H2 = HYPOT2(spos.x, spos.y); + if (L1 == L2) + C2 = H2 / L1_2_2 - 1; + else + C2 = (H2 - (L1_2 + L2_2)) / (2.0f * L1 * L2); - LIMIT(C2, -1, 1); + LIMIT(C2, -1, 1); - S2 = SQRT(1.0f - sq(C2)); + S2 = SQRT(1.0f - sq(C2)); - // Unrotated Arm1 plus rotated Arm2 gives the distance from Center to End - SK1 = L1 + L2 * 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; + // 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(spos.x, spos.y); + // 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); - // Angle of Arm2 - PSI = ATAN2(S2, C2); + // Angle of Arm2 + PSI = ATAN2(S2, C2); - delta.set(DEGREES(THETA), DEGREES(PSI + TERN0(MORGAN_SCARA, THETA)), raw.z); + delta.set(DEGREES(THETA), DEGREES(PSI + TERN0(MORGAN_SCARA, THETA)), raw.z); - /* - DEBUG_POS("SCARA IK", raw); - DEBUG_POS("SCARA IK", delta); - DEBUG_ECHOLNPAIR(" SCARA (x,y) ", sx, ",", sy, " C2=", C2, " S2=", S2, " Theta=", THETA, " Psi=", PSI); - //*/ -} + /* + DEBUG_POS("SCARA IK", raw); + DEBUG_POS("SCARA IK", delta); + DEBUG_ECHOLNPAIR(" SCARA (x,y) ", sx, ",", sy, " C2=", C2, " S2=", S2, " Theta=", THETA, " Psi=", PSI); + //*/ + } + +#elif ENABLED(MP_SCARA) + + void inverse_kinematics(const xyz_pos_t &raw) { + 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); + //*/ + } + +#elif ENABLED(AXEL_TPARA) + + static constexpr xyz_pos_t robot_offset = { TPARA_OFFSET_X, TPARA_OFFSET_Y, TPARA_OFFSET_Z }; + + // Convert ABC inputs in degrees to XYZ outputs in mm + void forward_kinematics_TPARA(const float &a, const float &b, const float &c) { + const float w = c - b, + r = L1 * cos(RADIANS(b)) + L2 * sin(RADIANS(w - (90 - b))), + x = r * cos(RADIANS(a)), + y = r * sin(RADIANS(a)), + rho2 = L1_2 + L2_2 - 2.0f * L1 * L2 * cos(RADIANS(w)); + + cartes = robot_offset + xyz_pos_t({ x, y, SQRT(rho2 - x * x - y * y) }); + } + + // Home YZ together, then X (or all at once). Based on quick_home_xy & home_delta + void home_TPARA() { + // Init the current position of all carriages to 0,0,0 + current_position.reset(); + destination.reset(); + sync_plan_position(); + + // Disable stealthChop if used. Enable diag1 pin on driver. + #if ENABLED(SENSORLESS_HOMING) + TERN_(X_SENSORLESS, sensorless_t stealth_states_x = start_sensorless_homing_per_axis(X_AXIS)); + TERN_(Y_SENSORLESS, sensorless_t stealth_states_y = start_sensorless_homing_per_axis(Y_AXIS)); + TERN_(Z_SENSORLESS, sensorless_t stealth_states_z = start_sensorless_homing_per_axis(Z_AXIS)); + #endif + + // const int x_axis_home_dir = x_home_dir(active_extruder); + + // const xy_pos_t pos { max_length(X_AXIS) , max_length(Y_AXIS) }; + // const float mlz = max_length(X_AXIS), + + // Move all carriages together linearly until an endstop is hit. + //do_blocking_move_to_xy_z(pos, mlz, homing_feedrate(Z_AXIS)); + + current_position.x = 0 ; + current_position.y = 0 ; + current_position.z = max_length(Z_AXIS) ; + line_to_current_position(homing_feedrate(Z_AXIS)); + planner.synchronize(); + + // Re-enable stealthChop if used. Disable diag1 pin on driver. + #if ENABLED(SENSORLESS_HOMING) + TERN_(X_SENSORLESS, end_sensorless_homing_per_axis(X_AXIS, stealth_states_x)); + TERN_(Y_SENSORLESS, end_sensorless_homing_per_axis(Y_AXIS, stealth_states_y)); + TERN_(Z_SENSORLESS, end_sensorless_homing_per_axis(Z_AXIS, stealth_states_z)); + #endif + + endstops.validate_homing_move(); + + // At least one motor has reached its endstop. + // Now re-home each motor separately. + homeaxis(A_AXIS); + homeaxis(C_AXIS); + homeaxis(B_AXIS); + + + // Set all carriages to their home positions + // Do this here all at once for Delta, because + // XYZ isn't ABC. Applying this per-tower would + // give the impression that they are the same. + LOOP_XYZ(i) set_axis_is_at_home((AxisEnum)i); + + sync_plan_position(); + } + + void inverse_kinematics(const xyz_pos_t &raw) { + const xyz_pos_t spos = raw - robot_offset; + + const float RXY = SQRT(HYPOT2(spos.x, spos.y)), + RHO2 = NORMSQ(spos.x, spos.y, spos.z), + //RHO = SQRT(RHO2), + LSS = L1_2 + L2_2, + LM = 2.0f * L1 * L2, + + CG = (LSS - RHO2) / LM, + SG = SQRT(1 - POW(CG, 2)), // Method 2 + K1 = L1 - L2 * CG, + K2 = L2 * SG, + + // Angle of Body Joint + THETA = ATAN2(spos.y, spos.x), + + // Angle of Elbow Joint + //GAMMA = ACOS(CG), + GAMMA = ATAN2(SG, CG), // Method 2 + + // Angle of Shoulder Joint, elevation angle measured from horizontal (r+) + //PHI = asin(spos.z/RHO) + asin(L2 * sin(GAMMA) / RHO), + PHI = ATAN2(spos.z, RXY) + ATAN2(K2, K1), // Method 2 + + // Elbow motor angle measured from horizontal, same frame as shoulder (r+) + PSI = PHI + GAMMA; + + delta.set(DEGREES(THETA), DEGREES(PHI), DEGREES(PSI)); + + //SERIAL_ECHOLNPAIR(" SCARA (x,y,z) ", spos.x , ",", spos.y, ",", spos.z, " Rho=", RHO, " Rho2=", RHO2, " Theta=", THETA, " Phi=", PHI, " Psi=", PSI, " Gamma=", GAMMA); + } + +#endif void scara_report_positions() { - SERIAL_ECHOLNPAIR( - "SCARA Theta:", planner.get_axis_position_degrees(A_AXIS), - " Psi" TERN_(MORGAN_SCARA, "+Theta") ":", planner.get_axis_position_degrees(B_AXIS) + SERIAL_ECHOLNPAIR("SCARA Theta:", planner.get_axis_position_degrees(A_AXIS) + #if ENABLED(AXEL_TPARA) + , " Phi:", planner.get_axis_position_degrees(B_AXIS) + , " Psi:", planner.get_axis_position_degrees(C_AXIS) + #else + , " Psi" TERN_(MORGAN_SCARA, "+Theta") ":", planner.get_axis_position_degrees(B_AXIS) + #endif ); SERIAL_EOL(); } diff --git a/Marlin/src/module/scara.h b/Marlin/src/module/scara.h index e2acaf308..d462842b5 100644 --- a/Marlin/src/module/scara.h +++ b/Marlin/src/module/scara.h @@ -29,14 +29,25 @@ extern float delta_segments_per_second; -// Float constants for SCARA calculations -float constexpr L1 = SCARA_LINKAGE_1, L2 = SCARA_LINKAGE_2, - L1_2 = sq(float(L1)), L1_2_2 = 2.0 * L1_2, - L2_2 = sq(float(L2)); +#if ENABLED(AXEL_TPARA) -void scara_set_axis_is_at_home(const AxisEnum axis); + float constexpr L1 = TPARA_LINKAGE_1, L2 = TPARA_LINKAGE_2, // Float constants for Robot arm calculations + L1_2 = sq(float(L1)), L1_2_2 = 2.0 * L1_2, + L2_2 = sq(float(L2)); + + void forward_kinematics_TPARA(const float &a, const float &b, const float &c); + void home_TPARA(); + +#else + + float constexpr L1 = SCARA_LINKAGE_1, L2 = SCARA_LINKAGE_2, // Float constants for SCARA calculations + L1_2 = sq(float(L1)), L1_2_2 = 2.0 * L1_2, + L2_2 = sq(float(L2)); + + void forward_kinematics_SCARA(const float &a, const float &b); + +#endif void inverse_kinematics(const xyz_pos_t &raw); -void forward_kinematics_SCARA(const float &a, const float &b); - +void scara_set_axis_is_at_home(const AxisEnum axis); void scara_report_positions();