From dd388aedfda25796c86b532605b26b1e598ce410 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Wed, 3 Mar 2021 17:57:24 -0600 Subject: [PATCH] TPARA followup --- Marlin/src/gcode/scara/M360-M364.cpp | 2 +- Marlin/src/module/delta.cpp | 2 +- Marlin/src/module/delta.h | 6 ++--- Marlin/src/module/motion.cpp | 6 ++--- Marlin/src/module/scara.cpp | 37 ++++++++++++++-------------- Marlin/src/module/scara.h | 4 +-- 6 files changed, 29 insertions(+), 28 deletions(-) diff --git a/Marlin/src/gcode/scara/M360-M364.cpp b/Marlin/src/gcode/scara/M360-M364.cpp index 562beee4f9..f32fa09de0 100644 --- a/Marlin/src/gcode/scara/M360-M364.cpp +++ b/Marlin/src/gcode/scara/M360-M364.cpp @@ -31,7 +31,7 @@ inline bool SCARA_move_to_cal(const uint8_t delta_a, const uint8_t delta_b) { if (IsRunning()) { - forward_kinematics_SCARA(delta_a, delta_b); + forward_kinematics(delta_a, delta_b); do_blocking_move_to_xy(cartes); return true; } diff --git a/Marlin/src/module/delta.cpp b/Marlin/src/module/delta.cpp index 565c676823..9857b89bf3 100644 --- a/Marlin/src/module/delta.cpp +++ b/Marlin/src/module/delta.cpp @@ -177,7 +177,7 @@ float delta_safe_distance_from_top() { * * The result is stored in the cartes[] array. */ -void forward_kinematics_DELTA(const float &z1, const float &z2, const float &z3) { +void forward_kinematics(const float &z1, const float &z2, const float &z3) { // Create a vector in old coordinates along x axis of new coordinate const float p12[3] = { delta_tower[B_AXIS].x - delta_tower[A_AXIS].x, delta_tower[B_AXIS].y - delta_tower[A_AXIS].y, z2 - z1 }, diff --git a/Marlin/src/module/delta.h b/Marlin/src/module/delta.h index 5e9a78bd86..a974da97c8 100644 --- a/Marlin/src/module/delta.h +++ b/Marlin/src/module/delta.h @@ -120,10 +120,10 @@ float delta_safe_distance_from_top(); * * The result is stored in the cartes[] array. */ -void forward_kinematics_DELTA(const float &z1, const float &z2, const float &z3); +void forward_kinematics(const float &z1, const float &z2, const float &z3); -FORCE_INLINE void forward_kinematics_DELTA(const abc_float_t &point) { - forward_kinematics_DELTA(point.a, point.b, point.c); +FORCE_INLINE void forward_kinematics(const abc_float_t &point) { + forward_kinematics(point.a, point.b, point.c); } void home_delta(); diff --git a/Marlin/src/module/motion.cpp b/Marlin/src/module/motion.cpp index 7b4d1e36b3..024f28dc9f 100644 --- a/Marlin/src/module/motion.cpp +++ b/Marlin/src/module/motion.cpp @@ -263,10 +263,10 @@ void sync_plan_position_e() { planner.set_e_position_mm(current_position.e); } */ void get_cartesian_from_steppers() { #if ENABLED(DELTA) - forward_kinematics_DELTA(planner.get_axis_positions_mm()); + forward_kinematics(planner.get_axis_positions_mm()); #else #if IS_SCARA - forward_kinematics_SCARA( + forward_kinematics( planner.get_axis_position_degrees(A_AXIS) , planner.get_axis_position_degrees(B_AXIS) #if ENABLED(AXEL_TPARA) @@ -949,7 +949,7 @@ FORCE_INLINE void segment_idle(millis_t &next_idle_ms) { float x_home_pos(const uint8_t extruder) { if (extruder == 0) - return base_home_pos(X_AXIS); + return X_HOME_POS; else /** * In dual carriage mode the extruder offset provides an override of the diff --git a/Marlin/src/module/scara.cpp b/Marlin/src/module/scara.cpp index a8a3acf0f8..7586b8136f 100644 --- a/Marlin/src/module/scara.cpp +++ b/Marlin/src/module/scara.cpp @@ -33,10 +33,7 @@ #include "planner.h" #if ENABLED(AXEL_TPARA) - // For homing, as in delta - #include "planner.h" #include "endstops.h" - #include "../lcd/marlinui.h" #include "../MarlinCore.h" #endif @@ -46,30 +43,35 @@ 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; - LOOP_XYZ(i) homeposition[i] = base_home_pos((AxisEnum)i); #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); - inverse_kinematics(homeposition); - forward_kinematics_SCARA(delta.a, delta.b); - current_position[axis] = cartes[axis]; #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); - delta.a = SCARA_OFFSET_THETA1; - delta.b = SCARA_OFFSET_THETA2; - forward_kinematics_SCARA(delta.a, delta.b); - current_position[axis] = cartes[axis]; #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); - inverse_kinematics(homeposition); - forward_kinematics_TPARA(delta.a, delta.b, delta.c); - current_position[axis] = cartes[axis]; #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); @@ -85,7 +87,7 @@ void scara_set_axis_is_at_home(const AxisEnum axis) { * 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) { + void forward_kinematics(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, @@ -174,7 +176,7 @@ 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 }; // Convert ABC inputs in degrees to XYZ outputs in mm - void forward_kinematics_TPARA(const float &a, const float &b, const float &c) { + void forward_kinematics(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)), @@ -227,7 +229,6 @@ void scara_set_axis_is_at_home(const AxisEnum 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 diff --git a/Marlin/src/module/scara.h b/Marlin/src/module/scara.h index d462842b57..5549e506b0 100644 --- a/Marlin/src/module/scara.h +++ b/Marlin/src/module/scara.h @@ -35,7 +35,7 @@ extern float delta_segments_per_second; L1_2 = sq(float(L1)), L1_2_2 = 2.0 * L1_2, L2_2 = sq(float(L2)); - void forward_kinematics_TPARA(const float &a, const float &b, const float &c); + void forward_kinematics(const float &a, const float &b, const float &c); void home_TPARA(); #else @@ -44,7 +44,7 @@ extern float delta_segments_per_second; L1_2 = sq(float(L1)), L1_2_2 = 2.0 * L1_2, L2_2 = sq(float(L2)); - void forward_kinematics_SCARA(const float &a, const float &b); + void forward_kinematics(const float &a, const float &b); #endif