From 8342a5a83e0bee0d5fd0ca8ca919272d471bd249 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Axel=20Sep=C3=BAlveda?= Date: Thu, 13 Nov 2025 23:14:57 -0300 Subject: [PATCH] =?UTF-8?q?=F0=9F=90=9B=20Fix=20TPARA=20kinematics=20(#280?= =?UTF-8?q?68)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Marlin/Configuration.h | 32 ++++- Marlin/src/inc/Conditionals-5-post.h | 26 +++- Marlin/src/inc/SanityCheck.h | 7 + Marlin/src/module/motion.cpp | 73 +++++++--- Marlin/src/module/motion.h | 2 +- Marlin/src/module/scara.cpp | 206 ++++++++++++++++++++------- Marlin/src/sd/cardreader.cpp | 4 +- buildroot/tests/mega2560 | 12 +- 8 files changed, 274 insertions(+), 88 deletions(-) diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h index a327c406ef..219cfe42c7 100644 --- a/Marlin/Configuration.h +++ b/Marlin/Configuration.h @@ -1110,17 +1110,37 @@ #define TPARA_LINKAGE_1 120 // (mm) #define TPARA_LINKAGE_2 120 // (mm) - // TPARA tower offset (position of Tower relative to bed zero position) + // Height of the Shoulder axis (pivot) relative to the tower floor + #define TPARA_SHOULDER_AXIS_HEIGHT 135.0 // (mm) + + // The position of the last linkage relative to the robot arm origin + // (intersection of the base axis and floor) when at the home position + #define TPARA_ARM_X_HOME_POS 28.75 // (mm) Measured from shoulder axis to tool holder axis in home position + #define TPARA_ARM_Y_HOME_POS 0 // (mm) + #define TPARA_ARM_Z_HOME_POS 250.00 // (mm) Measured from tool holder axis to the floor + + // TPARA Workspace offset relative to the tower (position of workspace origin relative to robot Tower origin ) // This needs to be reasonably accurate as it defines the printbed position in the TPARA space. - #define TPARA_OFFSET_X 0 // (mm) - #define TPARA_OFFSET_Y 0 // (mm) - #define TPARA_OFFSET_Z 0 // (mm) + #define TPARA_OFFSET_X 127.0 // (mm) to coincide with minimum radius MIDDLE_DEAD_ZONE_R, and W(0,0,0) is reachable + #define TPARA_OFFSET_Y 0.0 // (mm) + #define TPARA_OFFSET_Z 0.0 // (mm) + + // TPARA tool connection point offset, relative to the tool moving frame origin which is in the last linkage axis, + // (TCP: tool center/connection point) of the robot, + // the plane of measured offset must be alligned with home position plane + #define TPARA_TCP_OFFSET_X 27.0 // (mm) Tool flange: 27 (distance from pivot to bolt holes), extruder tool: 50.0, + #define TPARA_TCP_OFFSET_Y 0.0 // (mm) + #define TPARA_TCP_OFFSET_Z -65.0 // (mm) Tool flange (bottom): -6 (caution as Z 0 posiion will crash second linkage to the floor, -35 is safe for testing with no tool), extruder tool (depends on extruder): -65.0 #define FEEDRATE_SCALING // Convert XY feedrate from mm/s to degrees/s on the fly // Radius around the center where the arm cannot reach - #define MIDDLE_DEAD_ZONE_R 0 // (mm) -#endif + // For now use a hardcoded uniform limit, although it should be calculated, or fix a limit for each axis angle + #define MIDDLE_DEAD_ZONE_R 100 // (mm) + + // Max angle between L1 and L2 + #define TPARA_MAX_L1L2_ANGLE 140.0f // (degrees) +#endif // AXEL_TPARA // @section polar diff --git a/Marlin/src/inc/Conditionals-5-post.h b/Marlin/src/inc/Conditionals-5-post.h index 9eb5031055..f2b3e361c6 100644 --- a/Marlin/src/inc/Conditionals-5-post.h +++ b/Marlin/src/inc/Conditionals-5-post.h @@ -313,10 +313,24 @@ /** * SCARA cannot use SLOWDOWN and requires QUICKHOME * Printable radius assumes joints can fully extend + * + * TPARA cannot use SLOWDOWN nor QUICKHOME + * Printable radius assumes joints can't fully extend + * AXEL_TPARA is assigned a default Home Position unless overridden */ #if IS_SCARA #if ENABLED(AXEL_TPARA) - #define PRINTABLE_RADIUS (TPARA_LINKAGE_1 + TPARA_LINKAGE_2) + #define PRINTABLE_RADIUS_2 HYPOT2(TPARA_LINKAGE_1, TPARA_LINKAGE_2) - 2 * (TPARA_LINKAGE_1) * (TPARA_LINKAGE_2) * cosf(TPARA_MAX_L1L2_ANGLE) + #define PRINTABLE_RADIUS SQRT(PRINTABLE_RADIUS_2) + #ifndef MANUAL_X_HOME_POS + #define MANUAL_X_HOME_POS (TPARA_ARM_X_HOME_POS + TPARA_TCP_OFFSET_X - TPARA_OFFSET_X) + #endif + #ifndef MANUAL_Y_HOME_POS + #define MANUAL_Y_HOME_POS (TPARA_ARM_Y_HOME_POS + TPARA_TCP_OFFSET_Y - TPARA_OFFSET_Y) + #endif + #ifndef MANUAL_Z_HOME_POS + #define MANUAL_Z_HOME_POS (TPARA_ARM_Z_HOME_POS + TPARA_TCP_OFFSET_Z - TPARA_OFFSET_Z) + #endif #else #define QUICK_HOME #define PRINTABLE_RADIUS (SCARA_LINKAGE_1 + SCARA_LINKAGE_2) @@ -352,10 +366,12 @@ #endif #endif -#ifdef MANUAL_Z_HOME_POS - #define Z_HOME_POS MANUAL_Z_HOME_POS -#else - #define Z_HOME_POS TERN(Z_HOME_TO_MIN, Z_MIN_POS, Z_MAX_POS) +#if HAS_Z_AXIS + #ifdef MANUAL_Z_HOME_POS + #define Z_HOME_POS MANUAL_Z_HOME_POS + #else + #define Z_HOME_POS TERN(Z_HOME_TO_MIN, Z_MIN_POS, Z_MAX_POS) + #endif #endif #if HAS_I_AXIS diff --git a/Marlin/src/inc/SanityCheck.h b/Marlin/src/inc/SanityCheck.h index b8b4a42f40..fc31f9901b 100644 --- a/Marlin/src/inc/SanityCheck.h +++ b/Marlin/src/inc/SanityCheck.h @@ -1252,6 +1252,13 @@ static_assert(NUM_SERVOS <= NUM_SERVO_PLUGS, "NUM_SERVOS (or some servo index) i #endif #endif +/** + * Axel TPARA requirements + */ +#if ENABLED(AXEL_TPARA) && !ALL(HOME_Z_FIRST, HOME_Y_BEFORE_X) + #error "AXEL_TPARA requires both HOME_Z_FIRST and HOME_Y_BEFORE_X to be enabled." +#endif + /** * Junction deviation is incompatible with kinematic systems. */ diff --git a/Marlin/src/module/motion.cpp b/Marlin/src/module/motion.cpp index 34f516c60a..42d673db93 100644 --- a/Marlin/src/module/motion.cpp +++ b/Marlin/src/module/motion.cpp @@ -85,19 +85,34 @@ bool relative_mode; // = false bool z_min_trusted; // = false #endif +// Warn for unexpected TPARA home position +#if ENABLED(AXEL_TPARA) + static_assert( + ABS(X_HOME_POS - (TPARA_ARM_X_HOME_POS + TPARA_TCP_OFFSET_X - TPARA_OFFSET_X)) < 0.01f, + "X_HOME_POS should be equal to (TPARA_ARM_X_HOME_POS + TPARA_TCP_OFFSET_X - TPARA_OFFSET_X)"); + static_assert( + ABS(Y_HOME_POS - (TPARA_ARM_Y_HOME_POS + TPARA_TCP_OFFSET_Y - TPARA_OFFSET_Y)) < 0.01f, + "Y_HOME_POS should be equal to (TPARA_ARM_Y_HOME_POS + TPARA_TCP_OFFSET_Y - TPARA_OFFSET_Y)."); + static_assert( + ABS(Z_HOME_POS - (TPARA_ARM_Z_HOME_POS + TPARA_TCP_OFFSET_Z - TPARA_OFFSET_Z)) < 0.01f, + "Z_HOME_POS should be equal to (TPARA_ARM_Z_HOME_POS + TPARA_TCP_OFFSET_Z - TPARA_OFFSET_Z)."); +#endif + /** * Cartesian Current Position * Used to track the native machine position as moves are queued. * Used by 'line_to_current_position' to do a move after changing it. * Used by 'sync_plan_position' to update 'planner.position'. */ -#ifdef Z_IDLE_HEIGHT - #define Z_INIT_POS Z_IDLE_HEIGHT -#else - #define Z_INIT_POS Z_HOME_POS -#endif - -xyze_pos_t current_position = LOGICAL_AXIS_ARRAY(0, X_HOME_POS, Y_HOME_POS, Z_INIT_POS, I_HOME_POS, J_HOME_POS, K_HOME_POS, U_HOME_POS, V_HOME_POS, W_HOME_POS); +xyze_pos_t current_position = LOGICAL_AXIS_ARRAY(0, + X_HOME_POS, Y_HOME_POS, + #ifdef Z_IDLE_HEIGHT + Z_IDLE_HEIGHT + #else + Z_HOME_POS + #endif + , I_HOME_POS, J_HOME_POS, K_HOME_POS, U_HOME_POS, V_HOME_POS, W_HOME_POS +); /** * Cartesian Destination @@ -154,18 +169,18 @@ xyz_pos_t cartes; #if HAS_SCARA_OFFSET abc_pos_t scara_home_offset; #endif - + // If it does not have software endstops, use the printable radius #if HAS_SOFTWARE_ENDSTOPS float delta_max_radius, delta_max_radius_2; #elif IS_SCARA constexpr float delta_max_radius = PRINTABLE_RADIUS, - delta_max_radius_2 = sq(PRINTABLE_RADIUS); + delta_max_radius_2 = sq(float(PRINTABLE_RADIUS)); #elif ENABLED(POLAR) constexpr float delta_max_radius = PRINTABLE_RADIUS, - delta_max_radius_2 = sq(PRINTABLE_RADIUS); + delta_max_radius_2 = sq(float(PRINTABLE_RADIUS)); #else // DELTA constexpr float delta_max_radius = PRINTABLE_RADIUS, - delta_max_radius_2 = sq(PRINTABLE_RADIUS); + delta_max_radius_2 = sq(float(PRINTABLE_RADIUS)); #endif #endif @@ -179,6 +194,7 @@ xyz_pos_t cartes; // Set by M206, M428, or menu item. Saved to EEPROM. xyz_pos_t home_offset{0}; #endif + #if HAS_WORKSPACE_OFFSET // The above two are combined to save on computes xyz_pos_t workspace_offset{0}; @@ -633,12 +649,14 @@ void report_current_position_projected() { can_reach = HYPOT2(rx, ry) <= sq(PRINTABLE_RADIUS - inset + fslop); #elif ENABLED(AXEL_TPARA) - - const float R2 = HYPOT2(rx - TPARA_OFFSET_X, ry - TPARA_OFFSET_Y); + // TODO: A custom check, as reach depends also on Z destination. + // During printing assume destination Z is the current Z. + // For now use the max reach of the arm. + const float R2 = HYPOT2(rx + TPARA_OFFSET_X, ry + TPARA_OFFSET_Y); can_reach = ( - R2 <= sq(L1 + L2) - inset + R2 <= PRINTABLE_RADIUS_2 - inset #if MIDDLE_DEAD_ZONE_R > 0 - && R2 >= FLOAT_SQ(MIDDLE_DEAD_ZONE_R) + && R2 >= FLOAT_SQ(MIDDLE_DEAD_ZONE_R + TPARA_TCP_OFFSET_X) #endif ); @@ -726,6 +744,8 @@ void quickstop_stepper() { void sync_plan_position() { if (DEBUGGING(LEVELING)) DEBUG_POS("sync_plan_position", current_position); planner.set_position_mm(current_position); + //SERIAL_ECHOLNPGM("Sync_plan_position: ", current_position.x, ", ", current_position.y, ", ", current_position.z); + //SERIAL_EOL(); } #if HAS_EXTRUDERS @@ -818,6 +838,9 @@ void line_to_current_position(const feedRate_t fr_mm_s/*=feedrate_mm_s*/) { void prepare_fast_move_to_destination(const feedRate_t scaled_fr_mm_s/*=MMS_SCALED(feedrate_mm_s)*/) { if (DEBUGGING(LEVELING)) DEBUG_POS("prepare_fast_move_to_destination", destination); + //SERIAL_ECHOLNPGM("Prepare fast move to destination: ", destination.x , ",", destination.y, ",", destination.z, "," , destination.e); + //SERIAL_EOL(); + #if UBL_SEGMENTED // UBL segmented line will do Z-only moves in single segment bedlevel.line_to_destination_segmented(scaled_fr_mm_s); @@ -1260,6 +1283,8 @@ void restore_feedrate_and_scaling() { * radius within the set software endstops. */ void apply_motion_limits(xyz_pos_t &target) { + //SERIAL_ECHOLNPGM("Motion limits in: ", target.x, ", ", target.y, ", ", target.z); + //SERIAL_EOL(); if (!soft_endstop._enabled) return; @@ -1287,6 +1312,8 @@ void restore_feedrate_and_scaling() { #else if (TERN1(IS_SCARA, axis_was_homed(X_AXIS) && axis_was_homed(Y_AXIS))) { const float dist_2 = HYPOT2(target.x - offs.x, target.y - offs.y); + //SERIAL_ECHOLNPGM("Motion limits data: dist_2:", dist_2, " delta_max_radius_2: ", delta_max_radius_2); + //SERIAL_EOL(); if (dist_2 > delta_max_radius_2) target *= float(delta_max_radius / SQRT(dist_2)); // 200 / 300 = 0.66 } @@ -1538,8 +1565,13 @@ float get_move_distance(const xyze_pos_t &diff OPTARG(HAS_ROTATIONAL_AXES, bool const xyze_float_t diff = destination - current_position; - // If the move is only in Z/E don't split up the move - if (!diff.x && !diff.y) { + //SERIAL_ECHOLNPGM("Destination: ", destination.x, " , ", destination.y, " , ", destination.z, " , ", destination.e); + //SERIAL_ECHOLNPGM("Current pos: ", current_position.x, " , ", current_position.y, " , ", current_position.z, " , ", current_position.e); + //SERIAL_ECHOLNPGM("Difference : ", diff.x, " , ", diff.y, " , ", diff.z, " , ", diff.e); + + // For TPARA always split up the move, then skip next code + // For DELTA/SCARA if the move is only in Z/E don't split up the move + if (TERN0(AXEL_TPARA, !diff.x && !diff.y)) { planner.buffer_line(destination, scaled_fr_mm_s); return false; // caller will update current_position } @@ -1872,6 +1904,9 @@ float get_move_distance(const xyze_pos_t &diff OPTARG(HAS_ROTATIONAL_AXES, bool void prepare_line_to_destination() { apply_motion_limits(destination); + //SERIAL_ECHOLNPGM(">TPARA Prepare line to destination: ", destination.x , " , ", destination.y, " , ", destination.z, " , " , destination.e); + //SERIAL_EOL(); + #if ANY(PREVENT_COLD_EXTRUSION, PREVENT_LENGTHY_EXTRUDE) if (!DEBUGGING(DRYRUN) && destination.e != current_position.e) { @@ -2782,6 +2817,10 @@ void prepare_line_to_destination() { * current_position to home, because neither X nor Y is at home until * both are at home. Z can however be homed individually. * + * TPARA should wait until all YZ homing is done before setting the YZ + * current_position to home, because neither Y nor Z is at home until + * both are at home. X can however be homed individually. + * * Callers must sync the planner position after calling this! */ void set_axis_is_at_home(const AxisEnum axis) { diff --git a/Marlin/src/module/motion.h b/Marlin/src/module/motion.h index 7cafb2f019..0ebd1b5748 100644 --- a/Marlin/src/module/motion.h +++ b/Marlin/src/module/motion.h @@ -137,7 +137,7 @@ inline int8_t pgm_read_any(const int8_t *p) { return TERN(__IMXRT1062__, *p, pgm #define XYZ_DEFS(T, NAME, OPT) \ inline T NAME(const AxisEnum axis) { \ - static const XYZval NAME##_P DEFS_PROGMEM = NUM_AXIS_ARRAY(X_##OPT, Y_##OPT, Z_##OPT, I_##OPT, J_##OPT, K_##OPT, U_##OPT, V_##OPT, W_##OPT); \ + static constexpr XYZval NAME##_P DEFS_PROGMEM = NUM_AXIS_ARRAY(X_##OPT, Y_##OPT, Z_##OPT, I_##OPT, J_##OPT, K_##OPT, U_##OPT, V_##OPT, W_##OPT); \ return pgm_read_any(&NAME##_P[axis]); \ } XYZ_DEFS(float, base_min_pos, MIN_POS); // base_min_pos(axis) diff --git a/Marlin/src/module/scara.cpp b/Marlin/src/module/scara.cpp index d6016d77a1..8274335862 100644 --- a/Marlin/src/module/scara.cpp +++ b/Marlin/src/module/scara.cpp @@ -41,7 +41,7 @@ float segments_per_second = DEFAULT_SEGMENTS_PER_SECOND; #if ANY(MORGAN_SCARA, MP_SCARA) - static constexpr xy_pos_t scara_offset = { SCARA_OFFSET_X, SCARA_OFFSET_Y }; + constexpr xy_pos_t scara_offset = { SCARA_OFFSET_X, SCARA_OFFSET_Y }; /** * Morgan SCARA Forward Kinematics. Results in 'cartes'. @@ -178,22 +178,97 @@ float segments_per_second = DEFAULT_SEGMENTS_PER_SECOND; #elif ENABLED(AXEL_TPARA) - static constexpr xyz_pos_t robot_offset = { TPARA_OFFSET_X, TPARA_OFFSET_Y, TPARA_OFFSET_Z }; + // TPARA offset relative to the origin of the robot + constexpr xyz_pos_t robot_shoulder_offset = { 0, 0, TPARA_SHOULDER_AXIS_HEIGHT }; + // Workspace offset relative to the origin of the robot + constexpr xyz_pos_t robot_workspace_offset = { TPARA_OFFSET_X, TPARA_OFFSET_Y, TPARA_OFFSET_Z }; + // Tool offset relative to the tool center point of the robot + constexpr xyz_pos_t tool_offset = { TPARA_TCP_OFFSET_X, TPARA_TCP_OFFSET_Y, TPARA_TCP_OFFSET_Z }; + // Tool offset in cylindrical coordinates (r, phi, z) + constexpr xyz_pos_t tool_offset_cyl = { SQRT(sq(TPARA_TCP_OFFSET_X) + sq(TPARA_TCP_OFFSET_Y)) , ATAN2(TPARA_TCP_OFFSET_Y, TPARA_TCP_OFFSET_X), TPARA_TCP_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_ECHOLNPGM_P(PSTR("homeposition X"), homeposition.x, SP_Y_LBL, homeposition.y, SP_Z_LBL, homeposition.z); + //xyz_pos_t home_t_w_offset = tool_offset - robot_workspace_offset; - inverse_kinematics(homeposition); - forward_kinematics(delta.a, delta.b, delta.c); - current_position[axis] = cartes[axis]; + // Remove offset for calculation with trigonometric + // Tool offset coordinates are recalculated for each angle + xyz_pos_t remove_W_T_offset(const xyz_pos_t &raw) { + // Remove workspace offset first, so we can use trigonometrics relative to robot reference + // frame (otherwise a negative raw position would "mirror/invert" the tool offset) + xyz_pos_t toolhead_absolute = raw + robot_workspace_offset; - //DEBUG_ECHOLNPGM_P(PSTR("Cartesian X"), current_position.x, SP_Y_LBL, current_position.y); - update_software_endstops(axis); + // We should apply a rotation matrix, but is too costly to calculate sin and cos + const float r2 = HYPOT2(toolhead_absolute.x, toolhead_absolute.y); + xyz_pos_t tool_offset_rotated; + if (UNEAR_ZERO(r2)) { + // avoid zero div + tool_offset_rotated.x = tool_offset_cyl.x; + tool_offset_rotated.y = 0.0f; + tool_offset_rotated.z = tool_offset_cyl.z; } + else { + const float inv_r = RSQRT(r2); + tool_offset_rotated.x = tool_offset_cyl.x * toolhead_absolute.x * inv_r; // Equivalent to tool_offset_cyl.x * cos(atan2(toolhead_absolute.y, toolhead_absolute.x)) + tool_offset_rotated.y = tool_offset_cyl.x * toolhead_absolute.y * inv_r; // Equivalent to tool_offset_cyl.x * sin(atan2(toolhead_absolute.y, toolhead_absolute.x)) + tool_offset_rotated.z = tool_offset_cyl.z; + } + + return toolhead_absolute - tool_offset_rotated; // Returns the real robot position without tool or workspace offset + } + + // Apply tool and workspace offset to robot flange position, accounting for the rotated tool offset + constexpr xyz_pos_t apply_T_W_offset(const xyz_pos_t &rpos) { + // We should apply a rotation matrix, but it's too costly + const float r2 = rpos.x * rpos.x + rpos.y * rpos.y; + if (UNEAR_ZERO(r2)) { + // avoid zero div + xyz_pos_t tool_offset_rotated = { tool_offset_cyl.x, 0.0f, tool_offset_cyl.z }; + return rpos + tool_offset_rotated - robot_workspace_offset; + } + else { + const float inv_r = RSQRT(r2); + xyz_pos_t tool_offset_rotated = { + tool_offset_cyl.x * rpos.x * inv_r, + tool_offset_cyl.x * rpos.y * inv_r, + tool_offset_cyl.z + }; + //SERIAL_ECHOLNPGM(" Tool_offset_rotated(x,y,z) ", tool_offset_rotated.x, ",", tool_offset_rotated.y, ",", tool_offset_rotated.z ); + return rpos + tool_offset_rotated - robot_workspace_offset; + } + } + + /** + * Set an axis' current position to its home position (after homing). + * + * TPARA must wait for YZ homing before setting current_position.Y/Z to home. + * Neither Y nor Z is home until both are at home. + */ + void scara_set_axis_is_at_home(const AxisEnum axis) { + // Home position should be arm end position -+ offsets (+ tool offset - workspace offset), measured at home robot position + xyz_pos_t homeposition = { X_HOME_POS , Y_HOME_POS , Z_HOME_POS }; + + //SERIAL_ECHOLNPGM("TPARA Set axis is at home: ", C(iaxis_codes[axis])); + //SERIAL_XYZ("Home: ", homeposition); + //SERIAL_XYZ("Pos before IK: ", current_position); + //SERIAL_ECHOLNPGM("Angles Before: Theta: ", delta.a, " Phi: ", delta.b, " Psi: ", delta.c); + + inverse_kinematics(homeposition); + + //SERIAL_ECHOLNPGM("Angles After IK: Theta: ", delta.a, " Phi: ", delta.b, " Psi: ", delta.c); + + forward_kinematics(delta.a, delta.b, delta.c); + current_position[axis] = cartes[axis]; + + //SERIAL_XYZ("'current' after FK: ", current_position); + //SERIAL_XYZ("'cartes' after FK: ", cartes); + + update_software_endstops(axis); + + //SERIAL_ECHOLNPGM("Final Angles: Theta: ", delta.a, " Phi: ", delta.b, " Psi: ", delta.c); + //SERIAL_XYZ("Final Pos: ", current_position); + //SERIAL_XYZ("Robot Offsets Shoulder:", robot_shoulder_offset); + //SERIAL_XYZ("Robot Offsets Tool:", tool_offset); + //SERIAL_XYZ("Robot Offsets Workspace:", robot_workspace_offset); + //SERIAL_EOL(); } // Convert ABC inputs in degrees to XYZ outputs in mm @@ -204,35 +279,50 @@ float segments_per_second = DEFAULT_SEGMENTS_PER_SECOND; 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 - sq(x) - sq(y)) }); + const xyz_pos_t calculated_fk = xyz_pos_t({ x, y, SQRT(rho2 - sq(x) - sq(y)) }) ; + cartes = calculated_fk + robot_shoulder_offset + tool_offset - robot_workspace_offset; + + //SERIAL_ECHOPGM("TPARA FK Theta:", a, " Phi: ", b, " Psi: ", c); + //SERIAL_ECHOPGM(" Calculated X':", calculated_fk.x, " Y':", calculated_fk.y, " Z':", calculated_fk.z); + //SERIAL_XYZ(" Workspace", cartes); } // 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 + // First Init the current position of all carriages to 0,0,0 current_position.reset(); destination.reset(); sync_plan_position(); + //SERIAL_ECHOLNPGM("Reset and sync position to the assumed start position of the robot" ); + // Set the assumed start position of the robot for homing, so it home ZY axis at same time preserving the B and C motor angle + constexpr xyz_pos_t init_w_offset = apply_T_W_offset(xyz_pos_t({ L2, 0, 0 })); + + current_position.set(init_w_offset.x, init_w_offset.y, init_w_offset.z); + destination.set(init_w_offset.x, init_w_offset.y, init_w_offset.z); + 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)); + #if X_SENSORLESS + sensorless_t stealth_states_x = start_sensorless_homing_per_axis(X_AXIS); + #endif + #if Y_SENSORLESS + sensorless_t stealth_states_y = start_sensorless_homing_per_axis(Y_AXIS); + #endif + #if Z_SENSORLESS + sensorless_t stealth_states_z = start_sensorless_homing_per_axis(Z_AXIS); + #endif #endif - //const int x_axis_home_dir = TOOL_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)); - // Set the homing current for all motors TERN_(HAS_HOMING_CURRENT, set_homing_current(Z_AXIS)); - current_position.set(0, 0, max_length(Z_AXIS)); + // Move to home, should move Z, Y, then X. Move X to near 0 (to avoid div by zero + // and sign/angle stability around 0 for trigonometric functions), Y to 0 and Z to max_length + constexpr xyz_pos_t homing_pos_dir = apply_T_W_offset(xyz_pos_t({ 1, 0, Z_MAX_LENGTH })); + current_position.set(homing_pos_dir.x, homing_pos_dir.y, homing_pos_dir.z); + line_to_current_position(homing_feedrate(Z_AXIS)); planner.synchronize(); @@ -250,9 +340,12 @@ float segments_per_second = DEFAULT_SEGMENTS_PER_SECOND; // At least one motor has reached its endstop. // Now re-home each motor separately. - homeaxis(A_AXIS); - homeaxis(C_AXIS); - homeaxis(B_AXIS); + TERN_(HOME_Z_FIRST, homeaxis(C_AXIS)); + homeaxis(TERN(HOME_Y_BEFORE_X, B_AXIS, A_AXIS)); + homeaxis(TERN(HOME_Y_BEFORE_X, A_AXIS, B_AXIS)); + IF_DISABLED(HOME_Z_FIRST, homeaxis(C_AXIS)); + + //SERIAL_ECHOLNPGM("current_position After Homeaxis: ", current_position.x, ", ", current_position.y, ", ", current_position.z); // Set all carriages to their home positions // Do this here all at once for Delta, because @@ -260,50 +353,55 @@ float segments_per_second = DEFAULT_SEGMENTS_PER_SECOND; // give the impression that they are the same. LOOP_NUM_AXES(i) set_axis_is_at_home((AxisEnum)i); + //SERIAL_ECHOLNPGM("Sync_plan_position after home"); 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), + // Remove offsets to calculate with trigonometric + const xyz_pos_t tpos = remove_W_T_offset(raw) - robot_shoulder_offset; //raw + robot_workspace_offset - tool_offset_rotated - robot_shoulder_offset; + // IK, Refer to TPARA analysis + const float RXY = SQRT(HYPOT2(tpos.x, tpos.y)), + RHO_2 = NORMSQ(tpos.x, tpos.y, tpos.z), //RHO = SQRT(RHO2), - LSS = L1_2 + L2_2, - LM = 2.0f * L1 * L2, + LSS = L1_2 + L2_2, // L1^2 + L2^2 , LSS : Lenght square sum + LM = 2.0f * L1 * L2, // Length multiplication and doubled - CG = (LSS - RHO2) / LM, - SG = SQRT(1 - POW(CG, 2)), // Method 2 - K1 = L1 - L2 * CG, - K2 = L2 * SG, + // Method 2 + CG = (LSS - RHO_2) / LM, // Cosine of gamma + SG = SQRT(1 - POW(CG, 2)), // Sine of gamma + K1 = L1 - L2 * CG, // K1 projection + K2 = L2 * SG, // K2 projection - // Angle of Body Joint - THETA = ATAN2(spos.y, spos.x), + // Angle of Body/base Joint + THETA = ATAN2(tpos.y, tpos.x), - // Angle of Elbow Joint - //GAMMA = ACOS(CG), - GAMMA = ATAN2(SG, CG), // Method 2 + // Angle of Elbow Joint, between L1 and L2 + //GAMMA = ACOS(CG), // Method 1 + 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 + // Angle of Shoulder Joint, elevation angle measured from horizontal plane XY (r+) + //PHI = asin(tpos.z/RHO) + asin(L2 * sin(GAMMA) / RHO), // Method 1 + PHI = ATAN2(tpos.z, RXY) + ATAN2(K2, K1), // Method 2 - // Elbow motor angle measured from horizontal, same frame as shoulder (r+) + // Elbow motor angle measured from horizontal, same reference frame as shoulder angle (r+) PSI = PHI + GAMMA; delta.set(DEGREES(THETA), DEGREES(PHI), DEGREES(PSI)); - //SERIAL_ECHOLNPGM(" SCARA (x,y,z) ", spos.x, ",", spos.y, ",", spos.z, " Rho=", RHO, " Rho2=", RHO2, " Theta=", THETA, " Phi=", PHI, " Psi=", PSI, " Gamma=", GAMMA); + //SERIAL_ECHOLNPGM(" TPARA IK raw(x,y,z) ", raw.x, ",", raw.y, ",", raw.z, " Robot pos(x,y,z) ", tpos.x, ",", tpos.y, ",", tpos.z + robot_shoulder_offset.z, " Rho^2=", RHO_2, " Theta=", DEGREES(THETA), " Phi=", DEGREES(PHI), " Psi=", DEGREES(PSI), " Gamma=", DEGREES(GAMMA)); } -#endif +#endif // AXEL_TPARA void scara_report_positions() { - SERIAL_ECHOLNPGM("SCARA Theta:", planner.get_axis_position_degrees(A_AXIS) + SERIAL_ECHOLNPGM( #if ENABLED(AXEL_TPARA) - , " Phi:", planner.get_axis_position_degrees(B_AXIS) - , " Psi:", planner.get_axis_position_degrees(C_AXIS) + "TPARA Theta: ", planner.get_axis_position_degrees(A_AXIS) + , " Phi: ", planner.get_axis_position_degrees(B_AXIS) + , " Psi: ", planner.get_axis_position_degrees(C_AXIS) #else + "SCARA Theta:", planner.get_axis_position_degrees(A_AXIS) , " Psi" TERN_(MORGAN_SCARA, "+Theta") ":", planner.get_axis_position_degrees(B_AXIS) #endif ); diff --git a/Marlin/src/sd/cardreader.cpp b/Marlin/src/sd/cardreader.cpp index 7153b2fb1f..723f86a07e 100644 --- a/Marlin/src/sd/cardreader.cpp +++ b/Marlin/src/sd/cardreader.cpp @@ -371,7 +371,7 @@ void CardReader::ls(const uint8_t lsflags/*=0*/) { int i, pathLen = path ? strlen(path) : 0; - // SERIAL_ECHOPGM("Full Path: "); SERIAL_ECHOLN(path); + //SERIAL_ECHOPGM("Full Path: "); SERIAL_ECHOLN(path); // Zero out slashes to make segments for (i = 0; i < pathLen; i++) if (path[i] == '/') path[i] = '\0'; @@ -402,7 +402,7 @@ void CardReader::ls(const uint8_t lsflags/*=0*/) { // If the filename was printed then that's it if (!flag.filenameIsDir) break; - // SERIAL_ECHOPGM("Opening dir: "); SERIAL_ECHOLN(segment); + //SERIAL_ECHOPGM("Opening dir: "); SERIAL_ECHOLN(segment); // Open the sub-item as the new dive parent MediaFile dir; diff --git a/buildroot/tests/mega2560 b/buildroot/tests/mega2560 index f323602954..f90e2074ab 100755 --- a/buildroot/tests/mega2560 +++ b/buildroot/tests/mega2560 @@ -56,12 +56,12 @@ opt_disable SEGMENT_LEVELED_MOVES exec_test $1 $2 "Azteeg X3 Pro | EXTRUDERS 5 | RRDFGSC | UBL | LIN_ADVANCE | Sled Probe | Skew | JP-Kana | Babystep offsets ..." "$3" # -# 5 runout sensors with distinct states +# 4 runout sensors with distinct states # restore_configs opt_set MOTHERBOARD BOARD_AZTEEG_X3_PRO NUM_SERVOS 1 \ EXTRUDERS 4 TEMP_SENSOR_1 1 TEMP_SENSOR_2 1 TEMP_SENSOR_3 1 TEMP_SENSOR_4 1 FAN_KICKSTART_TIME 500 \ - NUM_RUNOUT_SENSORS 4 FIL_RUNOUT2_PIN 44 FIL_RUNOUT3_PIN 45 FIL_RUNOUT4_PIN 46 FIL_RUNOUT5_PIN 47 \ + NUM_RUNOUT_SENSORS 4 FIL_RUNOUT2_PIN 44 FIL_RUNOUT3_PIN 45 FIL_RUNOUT4_PIN 46 \ FIL_RUNOUT3_STATE HIGH FILAMENT_RUNOUT_SCRIPT '"M600 T%c"' opt_enable VIKI2 BOOT_MARLIN_LOGO_ANIMATED SDSUPPORT AUTO_REPORT_SD_STATUS \ Z_PROBE_SERVO_NR Z_SERVO_ANGLES Z_SERVO_MEASURE_ANGLE DEACTIVATE_SERVOS_AFTER_MOVE Z_SERVO_DEACTIVATE_AFTER_STOW \ @@ -72,6 +72,12 @@ opt_enable VIKI2 BOOT_MARLIN_LOGO_ANIMATED SDSUPPORT AUTO_REPORT_SD_STATUS \ FILAMENT_RUNOUT_SENSOR NOZZLE_PARK_FEATURE ADVANCED_PAUSE_FEATURE Z_SAFE_HOMING FIL_RUNOUT3_PULLUP exec_test $1 $2 "Azteeg X3 Pro | EXTRUDERS 4 | VIKI2 | Servo Probe | Multiple runout sensors (x4)" "$3" +# +# Axel TPARA kinematics +# +use_example_configs TPARA/AXEL_TPARA +exec_test $1 $2 "MEGA2560 RAMPS | AXEL_TPARA" "$3" + # # Extruder Only. No XYZ axes at all. # @@ -265,7 +271,7 @@ exec_test $1 $2 "Azteeg X3 | Mixing Extruder (x5) | Gradient Mix | Input Shaping #exec_test $1 $2 "Stuff" "$3" # -# Makibox Config need to check board type for Teensy++ 2.0 +# Makibox Config - need to check board type for Teensy++ 2.0 # #use_example_configs makibox #exec_test $1 $2 "Stuff" "$3"