|
|
@ -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, |
|
|
|
|
|
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); // phi
|
|
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
|
|
DEBUG_ECHOLNPAIR( |
|
|
|
|
|
"SCARA FK Angle a=", a, |
|
|
|
|
|
" b=", b, |
|
|
|
|
|
" a_sin=", a_sin, |
|
|
|
|
|
" a_cos=", a_cos, |
|
|
|
|
|
" b_sin=", b_sin, |
|
|
|
|
|
" b_cos=", b_cos |
|
|
|
|
|
); |
|
|
|
|
|
DEBUG_ECHOLNPAIR(" cartes (X,Y) = "(cartes.x, ", ", cartes.y, ")"); |
|
|
|
|
|
//*/
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
/**
|
|
|
* SCARA Inverse Kinematics. Results in 'delta'. |
|
|
* Morgan SCARA Forward Kinematics. Results in 'cartes'. |
|
|
* |
|
|
* Maths and first version by QHARLEY. |
|
|
* See https://reprap.org/forum/read.php?185,283327
|
|
|
* Integrated into Marlin and slightly restructured by Joachim Cerny. |
|
|
* |
|
|
*/ |
|
|
* Maths and first version by QHARLEY. |
|
|
void forward_kinematics_SCARA(const float &a, const float &b) { |
|
|
* Integrated into Marlin and slightly restructured by Joachim Cerny. |
|
|
const float a_sin = sin(RADIANS(a)) * L1, |
|
|
*/ |
|
|
a_cos = cos(RADIANS(a)) * L1, |
|
|
void inverse_kinematics(const xyz_pos_t &raw) { |
|
|
b_sin = sin(RADIANS(b + TERN0(MP_SCARA, a))) * L2, |
|
|
float C2, S2, SK1, SK2, THETA, PSI; |
|
|
b_cos = cos(RADIANS(b + TERN0(MP_SCARA, a))) * L2; |
|
|
|
|
|
|
|
|
// Translate SCARA to standard XY with scaling factor
|
|
|
cartes.x = a_cos + b_cos + scara_offset.x; // theta
|
|
|
const xy_pos_t spos = raw - scara_offset; |
|
|
cartes.y = a_sin + b_sin + scara_offset.y; // phi
|
|
|
|
|
|
|
|
|
const float H2 = HYPOT2(spos.x, spos.y); |
|
|
/*
|
|
|
if (L1 == L2) |
|
|
DEBUG_ECHOLNPAIR( |
|
|
C2 = H2 / L1_2_2 - 1; |
|
|
"SCARA FK Angle a=", a, |
|
|
else |
|
|
" b=", b, |
|
|
C2 = (H2 - (L1_2 + L2_2)) / (2.0f * L1 * L2); |
|
|
" a_sin=", a_sin, |
|
|
|
|
|
" a_cos=", a_cos, |
|
|
|
|
|
" b_sin=", b_sin, |
|
|
|
|
|
" b_cos=", b_cos |
|
|
|
|
|
); |
|
|
|
|
|
DEBUG_ECHOLNPAIR(" cartes (X,Y) = "(cartes.x, ", ", cartes.y, ")"); |
|
|
|
|
|
//*/
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
LIMIT(C2, -1, 1); |
|
|
/**
|
|
|
|
|
|
* Morgan SCARA Inverse Kinematics. Results are stored 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; |
|
|
|
|
|
|
|
|
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
|
|
|
const float H2 = HYPOT2(spos.x, spos.y); |
|
|
SK1 = L1 + L2 * C2; |
|
|
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
|
|
|
LIMIT(C2, -1, 1); |
|
|
SK2 = L2 * S2; |
|
|
|
|
|
|
|
|
|
|
|
// Angle of Arm1 is the difference between Center-to-End angle and the Center-to-Elbow
|
|
|
S2 = SQRT(1.0f - sq(C2)); |
|
|
THETA = ATAN2(SK1, SK2) - ATAN2(spos.x, spos.y); |
|
|
|
|
|
|
|
|
|
|
|
// Angle of Arm2
|
|
|
// Unrotated Arm1 plus rotated Arm2 gives the distance from Center to End
|
|
|
PSI = ATAN2(S2, C2); |
|
|
SK1 = L1 + L2 * C2; |
|
|
|
|
|
|
|
|
delta.set(DEGREES(THETA), DEGREES(PSI + TERN0(MORGAN_SCARA, THETA)), raw.z); |
|
|
// 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
|
|
|
DEBUG_POS("SCARA IK", raw); |
|
|
THETA = ATAN2(SK1, SK2) - ATAN2(spos.x, spos.y); |
|
|
DEBUG_POS("SCARA IK", delta); |
|
|
|
|
|
DEBUG_ECHOLNPAIR(" SCARA (x,y) ", sx, ",", sy, " C2=", C2, " S2=", S2, " Theta=", THETA, " Psi=", PSI); |
|
|
// Angle of Arm2
|
|
|
//*/
|
|
|
PSI = ATAN2(S2, C2); |
|
|
} |
|
|
|
|
|
|
|
|
delta.set(DEGREES(THETA), DEGREES(PSI + TERN0(MORGAN_SCARA, THETA)), raw.z); |
|
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
|
|
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); |
|
|
|
|
|
//*/
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
#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(); |
|
|
} |
|
|
} |
|
|
|
Axel
4 years ago
Scott Lahteine