andrey
/
Marlin_FB4S
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Fixes for MP_SCARA (#21113) 

Co-Authored-By: svsergo <52426708+svsergo@users.noreply.github.com>
vanilla_fb_2.0.x
Scott Lahteine 4 years ago
parent
7e8b02145c
commit
c4fcf8bfd7
1 changed files with 50 additions and 61 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 111
      Marlin/src/module/scara.cpp

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)
// 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);
forward_kinematics_SCARA(delta.a, delta.b);
current_position[axis] = cartes[axis];
#else
// MP_SCARA uses a Cartesian XY home position
// SERIAL_ECHOPGM("homeposition");
// SERIAL_ECHOLNPAIR_P(SP_X_LBL, homeposition.x, SP_Y_LBL, homeposition.y);
current_position[axis] = homeposition[axis];
//DEBUG_ECHOPGM("homeposition");
//DEBUG_ECHOLNPAIR_P(SP_X_LBL, homeposition.x, SP_Y_LBL, homeposition.y);
delta.a = SCARA_OFFSET_THETA1;
delta.b = SCARA_OFFSET_THETA2;
forward_kinematics_SCARA(delta.a, delta.b);
current_position[axis] = cartes[axis];
#endif
// SERIAL_ECHOPGM("Cartesian");
// SERIAL_ECHOLNPAIR_P(SP_X_LBL, current_position.x, SP_Y_LBL, current_position.y);
//DEBUG_ECHOPGM("Cartesian");
//DEBUG_ECHOLNPAIR_P(SP_X_LBL, current_position.x, SP_Y_LBL, current_position.y);
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,
a_cos = cos(RADIANS(a)) * L1,
b_sin = sin(RADIANS(b)) * L2,
b_cos = cos(RADIANS(b)) * L2;
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); // theta+phi
a_sin + b_sin + scara_offset.y); // phi
/*
SERIAL_ECHOLNPAIR(
DEBUG_ECHOLNPAIR(
"SCARA FK Angle a=", a,
" b=", b,
" a_sin=", a_sin,
@ -90,74 +93,60 @@ void forward_kinematics_SCARA(const float &a, const float &b) {
" b_sin=", b_sin,
" 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) {
float C2, S2, SK1, SK2, THETA, PSI;
#if ENABLED(MORGAN_SCARA)
/**
* 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 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);
S2 = SQRT(1.0f - sq(C2));
// Translate SCARA to standard XY with scaling factor
const xy_pos_t spos = raw - scara_offset;
// Unrotated Arm1 plus rotated Arm2 gives the distance from Center to End
SK1 = L1 + L2 * C2;
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);
// Rotated Arm2 gives the distance from Arm1 to Arm2
SK2 = L2 * S2;
LIMIT(C2, -1, 1);
// 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);
S2 = SQRT(1.0f - sq(C2));
// Angle of Arm2
PSI = ATAN2(S2, C2);
// Unrotated Arm1 plus rotated Arm2 gives the distance from Center to End
SK1 = L1 + L2 * C2;
delta.set(DEGREES(THETA), DEGREES(THETA + PSI), raw.z);
// Rotated Arm2 gives the distance from Arm1 to Arm2
SK2 = L2 * S2;
/*
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);
//*/
// 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);
#else // MP_SCARA
// Angle of Arm2
PSI = ATAN2(S2, C2);
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(THETA), DEGREES(PSI + TERN0(MORGAN_SCARA, THETA)), raw.z);
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
/*
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);
//*/
}
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();
}

Write Preview
Loading…
Cancel
Save