MPCNC/Firmware2
1
0
Fork 0
Code Issues 1 Pull Requests Releases Wiki Activity

Fixes for MP_SCARA (#21113)

Browse Source 
Co-Authored-By: svsergo <52426708+svsergo@users.noreply.github.com>
This commit is contained in:
Scott Lahteine 2021-02-17 17:04:51 -06:00
parent 7e8b02145c
commit c4fcf8bfd7
1 changed files with 50 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -47,19 +47,22 @@ void scara_set_axis_is_at_home(const AxisEnum axis) {
#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
// SERIAL_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 #else
// MP_SCARA uses a Cartesian XY home position // MP_SCARA uses a Cartesian XY home position
// SERIAL_ECHOPGM("homeposition"); //DEBUG_ECHOPGM("homeposition");
// SERIAL_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);
current_position[axis] = homeposition[axis]; delta.a = SCARA_OFFSET_THETA1;
delta.b = SCARA_OFFSET_THETA2;
forward_kinematics_SCARA(delta.a, delta.b);
current_position[axis] = cartes[axis];
#endif #endif
// SERIAL_ECHOPGM("Cartesian"); //DEBUG_ECHOPGM("Cartesian");
// SERIAL_ECHOLNPAIR_P(SP_X_LBL, current_position.x, SP_Y_LBL, current_position.y); //DEBUG_ECHOLNPAIR_P(SP_X_LBL, current_position.x, SP_Y_LBL, current_position.y);
update_software_endstops(axis); update_software_endstops(axis);
} }
} }
@ -75,14 +78,14 @@ void forward_kinematics_SCARA(const float &a, const float &b) {
const float a_sin = sin(RADIANS(a)) * L1, const float a_sin = sin(RADIANS(a)) * L1,
a_cos = cos(RADIANS(a)) * L1, a_cos = cos(RADIANS(a)) * L1,
b_sin = sin(RADIANS(b)) * L2, b_sin = sin(RADIANS(b + TERN0(MP_SCARA, a))) * L2,
b_cos = cos(RADIANS(b)) * L2; b_cos = cos(RADIANS(b + TERN0(MP_SCARA, a))) * L2;
cartes.set(a_cos + b_cos + scara_offset.x, // theta cartes.set(a_cos + b_cos + scara_offset.x, // theta
a_sin + b_sin + scara_offset.y); // theta+phi a_sin + b_sin + scara_offset.y); // phi
/* /*
SERIAL_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,
@ -90,74 +93,60 @@ void forward_kinematics_SCARA(const float &a, const float &b) {
" b_sin=", b_sin, " b_sin=", b_sin,
" b_cos=", b_cos " b_cos=", b_cos
); );
SERIAL_ECHOLNPAIR(" cartes (X,Y) = "(cartes.x, ", ", cartes.y, ")"); 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) { void inverse_kinematics(const xyz_pos_t &raw) {
float C2, S2, SK1, SK2, THETA, PSI;
#if ENABLED(MORGAN_SCARA) // Translate SCARA to standard XY with scaling factor
/** const xy_pos_t spos = raw - scara_offset;
* Morgan 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.
*/
float C2, S2, SK1, SK2, THETA, PSI;
// Translate SCARA to standard XY with scaling factor const float H2 = HYPOT2(spos.x, spos.y);
const xy_pos_t spos = raw - scara_offset; 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); LIMIT(C2, -1, 1);
if (L1 == L2)
C2 = H2 / L1_2_2 - 1;
else
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(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);
SERIAL_ECHOLNPAIR(" SCARA (x,y) ", sx, ",", sy, " C2=", C2, " S2=", S2, " Theta=", THETA, " Phi=", PHI); DEBUG_ECHOLNPAIR(" SCARA (x,y) ", sx, ",", sy, " C2=", C2, " S2=", S2, " Theta=", THETA, " Psi=", PSI);
//*/ //*/
#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() { 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_ECHOLNPAIR(
"SCARA Theta:", planner.get_axis_position_degrees(A_AXIS),
" Psi" TERN_(MORGAN_SCARA, "+Theta") ":", planner.get_axis_position_degrees(B_AXIS)
);
SERIAL_EOL(); SERIAL_EOL();
} }