From 6cfd190107e5a4aa3ad07dc1049b0bae1a101ce5 Mon Sep 17 00:00:00 2001 From: Tanguy Pruvot Date: Fri, 5 Mar 2021 00:34:38 +0100 Subject: [PATCH] Followup to MP_SCARA/TPARA patches (#21248) --- .../src/feature/bedlevel/ubl/ubl_motion.cpp | 2 +- Marlin/src/gcode/calibrate/M665.cpp | 4 +- Marlin/src/module/delta.cpp | 2 +- Marlin/src/module/delta.h | 2 +- Marlin/src/module/motion.cpp | 2 +- Marlin/src/module/scara.cpp | 101 +++++++++++------- Marlin/src/module/scara.h | 2 +- Marlin/src/module/settings.cpp | 12 +-- 8 files changed, 74 insertions(+), 53 deletions(-) diff --git a/Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp b/Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp index 8b7cd15a3..33b4f03ac 100644 --- a/Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp +++ b/Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp @@ -335,7 +335,7 @@ #if IS_KINEMATIC const float seconds = cart_xy_mm / scaled_fr_mm_s; // Duration of XY move at requested rate - uint16_t segments = LROUND(delta_segments_per_second * seconds), // Preferred number of segments for distance @ feedrate + uint16_t segments = LROUND(segments_per_second * seconds), // Preferred number of segments for distance @ feedrate seglimit = LROUND(cart_xy_mm * RECIPROCAL(DELTA_SEGMENT_MIN_LENGTH)); // Number of segments at minimum segment length NOMORE(segments, seglimit); // Limit to minimum segment length (fewer segments) #else diff --git a/Marlin/src/gcode/calibrate/M665.cpp b/Marlin/src/gcode/calibrate/M665.cpp index 3eac54f26..0d0c4146d 100644 --- a/Marlin/src/gcode/calibrate/M665.cpp +++ b/Marlin/src/gcode/calibrate/M665.cpp @@ -48,7 +48,7 @@ if (parser.seenval('H')) delta_height = parser.value_linear_units(); if (parser.seenval('L')) delta_diagonal_rod = parser.value_linear_units(); if (parser.seenval('R')) delta_radius = parser.value_linear_units(); - if (parser.seenval('S')) delta_segments_per_second = parser.value_float(); + if (parser.seenval('S')) segments_per_second = parser.value_float(); if (parser.seenval('X')) delta_tower_angle_trim.a = parser.value_float(); if (parser.seenval('Y')) delta_tower_angle_trim.b = parser.value_float(); if (parser.seenval('Z')) delta_tower_angle_trim.c = parser.value_float(); @@ -76,7 +76,7 @@ * B, T, and Y are all aliases for the elbow angle */ void GcodeSuite::M665() { - if (parser.seenval('S')) delta_segments_per_second = parser.value_float(); + if (parser.seenval('S')) segments_per_second = parser.value_float(); #if HAS_SCARA_OFFSET diff --git a/Marlin/src/module/delta.cpp b/Marlin/src/module/delta.cpp index 9857b89bf..a1676b3ba 100644 --- a/Marlin/src/module/delta.cpp +++ b/Marlin/src/module/delta.cpp @@ -54,7 +54,7 @@ float delta_height; abc_float_t delta_endstop_adj{0}; float delta_radius, delta_diagonal_rod, - delta_segments_per_second; + segments_per_second; abc_float_t delta_tower_angle_trim; xy_float_t delta_tower[ABC]; abc_float_t delta_diagonal_rod_2_tower; diff --git a/Marlin/src/module/delta.h b/Marlin/src/module/delta.h index a974da97c..9caec5302 100644 --- a/Marlin/src/module/delta.h +++ b/Marlin/src/module/delta.h @@ -32,7 +32,7 @@ extern float delta_height; extern abc_float_t delta_endstop_adj; extern float delta_radius, delta_diagonal_rod, - delta_segments_per_second; + segments_per_second; extern abc_float_t delta_tower_angle_trim; extern xy_float_t delta_tower[ABC]; extern abc_float_t delta_diagonal_rod_2_tower; diff --git a/Marlin/src/module/motion.cpp b/Marlin/src/module/motion.cpp index 024f28dc9..2f4f5e283 100644 --- a/Marlin/src/module/motion.cpp +++ b/Marlin/src/module/motion.cpp @@ -763,7 +763,7 @@ FORCE_INLINE void segment_idle(millis_t &next_idle_ms) { // The number of segments-per-second times the duration // gives the number of segments - uint16_t segments = delta_segments_per_second * seconds; + uint16_t segments = segments_per_second * seconds; // For SCARA enforce a minimum segment size #if IS_SCARA diff --git a/Marlin/src/module/scara.cpp b/Marlin/src/module/scara.cpp index 7586b8136..1767e230c 100644 --- a/Marlin/src/module/scara.cpp +++ b/Marlin/src/module/scara.cpp @@ -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, diff --git a/Marlin/src/module/scara.h b/Marlin/src/module/scara.h index 5549e506b..d24a4110f 100644 --- a/Marlin/src/module/scara.h +++ b/Marlin/src/module/scara.h @@ -27,7 +27,7 @@ #include "../core/macros.h" -extern float delta_segments_per_second; +extern float segments_per_second; #if ENABLED(AXEL_TPARA) diff --git a/Marlin/src/module/settings.cpp b/Marlin/src/module/settings.cpp index d78d97e68..c82ed0eb8 100644 --- a/Marlin/src/module/settings.cpp +++ b/Marlin/src/module/settings.cpp @@ -279,7 +279,7 @@ typedef struct SettingsDataStruct { abc_float_t delta_endstop_adj; // M666 X Y Z float delta_radius, // M665 R delta_diagonal_rod, // M665 L - delta_segments_per_second; // M665 S + segments_per_second; // M665 S abc_float_t delta_tower_angle_trim, // M665 X Y Z delta_diagonal_rod_trim; // M665 A B C #elif HAS_EXTRA_ENDSTOPS @@ -840,7 +840,7 @@ void MarlinSettings::postprocess() { EEPROM_WRITE(delta_endstop_adj); // 3 floats EEPROM_WRITE(delta_radius); // 1 float EEPROM_WRITE(delta_diagonal_rod); // 1 float - EEPROM_WRITE(delta_segments_per_second); // 1 float + EEPROM_WRITE(segments_per_second); // 1 float EEPROM_WRITE(delta_tower_angle_trim); // 3 floats EEPROM_WRITE(delta_diagonal_rod_trim); // 3 floats @@ -1721,7 +1721,7 @@ void MarlinSettings::postprocess() { EEPROM_READ(delta_endstop_adj); // 3 floats EEPROM_READ(delta_radius); // 1 float EEPROM_READ(delta_diagonal_rod); // 1 float - EEPROM_READ(delta_segments_per_second); // 1 float + EEPROM_READ(segments_per_second); // 1 float EEPROM_READ(delta_tower_angle_trim); // 3 floats EEPROM_READ(delta_diagonal_rod_trim); // 3 floats @@ -2711,7 +2711,7 @@ void MarlinSettings::reset() { delta_endstop_adj = adj; delta_radius = DELTA_RADIUS; delta_diagonal_rod = DELTA_DIAGONAL_ROD; - delta_segments_per_second = DELTA_SEGMENTS_PER_SECOND; + segments_per_second = DELTA_SEGMENTS_PER_SECOND; delta_tower_angle_trim = dta; delta_diagonal_rod_trim = ddr; #endif @@ -3320,7 +3320,7 @@ void MarlinSettings::reset() { CONFIG_ECHO_HEADING("SCARA settings: S P T"); CONFIG_ECHO_START(); SERIAL_ECHOLNPAIR_P( - PSTR(" M665 S"), delta_segments_per_second + PSTR(" M665 S"), segments_per_second , SP_P_STR, scara_home_offset.a , SP_T_STR, scara_home_offset.b , SP_Z_STR, LINEAR_UNIT(scara_home_offset.z) @@ -3342,7 +3342,7 @@ void MarlinSettings::reset() { PSTR(" M665 L"), LINEAR_UNIT(delta_diagonal_rod) , PSTR(" R"), LINEAR_UNIT(delta_radius) , PSTR(" H"), LINEAR_UNIT(delta_height) - , PSTR(" S"), delta_segments_per_second + , PSTR(" S"), segments_per_second , SP_X_STR, LINEAR_UNIT(delta_tower_angle_trim.a) , SP_Y_STR, LINEAR_UNIT(delta_tower_angle_trim.b) , SP_Z_STR, LINEAR_UNIT(delta_tower_angle_trim.c)