mirror of
https://github.com/MarlinFirmware/Marlin.git
synced 2026-01-03 21:30:31 -07:00
🐛 Fix TPARA kinematics (#28068)
This commit is contained in:
Axel Sepúlveda
GitHub
parent
e3fa27ac9f
commit
8342a5a83e
8 changed files with 274 additions and 88 deletions
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
|
|
|
|||
|
|
@ -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<T> 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<T> 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)
|
||||
|
|
|
|||
|
|
@ -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
|
||||
);
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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"
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue