bissen21/Marlin_Firmware
1
0
Fork 0
Code Issues Pull Requests Projects Releases 4 Wiki Activity

Mostly Printed SCARA (MPSCARA) support (#15573)

Browse Source
This commit is contained in:
brian park
2019-11-30 23:57:34 +09:00
committed by Scott Lahteine
parent 19f840a9b5
commit b258cc85bf
6 changed files with 5091 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -251,6 +251,7 @@
//
// Maths macros that can be overridden by HAL
//
#define ACOS(x) acosf(x)
#define ATAN2(y, x) atan2f(y, x)
#define POW(x, y) powf(x, y)
#define SQRT(x) sqrtf(x)

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

@ -45,19 +45,19 @@ void scara_set_axis_is_at_home(const AxisEnum axis) {
xyz_pos_t homeposition;
LOOP_XYZ(i) homeposition[i] = base_home_pos((AxisEnum)i);
// SERIAL_ECHOLNPAIR("homeposition X:", homeposition.x, " Y:", homeposition.y);
/**
* Get Home position SCARA arm angles using inverse kinematics,
* and calculate homing offset using forward kinematics
*/
inverse_kinematics(homeposition);
forward_kinematics_SCARA(delta.a, delta.b);
// SERIAL_ECHOLNPAIR("Cartesian X:", cartes.x, " Y:", cartes.y);
current_position[axis] = cartes[axis];
#if ENABLED(MORGAN_SCARA)
// MORGAN_SCARA uses arm angles for AB home position
// SERIAL_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_ECHOLNPAIR("homeposition X:", homeposition.x, " Y:", homeposition.y);
current_position[axis] = homeposition[axis];
#endif
// SERIAL_ECHOLNPAIR("Cartesian X:", current_position.x, " Y:", current_position.y);
update_software_endstops(axis);
}
}
@ -92,48 +92,67 @@ void forward_kinematics_SCARA(const float &a, const float &b) {
//*/
}
/**
* 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 xyz_pos_t &raw) {
float C2, S2, SK1, SK2, THETA, PSI;
#if ENABLED(MORGAN_SCARA)
/**
* 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.
*/
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.0 * 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);
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(THETA + PSI), raw.z);
delta.set(DEGREES(THETA), DEGREES(THETA + PSI), raw.z);
/*
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);
//*/
/*
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);
//*/
#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() {