CMS/Marlin
3
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

TPARA - 3DOF robot arm IK (#21005)

Browse Source 
Co-authored-by: Scott Lahteine <github@thinkyhead.com>
This commit is contained in:
Axel 2021-03-03 20:46:32 -03:00 committed by Scott Lahteine
parent 3c86eb18a5
commit 864d27d460
12 changed files with 285 additions and 117 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
Marlin/Configuration.h
View File

@ -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.
// //
// Download configurations from the link above and customize for your machine.
//=========================================================================== // Examples are located in config/examples/delta, .../SCARA, and .../TPARA.
//============================= 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.
// //
//===========================================================================
// @section info // @section info

1
Marlin/src/core/macros.h
View File

@ -104,6 +104,7 @@
#define RADIANS(d) ((d)*float(M_PI)/180.0f) #define RADIANS(d) ((d)*float(M_PI)/180.0f)
#define DEGREES(r) ((r)*180.0f/float(M_PI)) #define DEGREES(r) ((r)*180.0f/float(M_PI))
#define HYPOT2(x,y) (sq(x)+sq(y)) #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_AREA(R) (float(M_PI) * sq(float(R)))
#define CIRCLE_CIRC(R) (2 * float(M_PI) * float(R)) #define CIRCLE_CIRC(R) (2 * float(M_PI) * float(R))

10
Marlin/src/gcode/calibrate/G28.cpp
View File

@ -311,7 +311,13 @@ void GcodeSuite::G28() {
TERN_(IMPROVE_HOMING_RELIABILITY, end_slow_homing(slow_homing)); 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'), const bool homeZ = parser.seen('Z'),
needX = homeZ && TERN0(Z_SAFE_HOMING, axes_should_home(_BV(X_AXIS))), needX = homeZ && TERN0(Z_SAFE_HOMING, axes_should_home(_BV(X_AXIS))),
@ -392,7 +398,7 @@ void GcodeSuite::G28() {
sync_plan_position(); sync_plan_position();
#endif // !DELTA (G28) #endif
/** /**
* Preserve DXC mode across a G28 for IDEX printers in DXC_DUPLICATION_MODE. * Preserve DXC mode across a G28 for IDEX printers in DXC_DUPLICATION_MODE.

26
Marlin/src/gcode/calibrate/M665.cpp
View File

@ -40,21 +40,21 @@
* X = Alpha (Tower 1) angle trim * X = Alpha (Tower 1) angle trim
* Y = Beta (Tower 2) angle trim * Y = Beta (Tower 2) angle trim
* Z = Gamma (Tower 3) angle trim * Z = Gamma (Tower 3) angle trim
* A = Alpha (Tower 1) digonal rod trim * A = Alpha (Tower 1) diagonal rod trim
* B = Beta (Tower 2) digonal rod trim * B = Beta (Tower 2) diagonal rod trim
* C = Gamma (Tower 3) digonal rod trim * C = Gamma (Tower 3) diagonal rod trim
*/ */
void GcodeSuite::M665() { void GcodeSuite::M665() {
if (parser.seen('H')) delta_height = parser.value_linear_units(); if (parser.seenval('H')) delta_height = parser.value_linear_units();
if (parser.seen('L')) delta_diagonal_rod = parser.value_linear_units(); if (parser.seenval('L')) delta_diagonal_rod = parser.value_linear_units();
if (parser.seen('R')) delta_radius = parser.value_linear_units(); if (parser.seenval('R')) delta_radius = parser.value_linear_units();
if (parser.seen('S')) delta_segments_per_second = parser.value_float(); if (parser.seenval('S')) delta_segments_per_second = parser.value_float();
if (parser.seen('X')) delta_tower_angle_trim.a = parser.value_float(); if (parser.seenval('X')) delta_tower_angle_trim.a = parser.value_float();
if (parser.seen('Y')) delta_tower_angle_trim.b = parser.value_float(); if (parser.seenval('Y')) delta_tower_angle_trim.b = parser.value_float();
if (parser.seen('Z')) delta_tower_angle_trim.c = parser.value_float(); if (parser.seenval('Z')) delta_tower_angle_trim.c = parser.value_float();
if (parser.seen('A')) delta_diagonal_rod_trim.a = parser.value_float(); if (parser.seenval('A')) delta_diagonal_rod_trim.a = parser.value_float();
if (parser.seen('B')) delta_diagonal_rod_trim.b = parser.value_float(); if (parser.seenval('B')) delta_diagonal_rod_trim.b = parser.value_float();
if (parser.seen('C')) delta_diagonal_rod_trim.c = parser.value_float(); if (parser.seenval('C')) delta_diagonal_rod_trim.c = parser.value_float();
recalc_delta_settings(); recalc_delta_settings();
} }

2
Marlin/src/inc/Conditionals_LCD.h
View File

@ -924,7 +924,7 @@
#define NORMAL_AXIS Z_AXIS #define NORMAL_AXIS Z_AXIS
#endif #endif
#if ENABLED(MORGAN_SCARA) #if EITHER(MORGAN_SCARA, AXEL_TPARA)
#define IS_SCARA 1 #define IS_SCARA 1
#define IS_KINEMATIC 1 #define IS_KINEMATIC 1
#elif ENABLED(DELTA) #elif ENABLED(DELTA)

4
Marlin/src/inc/Conditionals_post.h
View File

@ -143,7 +143,9 @@
*/ */
#if IS_SCARA #if IS_SCARA
#undef SLOWDOWN #undef SLOWDOWN
#define QUICK_HOME #if DISABLED(AXEL_TPARA)
#define QUICK_HOME
#endif
#define SCARA_PRINTABLE_RADIUS (SCARA_LINKAGE_1 + SCARA_LINKAGE_2) #define SCARA_PRINTABLE_RADIUS (SCARA_LINKAGE_1 + SCARA_LINKAGE_2)
#endif #endif

4
Marlin/src/inc/SanityCheck.h
View File

@ -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 * Allow only one kinematic type to be defined
*/ */
#if MANY(DELTA, MORGAN_SCARA, COREXY, COREXZ, COREYZ, COREYX, COREZX, COREZY, 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, COREXY, COREYX, COREXZ, COREZX, COREYZ, COREZY, or MARKFORGED_XY." #error "Please enable only one of DELTA, MORGAN_SCARA, AXEL_TPARA, COREXY, COREYX, COREXZ, COREZX, COREYZ, COREZY, or MARKFORGED_XY."
#endif #endif
/** /**

12
Marlin/src/module/delta.cpp
View File

@ -242,9 +242,9 @@ void home_delta() {
// Disable stealthChop if used. Enable diag1 pin on driver. // Disable stealthChop if used. Enable diag1 pin on driver.
#if ENABLED(SENSORLESS_HOMING) #if ENABLED(SENSORLESS_HOMING)
TERN_(X_SENSORLESS, sensorless_t stealth_states_x = start_sensorless_homing_per_axis(X_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_(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_(Z_SENSORLESS, sensorless_t stealth_states_z = start_sensorless_homing_per_axis(Z_AXIS));
#endif #endif
// Move all carriages together linearly until an endstop is hit. // 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. // Re-enable stealthChop if used. Disable diag1 pin on driver.
#if ENABLED(SENSORLESS_HOMING) #if ENABLED(SENSORLESS_HOMING)
TERN_(X_SENSORLESS, end_sensorless_homing_per_axis(X_AXIS, stealth_states_x)); 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_(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_(Z_SENSORLESS, end_sensorless_homing_per_axis(Z_AXIS, stealth_states_z));
#endif #endif
endstops.validate_homing_move(); endstops.validate_homing_move();

19
Marlin/src/module/motion.cpp
View File

@ -267,11 +267,15 @@ void get_cartesian_from_steppers() {
#else #else
#if IS_SCARA #if IS_SCARA
forward_kinematics_SCARA( forward_kinematics_SCARA(
planner.get_axis_position_degrees(A_AXIS), planner.get_axis_position_degrees(A_AXIS)
planner.get_axis_position_degrees(B_AXIS) , planner.get_axis_position_degrees(B_AXIS)
#if ENABLED(AXEL_TPARA)
, planner.get_axis_position_degrees(C_AXIS)
#endif
); );
#else #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 #endif
cartes.z = planner.get_axis_position_mm(Z_AXIS); cartes.z = planner.get_axis_position_mm(Z_AXIS);
#endif #endif
@ -1340,7 +1344,7 @@ void prepare_line_to_destination() {
TERN_(SENSORLESS_HOMING, stealth_states = start_sensorless_homing_per_axis(axis)); 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 // Tell the planner the axis is at 0
current_position[axis] = 0; current_position[axis] = 0;
sync_plan_position(); sync_plan_position();
@ -1490,7 +1494,7 @@ void prepare_line_to_destination() {
void homeaxis(const AxisEnum axis) { void homeaxis(const AxisEnum axis) {
#if IS_SCARA #if EITHER(MORGAN_SCARA, MP_SCARA)
// Only Z homing (with probe) is permitted // Only Z homing (with probe) is permitted
if (axis != Z_AXIS) { BUZZ(100, 880); return; } if (axis != Z_AXIS) { BUZZ(100, 880); return; }
#else #else
@ -1732,7 +1736,8 @@ void prepare_line_to_destination() {
TERN_(Z_MULTI_ENDSTOPS, case Z_AXIS:) TERN_(Z_MULTI_ENDSTOPS, case Z_AXIS:)
stepper.set_separate_multi_axis(false); stepper.set_separate_multi_axis(false);
} }
#endif
#endif // HAS_EXTRA_ENDSTOPS
#ifdef TMC_HOME_PHASE #ifdef TMC_HOME_PHASE
// move back to homing phase if configured and capable // move back to homing phase if configured and capable
@ -1839,7 +1844,7 @@ void set_axis_is_at_home(const AxisEnum axis) {
} }
#endif #endif
#if ENABLED(MORGAN_SCARA) #if EITHER(MORGAN_SCARA, AXEL_TPARA)
scara_set_axis_is_at_home(axis); scara_set_axis_is_at_home(axis);
#elif ENABLED(DELTA) #elif ENABLED(DELTA)
current_position[axis] = (axis == Z_AXIS) ? delta_height - TERN0(HAS_BED_PROBE, probe.offset.z) : base_home_pos(axis); current_position[axis] = (axis == Z_AXIS) ? delta_height - TERN0(HAS_BED_PROBE, probe.offset.z) : base_home_pos(axis);

14
Marlin/src/module/motion.h
View File

@ -395,8 +395,21 @@ FORCE_INLINE bool all_axes_trusted() { return xyz_bits ==
// Return true if the given point is within the printable area // 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) { inline bool position_is_reachable(const float &rx, const float &ry, const float inset=0) {
#if ENABLED(DELTA) #if ENABLED(DELTA)
return HYPOT2(rx, ry) <= sq(DELTA_PRINTABLE_RADIUS - inset + fslop); 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 #elif IS_SCARA
const float R2 = HYPOT2(rx - SCARA_OFFSET_X, ry - SCARA_OFFSET_Y); const float R2 = HYPOT2(rx - SCARA_OFFSET_X, ry - SCARA_OFFSET_Y);
return ( return (
R2 <= sq(L1 + L2) - inset 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)) && R2 >= sq(float(MIDDLE_DEAD_ZONE_R))
#endif #endif
); );
#endif #endif
} }

272
Marlin/src/module/scara.cpp
View File

@ -32,26 +32,29 @@
#include "motion.h" #include "motion.h"
#include "planner.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) { void scara_set_axis_is_at_home(const AxisEnum axis) {
if (axis == Z_AXIS) if (axis == Z_AXIS)
current_position.z = Z_HOME_POS; current_position.z = Z_HOME_POS;
else { else {
/**
* SCARA homes XY at the same time
*/
xyz_pos_t homeposition; xyz_pos_t homeposition;
LOOP_XYZ(i) homeposition[i] = base_home_pos((AxisEnum)i); LOOP_XYZ(i) homeposition[i] = base_home_pos((AxisEnum)i);
#if ENABLED(MORGAN_SCARA) #if ENABLED(MORGAN_SCARA)
// MORGAN_SCARA uses arm angles for AB home position // MORGAN_SCARA uses arm angles for AB home position
//DEBUG_ECHOLNPAIR("homeposition A:", homeposition.a, " B:", homeposition.b); //DEBUG_ECHOLNPAIR("homeposition A:", homeposition.a, " B:", homeposition.b);
inverse_kinematics(homeposition); inverse_kinematics(homeposition);
forward_kinematics_SCARA(delta.a, delta.b); forward_kinematics_SCARA(delta.a, delta.b);
current_position[axis] = cartes[axis]; current_position[axis] = cartes[axis];
#else #elif ENABLED(MP_SCARA)
// MP_SCARA uses a Cartesian XY home position // MP_SCARA uses a Cartesian XY home position
//DEBUG_ECHOPGM("homeposition"); //DEBUG_ECHOPGM("homeposition");
//DEBUG_ECHOLNPAIR_P(SP_X_LBL, homeposition.x, SP_Y_LBL, homeposition.y); //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; delta.b = SCARA_OFFSET_THETA2;
forward_kinematics_SCARA(delta.a, delta.b); forward_kinematics_SCARA(delta.a, delta.b);
current_position[axis] = cartes[axis]; 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 #endif
//DEBUG_ECHOPGM("Cartesian"); //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)
/** static constexpr xy_pos_t scara_offset = { SCARA_OFFSET_X, SCARA_OFFSET_Y };
* 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, * Morgan SCARA Forward Kinematics. Results in 'cartes'.
b_sin = sin(RADIANS(b + TERN0(MP_SCARA, a))) * L2, * Maths and first version by QHARLEY.
b_cos = cos(RADIANS(b + TERN0(MP_SCARA, a))) * L2; * 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 cartes.x = a_cos + b_cos + scara_offset.x; // theta
a_sin + b_sin + scara_offset.y); // phi cartes.y = a_sin + b_sin + scara_offset.y; // phi
/* /*
DEBUG_ECHOLNPAIR( DEBUG_ECHOLNPAIR(
"SCARA FK Angle a=", a, "SCARA FK Angle a=", a,
" b=", b, " b=", b,
" a_sin=", a_sin, " a_sin=", a_sin,
" a_cos=", a_cos, " a_cos=", a_cos,
" b_sin=", b_sin, " b_sin=", b_sin,
" b_cos=", b_cos " b_cos=", b_cos
); );
DEBUG_ECHOLNPAIR(" cartes (X,Y) = "(cartes.x, ", ", cartes.y, ")"); DEBUG_ECHOLNPAIR(" cartes (X,Y) = "(cartes.x, ", ", cartes.y, ")");
//*/ //*/
} }
/** /**
* SCARA Inverse Kinematics. Results in 'delta'. * Morgan SCARA Inverse Kinematics. Results are stored in 'delta'.
* *
* See https://reprap.org/forum/read.php?185,283327 * See https://reprap.org/forum/read.php?185,283327
* *
* Maths and first version by QHARLEY. * Maths and first version by QHARLEY.
* Integrated into Marlin and slightly restructured by Joachim Cerny. * Integrated into Marlin and slightly restructured by Joachim Cerny.
*/ */
void inverse_kinematics(const xyz_pos_t &raw) { void inverse_kinematics(const xyz_pos_t &raw) {
float C2, S2, SK1, SK2, THETA, PSI; 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.0f * L1 * L2); 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 // 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(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", raw);
DEBUG_POS("SCARA IK", delta); DEBUG_POS("SCARA IK", delta);
DEBUG_ECHOLNPAIR(" SCARA (x,y) ", sx, ",", sy, " C2=", C2, " S2=", S2, " Theta=", THETA, " Psi=", PSI); 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() { void scara_report_positions() {
SERIAL_ECHOLNPAIR( SERIAL_ECHOLNPAIR("SCARA Theta:", planner.get_axis_position_degrees(A_AXIS)
"SCARA Theta:", planner.get_axis_position_degrees(A_AXIS), #if ENABLED(AXEL_TPARA)
" Psi" TERN_(MORGAN_SCARA, "+Theta") ":", planner.get_axis_position_degrees(B_AXIS) , " 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(); SERIAL_EOL();
} }

25
Marlin/src/module/scara.h
View File

@ -29,14 +29,25 @@
extern float delta_segments_per_second; extern float delta_segments_per_second;
// Float constants for SCARA calculations #if ENABLED(AXEL_TPARA)
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));
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 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(); void scara_report_positions();