thebears/marlin_skr
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Followup to MP_SCARA/TPARA patches (#21248)

Browse Source
This commit is contained in:
Tanguy Pruvot
2021-03-05 00:34:38 +01:00
committed by GitHub
parent 6903a2ffc5
commit a0d312396a
8 changed files with 74 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -37,46 +37,7 @@
#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) {
if (axis == Z_AXIS)
current_position.z = Z_HOME_POS;
else {
#if ENABLED(MORGAN_SCARA)
// MORGAN_SCARA uses arm angles for AB home position
ab_float_t homeposition = { SCARA_OFFSET_THETA1, SCARA_OFFSET_THETA2 };
//DEBUG_ECHOLNPAIR("homeposition A:", homeposition.a, " B:", homeposition.b);
#elif ENABLED(MP_SCARA)
// MP_SCARA uses a Cartesian XY home position
xyz_pos_t homeposition = { X_HOME_POS, Y_HOME_POS, Z_HOME_POS };
//DEBUG_ECHOPGM("homeposition");
//DEBUG_ECHOLNPAIR_P(SP_X_LBL, homeposition.x, SP_Y_LBL, homeposition.y);
#elif ENABLED(AXEL_TPARA)
xyz_pos_t homeposition = { X_HOME_POS, Y_HOME_POS, Z_HOME_POS };
//DEBUG_ECHOPGM("homeposition");
//DEBUG_ECHOLNPAIR_P(SP_X_LBL, homeposition.x, SP_Y_LBL, homeposition.y, SP_Z_LBL, homeposition.z);
#endif
#if ENABLED(MORGAN_SCARA)
delta = homeposition;
#else
inverse_kinematics(homeposition);
#endif
#if EITHER(MORGAN_SCARA, MP_SCARA)
forward_kinematics(delta.a, delta.b);
#elif ENABLED(AXEL_TPARA)
forward_kinematics(delta.a, delta.b, delta.c);
#endif
current_position[axis] = cartes[axis];
//DEBUG_ECHOPGM("Cartesian");
//DEBUG_ECHOLNPAIR_P(SP_X_LBL, current_position.x, SP_Y_LBL, current_position.y);
update_software_endstops(axis);
}
}
float segments_per_second = TERN(AXEL_TPARA, TPARA_SEGMENTS_PER_SECOND, SCARA_SEGMENTS_PER_SECOND);
#if EITHER(MORGAN_SCARA, MP_SCARA)
@@ -109,6 +70,27 @@ void scara_set_axis_is_at_home(const AxisEnum axis) {
//*/
}
#endif
#if ENABLED(MORGAN_SCARA)
void scara_set_axis_is_at_home(const AxisEnum axis) {
if (axis == Z_AXIS)
current_position.z = Z_HOME_POS;
else {
// MORGAN_SCARA uses a Cartesian XY home position
xyz_pos_t homeposition = { X_HOME_POS, Y_HOME_POS, Z_HOME_POS };
//DEBUG_ECHOLNPAIR_P(PSTR("homeposition X"), homeposition.x, SP_Y_LBL, homeposition.y);
delta = homeposition;
forward_kinematics(delta.a, delta.b);
current_position[axis] = cartes[axis];
//DEBUG_ECHOLNPAIR_P(PSTR("Cartesian X"), current_position.x, SP_Y_LBL, current_position.y);
update_software_endstops(axis);
}
}
/**
* Morgan SCARA Inverse Kinematics. Results are stored in 'delta'.
*
@@ -156,6 +138,29 @@ void scara_set_axis_is_at_home(const AxisEnum axis) {
#elif ENABLED(MP_SCARA)
void scara_set_axis_is_at_home(const AxisEnum axis) {
if (axis == Z_AXIS)
current_position.z = Z_HOME_POS;
else {
// MP_SCARA uses arm angles for AB home position
#ifndef SCARA_OFFSET_THETA1
#define SCARA_OFFSET_THETA1 12 // degrees
#endif
#ifndef SCARA_OFFSET_THETA2
#define SCARA_OFFSET_THETA2 131 // degrees
#endif
ab_float_t homeposition = { SCARA_OFFSET_THETA1, SCARA_OFFSET_THETA2 };
//DEBUG_ECHOLNPAIR("homeposition A:", homeposition.a, " B:", homeposition.b);
inverse_kinematics(homeposition);
forward_kinematics(delta.a, delta.b);
current_position[axis] = cartes[axis];
//DEBUG_ECHOLNPAIR_P(PSTR("Cartesian X"), current_position.x, SP_Y_LBL, current_position.y);
update_software_endstops(axis);
}
}
void inverse_kinematics(const xyz_pos_t &raw) {
const float x = raw.x, y = raw.y, c = HYPOT(x, y),
THETA3 = ATAN2(y, x),
@@ -175,6 +180,22 @@ void scara_set_axis_is_at_home(const AxisEnum axis) {
static constexpr xyz_pos_t robot_offset = { TPARA_OFFSET_X, TPARA_OFFSET_Y, TPARA_OFFSET_Z };
void scara_set_axis_is_at_home(const AxisEnum axis) {
if (axis == Z_AXIS)
current_position.z = Z_HOME_POS;
else {
xyz_pos_t homeposition = { X_HOME_POS, Y_HOME_POS, Z_HOME_POS };
//DEBUG_ECHOLNPAIR_P(PSTR("homeposition X"), homeposition.x, SP_Y_LBL, homeposition.y, SP_Z_LBL, homeposition.z);
inverse_kinematics(homeposition);
forward_kinematics(delta.a, delta.b, delta.c);
current_position[axis] = cartes[axis];
//DEBUG_ECHOLNPAIR_P(PSTR("Cartesian X"), current_position.x, SP_Y_LBL, current_position.y);
update_software_endstops(axis);
}
}
// Convert ABC inputs in degrees to XYZ outputs in mm
void forward_kinematics(const float &a, const float &b, const float &c) {
const float w = c - b,