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 Scott Lahteine
parent
10ec13b2c3
commit
6cfd190107
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
Split 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
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

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('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

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

@ -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;

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

@ -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;

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
// 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

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

@ -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,

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

@ -27,7 +27,7 @@
#include "../core/macros.h"
extern float delta_segments_per_second;
extern float segments_per_second;
#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
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<seg-per-sec> P<theta-psi-offset> T<theta-offset>");
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)

Write Preview
Loading…
Cancel
Save