andrey
/
Marlin_FB4S
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Followup to MP_SCARA/TPARA patches (#21248)

vanilla_fb_2.0.x
Tanguy Pruvot 4 years ago
committed by GitHub
parent
6903a2ffc5
commit
a0d312396a
No known key found for this signature in database GPG Key ID: 4AEE18F83AFDEB23
8 changed files with 74 additions and 53 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp
  2. 4
      Marlin/src/gcode/calibrate/M665.cpp
  3. 2
      Marlin/src/module/delta.cpp
  4. 2
      Marlin/src/module/delta.h
  5. 2
      Marlin/src/module/motion.cpp
  6. 101
      Marlin/src/module/scara.cpp
  7. 2
      Marlin/src/module/scara.h
  8. 12
      Marlin/src/module/settings.cpp

2
Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp
View File

@ -335,7 +335,7 @@
#if IS_KINEMATIC #if IS_KINEMATIC
const float seconds = cart_xy_mm / scaled_fr_mm_s; // Duration of XY move at requested rate 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 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) NOMORE(segments, seglimit); // Limit to minimum segment length (fewer segments)
#else #else

4
Marlin/src/gcode/calibrate/M665.cpp
View File

@ -48,7 +48,7 @@
if (parser.seenval('H')) delta_height = parser.value_linear_units(); if (parser.seenval('H')) delta_height = parser.value_linear_units();
if (parser.seenval('L')) delta_diagonal_rod = 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('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('X')) delta_tower_angle_trim.a = parser.value_float();
if (parser.seenval('Y')) delta_tower_angle_trim.b = 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(); 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 * B, T, and Y are all aliases for the elbow angle
*/ */
void GcodeSuite::M665() { 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 #if HAS_SCARA_OFFSET

2
Marlin/src/module/delta.cpp
View File

@ -54,7 +54,7 @@ float delta_height;
abc_float_t delta_endstop_adj{0}; abc_float_t delta_endstop_adj{0};
float delta_radius, float delta_radius,
delta_diagonal_rod, delta_diagonal_rod,
delta_segments_per_second; segments_per_second;
abc_float_t delta_tower_angle_trim; abc_float_t delta_tower_angle_trim;
xy_float_t delta_tower[ABC]; xy_float_t delta_tower[ABC];
abc_float_t delta_diagonal_rod_2_tower; abc_float_t delta_diagonal_rod_2_tower;

2
Marlin/src/module/delta.h
View File

@ -32,7 +32,7 @@ extern float delta_height;
extern abc_float_t delta_endstop_adj; extern abc_float_t delta_endstop_adj;
extern float delta_radius, extern float delta_radius,
delta_diagonal_rod, delta_diagonal_rod,
delta_segments_per_second; segments_per_second;
extern abc_float_t delta_tower_angle_trim; extern abc_float_t delta_tower_angle_trim;
extern xy_float_t delta_tower[ABC]; extern xy_float_t delta_tower[ABC];
extern abc_float_t delta_diagonal_rod_2_tower; extern abc_float_t delta_diagonal_rod_2_tower;

2
Marlin/src/module/motion.cpp
View File

@ -763,7 +763,7 @@ FORCE_INLINE void segment_idle(millis_t &next_idle_ms) {
// The number of segments-per-second times the duration // The number of segments-per-second times the duration
// gives the number of segments // 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 // For SCARA enforce a minimum segment size
#if IS_SCARA #if IS_SCARA

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

@ -37,46 +37,7 @@
#include "../MarlinCore.h" #include "../MarlinCore.h"
#endif #endif
float delta_segments_per_second = TERN(AXEL_TPARA, TPARA_SEGMENTS_PER_SECOND, SCARA_SEGMENTS_PER_SECOND); float 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);
}
}
#if EITHER(MORGAN_SCARA, MP_SCARA) #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'. * 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) #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) { void inverse_kinematics(const xyz_pos_t &raw) {
const float x = raw.x, y = raw.y, c = HYPOT(x, y), const float x = raw.x, y = raw.y, c = HYPOT(x, y),
THETA3 = ATAN2(y, x), 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 }; 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 // Convert ABC inputs in degrees to XYZ outputs in mm
void forward_kinematics(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, const float w = c - b,

2
Marlin/src/module/scara.h
View File

@ -27,7 +27,7 @@
#include "../core/macros.h" #include "../core/macros.h"
extern float delta_segments_per_second; extern float segments_per_second;
#if ENABLED(AXEL_TPARA) #if ENABLED(AXEL_TPARA)

12
Marlin/src/module/settings.cpp
View File

@ -279,7 +279,7 @@ typedef struct SettingsDataStruct {
abc_float_t delta_endstop_adj; // M666 X Y Z abc_float_t delta_endstop_adj; // M666 X Y Z
float delta_radius, // M665 R float delta_radius, // M665 R
delta_diagonal_rod, // M665 L 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 abc_float_t delta_tower_angle_trim, // M665 X Y Z
delta_diagonal_rod_trim; // M665 A B C delta_diagonal_rod_trim; // M665 A B C
#elif HAS_EXTRA_ENDSTOPS #elif HAS_EXTRA_ENDSTOPS
@ -840,7 +840,7 @@ void MarlinSettings::postprocess() {
EEPROM_WRITE(delta_endstop_adj); // 3 floats EEPROM_WRITE(delta_endstop_adj); // 3 floats
EEPROM_WRITE(delta_radius); // 1 float EEPROM_WRITE(delta_radius); // 1 float
EEPROM_WRITE(delta_diagonal_rod); // 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_tower_angle_trim); // 3 floats
EEPROM_WRITE(delta_diagonal_rod_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_endstop_adj); // 3 floats
EEPROM_READ(delta_radius); // 1 float EEPROM_READ(delta_radius); // 1 float
EEPROM_READ(delta_diagonal_rod); // 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_tower_angle_trim); // 3 floats
EEPROM_READ(delta_diagonal_rod_trim); // 3 floats EEPROM_READ(delta_diagonal_rod_trim); // 3 floats
@ -2711,7 +2711,7 @@ void MarlinSettings::reset() {
delta_endstop_adj = adj; delta_endstop_adj = adj;
delta_radius = DELTA_RADIUS; delta_radius = DELTA_RADIUS;
delta_diagonal_rod = DELTA_DIAGONAL_ROD; 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_tower_angle_trim = dta;
delta_diagonal_rod_trim = ddr; delta_diagonal_rod_trim = ddr;
#endif #endif
@ -3320,7 +3320,7 @@ void MarlinSettings::reset() {
CONFIG_ECHO_HEADING("SCARA settings: S<seg-per-sec> P<theta-psi-offset> T<theta-offset>"); CONFIG_ECHO_HEADING("SCARA settings: S<seg-per-sec> P<theta-psi-offset> T<theta-offset>");
CONFIG_ECHO_START(); CONFIG_ECHO_START();
SERIAL_ECHOLNPAIR_P( SERIAL_ECHOLNPAIR_P(
PSTR(" M665 S"), delta_segments_per_second PSTR(" M665 S"), segments_per_second
, SP_P_STR, scara_home_offset.a , SP_P_STR, scara_home_offset.a
, SP_T_STR, scara_home_offset.b , SP_T_STR, scara_home_offset.b
, SP_Z_STR, LINEAR_UNIT(scara_home_offset.z) , 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(" M665 L"), LINEAR_UNIT(delta_diagonal_rod)
, PSTR(" R"), LINEAR_UNIT(delta_radius) , PSTR(" R"), LINEAR_UNIT(delta_radius)
, PSTR(" H"), LINEAR_UNIT(delta_height) , 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_X_STR, LINEAR_UNIT(delta_tower_angle_trim.a)
, SP_Y_STR, LINEAR_UNIT(delta_tower_angle_trim.b) , SP_Y_STR, LINEAR_UNIT(delta_tower_angle_trim.b)
, SP_Z_STR, LINEAR_UNIT(delta_tower_angle_trim.c) , SP_Z_STR, LINEAR_UNIT(delta_tower_angle_trim.c)

Write Preview
Loading…
Cancel
Save