Firmware2/Marlin/src/module/scara.cpp

165 lines
4.9 KiB
C++
Raw Normal View History

2017-09-08 22:35:25 +02:00
/**
* Marlin 3D Printer Firmware
2020-02-03 15:00:57 +01:00
* Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
2017-09-08 22:35:25 +02:00
*
* Based on Sprinter and grbl.
2019-06-28 06:57:50 +02:00
* Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
2017-09-08 22:35:25 +02:00
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
2020-07-23 05:20:14 +02:00
* along with this program. If not, see <https://www.gnu.org/licenses/>.
2017-09-08 22:35:25 +02:00
*
*/
/**
* scara.cpp
*/
#include "../inc/MarlinConfig.h"
#if IS_SCARA
#include "scara.h"
#include "motion.h"
#include "planner.h"
2017-09-08 22:35:25 +02:00
float delta_segments_per_second = SCARA_SEGMENTS_PER_SECOND;
void scara_set_axis_is_at_home(const AxisEnum axis) {
if (axis == Z_AXIS)
2019-09-29 11:25:39 +02:00
current_position.z = Z_HOME_POS;
2017-09-08 22:35:25 +02:00
else {
/**
* SCARA homes XY at the same time
*/
2019-09-29 11:25:39 +02:00
xyz_pos_t homeposition;
2017-11-03 05:59:42 +01:00
LOOP_XYZ(i) homeposition[i] = base_home_pos((AxisEnum)i);
2017-09-08 22:35:25 +02:00
#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
2020-03-01 14:36:25 +01:00
// SERIAL_ECHOPGM("homeposition");
// SERIAL_ECHOLNPAIR_P(SP_X_LBL, homeposition.x, SP_Y_LBL, homeposition.y);
current_position[axis] = homeposition[axis];
#endif
2020-03-01 14:36:25 +01:00
// SERIAL_ECHOPGM("Cartesian");
// SERIAL_ECHOLNPAIR_P(SP_X_LBL, current_position.x, SP_Y_LBL, current_position.y);
update_software_endstops(axis);
2017-09-08 22:35:25 +02:00
}
}
2019-09-29 11:25:39 +02:00
static constexpr xy_pos_t scara_offset = { SCARA_OFFSET_X, SCARA_OFFSET_Y };
2017-09-08 22:35:25 +02:00
/**
2019-09-29 11:25:39 +02:00
* Morgan SCARA Forward Kinematics. Results in 'cartes'.
2017-09-08 22:35:25 +02:00
* 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)) * L2,
b_cos = cos(RADIANS(b)) * L2;
2019-09-29 11:25:39 +02:00
cartes.set(a_cos + b_cos + scara_offset.x, // theta
a_sin + b_sin + scara_offset.y); // theta+phi
2017-09-08 22:35:25 +02:00
/*
SERIAL_ECHOLNPAIR(
"SCARA FK Angle a=", a,
" b=", b,
" a_sin=", a_sin,
" a_cos=", a_cos,
" b_sin=", b_sin,
" b_cos=", b_cos
);
2019-09-29 11:25:39 +02:00
SERIAL_ECHOLNPAIR(" cartes (X,Y) = "(cartes.x, ", ", cartes.y, ")");
2017-09-08 22:35:25 +02:00
//*/
}
2019-09-29 11:25:39 +02:00
void inverse_kinematics(const xyz_pos_t &raw) {
2017-09-08 22:35:25 +02:00
#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;
2017-09-08 22:35:25 +02:00
// Translate SCARA to standard XY with scaling factor
const xy_pos_t spos = raw - scara_offset;
2017-09-08 22:35:25 +02:00
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);
2017-09-08 22:35:25 +02:00
S2 = SQRT(1.0f - sq(C2));
2017-09-08 22:35:25 +02:00
// Unrotated Arm1 plus rotated Arm2 gives the distance from Center to End
SK1 = L1 + L2 * C2;
2017-09-08 22:35:25 +02:00
// Rotated Arm2 gives the distance from Arm1 to Arm2
SK2 = L2 * S2;
2017-09-08 22:35:25 +02:00
// 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);
2017-09-08 22:35:25 +02:00
// Angle of Arm2
PSI = ATAN2(S2, C2);
2017-09-08 22:35:25 +02:00
delta.set(DEGREES(THETA), DEGREES(THETA + PSI), raw.z);
2017-09-08 22:35:25 +02:00
/*
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
2017-09-08 22:35:25 +02:00
}
void scara_report_positions() {
SERIAL_ECHOLNPAIR("SCARA Theta:", planner.get_axis_position_degrees(A_AXIS), " Psi+Theta:", planner.get_axis_position_degrees(B_AXIS));
2017-09-08 22:35:25 +02:00
SERIAL_EOL();
}
#endif // IS_SCARA