Mostly Printed SCARA (MPSCARA) support (#15573)
This commit is contained in:
parent
19f840a9b5
commit
b258cc85bf
@ -251,6 +251,7 @@
|
|||||||
//
|
//
|
||||||
// Maths macros that can be overridden by HAL
|
// Maths macros that can be overridden by HAL
|
||||||
//
|
//
|
||||||
|
#define ACOS(x) acosf(x)
|
||||||
#define ATAN2(y, x) atan2f(y, x)
|
#define ATAN2(y, x) atan2f(y, x)
|
||||||
#define POW(x, y) powf(x, y)
|
#define POW(x, y) powf(x, y)
|
||||||
#define SQRT(x) sqrtf(x)
|
#define SQRT(x) sqrtf(x)
|
||||||
|
@ -45,19 +45,19 @@ void scara_set_axis_is_at_home(const AxisEnum axis) {
|
|||||||
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);
|
||||||
|
|
||||||
// SERIAL_ECHOLNPAIR("homeposition X:", homeposition.x, " Y:", homeposition.y);
|
#if ENABLED(MORGAN_SCARA)
|
||||||
|
// MORGAN_SCARA uses arm angles for AB home position
|
||||||
/**
|
// SERIAL_ECHOLNPAIR("homeposition A:", homeposition.a, " B:", homeposition.b);
|
||||||
* Get Home position SCARA arm angles using inverse kinematics,
|
|
||||||
* and calculate homing offset using forward kinematics
|
|
||||||
*/
|
|
||||||
inverse_kinematics(homeposition);
|
inverse_kinematics(homeposition);
|
||||||
forward_kinematics_SCARA(delta.a, delta.b);
|
forward_kinematics_SCARA(delta.a, delta.b);
|
||||||
|
|
||||||
// SERIAL_ECHOLNPAIR("Cartesian X:", cartes.x, " Y:", cartes.y);
|
|
||||||
|
|
||||||
current_position[axis] = cartes[axis];
|
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);
|
update_software_endstops(axis);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -92,7 +92,11 @@ void forward_kinematics_SCARA(const float &a, const float &b) {
|
|||||||
//*/
|
//*/
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
void inverse_kinematics(const xyz_pos_t &raw) {
|
||||||
|
|
||||||
|
#if ENABLED(MORGAN_SCARA)
|
||||||
|
|
||||||
|
/**
|
||||||
* Morgan SCARA Inverse Kinematics. Results in 'delta'.
|
* Morgan SCARA Inverse Kinematics. Results in 'delta'.
|
||||||
*
|
*
|
||||||
* See http://forums.reprap.org/read.php?185,283327
|
* See http://forums.reprap.org/read.php?185,283327
|
||||||
@ -100,8 +104,6 @@ void forward_kinematics_SCARA(const float &a, const float &b) {
|
|||||||
* 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) {
|
|
||||||
|
|
||||||
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
|
||||||
@ -111,7 +113,7 @@ void inverse_kinematics(const xyz_pos_t &raw) {
|
|||||||
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.0 * L1 * L2);
|
C2 = (H2 - (L1_2 + L2_2)) / (2.0f * L1 * L2);
|
||||||
|
|
||||||
S2 = SQRT(1.0f - sq(C2));
|
S2 = SQRT(1.0f - sq(C2));
|
||||||
|
|
||||||
@ -134,6 +136,23 @@ void inverse_kinematics(const xyz_pos_t &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);
|
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() {
|
void scara_report_positions() {
|
||||||
|
2245
config/examples/SCARA/MP_SCARA/Configuration.h
Normal file
2245
config/examples/SCARA/MP_SCARA/Configuration.h
Normal file
File diff suppressed because it is too large
Load Diff
2756
config/examples/SCARA/MP_SCARA/Configuration_adv.h
Normal file
2756
config/examples/SCARA/MP_SCARA/Configuration_adv.h
Normal file
File diff suppressed because it is too large
Load Diff
@ -62,32 +62,45 @@
|
|||||||
* MORGAN_SCARA was developed by QHARLEY in South Africa in 2012-2013.
|
* MORGAN_SCARA was developed by QHARLEY in South Africa in 2012-2013.
|
||||||
* Implemented and slightly reworked by JCERNY in June, 2014.
|
* Implemented and slightly reworked by JCERNY in June, 2014.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
// Specify the specific SCARA model
|
|
||||||
#define MORGAN_SCARA
|
#define MORGAN_SCARA
|
||||||
|
|
||||||
#if ENABLED(MORGAN_SCARA)
|
/**
|
||||||
|
* Mostly Printed SCARA is an open source design by Tyler Williams. See:
|
||||||
//#define DEBUG_SCARA_KINEMATICS
|
* https://www.thingiverse.com/thing:2487048
|
||||||
#define SCARA_FEEDRATE_SCALING // Convert XY feedrate from mm/s to degrees/s on the fly
|
* https://www.thingiverse.com/thing:1241491
|
||||||
|
*/
|
||||||
|
//#define MP_SCARA
|
||||||
|
|
||||||
|
#if EITHER(MORGAN_SCARA, MP_SCARA)
|
||||||
// If movement is choppy try lowering this value
|
// If movement is choppy try lowering this value
|
||||||
#define SCARA_SEGMENTS_PER_SECOND 200
|
#define SCARA_SEGMENTS_PER_SECOND 200
|
||||||
|
|
||||||
// Length of inner and outer support arms. Measure arm lengths precisely.
|
// Length of inner and outer support arms. Measure arm lengths precisely.
|
||||||
#define SCARA_LINKAGE_1 150 //mm
|
#define SCARA_LINKAGE_1 150 // (mm)
|
||||||
#define SCARA_LINKAGE_2 150 //mm
|
#define SCARA_LINKAGE_2 150 // (mm)
|
||||||
|
|
||||||
// SCARA tower offset (position of Tower relative to bed zero position)
|
// SCARA tower offset (position of Tower relative to bed zero position)
|
||||||
// This needs to be reasonably accurate as it defines the printbed position in the SCARA space.
|
// This needs to be reasonably accurate as it defines the printbed position in the SCARA space.
|
||||||
#define SCARA_OFFSET_X 100 //mm
|
#define SCARA_OFFSET_X 100 // (mm)
|
||||||
#define SCARA_OFFSET_Y -56 //mm
|
#define SCARA_OFFSET_Y -56 // (mm)
|
||||||
|
|
||||||
|
#if ENABLED(MORGAN_SCARA)
|
||||||
|
|
||||||
|
//#define DEBUG_SCARA_KINEMATICS
|
||||||
|
#define SCARA_FEEDRATE_SCALING // Convert XY feedrate from mm/s to degrees/s on the fly
|
||||||
|
|
||||||
// Radius around the center where the arm cannot reach
|
// Radius around the center where the arm cannot reach
|
||||||
#define MIDDLE_DEAD_ZONE_R 0 //mm
|
#define MIDDLE_DEAD_ZONE_R 0 // (mm)
|
||||||
|
|
||||||
#define THETA_HOMING_OFFSET 0 //calculatated from Calibration Guide and command M360 / M114 see picture in http://reprap.harleystudio.co.za/?page_id=1073
|
#define THETA_HOMING_OFFSET 0 // Calculated from Calibration Guide and M360 / M114. See http://reprap.harleystudio.co.za/?page_id=1073
|
||||||
#define PSI_HOMING_OFFSET 0 //calculatated from Calibration Guide and command M364 / M114 see picture in http://reprap.harleystudio.co.za/?page_id=1073
|
#define PSI_HOMING_OFFSET 0 // Calculated from Calibration Guide and M364 / M114. See http://reprap.harleystudio.co.za/?page_id=1073
|
||||||
|
|
||||||
|
#elif ENABLED(MP_SCARA)
|
||||||
|
|
||||||
|
#define SCARA_OFFSET_THETA1 12 // degrees
|
||||||
|
#define SCARA_OFFSET_THETA2 131 // degrees
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
Loading…
Reference in New Issue
Block a user