MPCNC/Firmware2
1
0
Fork 0
Code Issues 1 Pull Requests Releases Wiki Activity

Followup to MP_SCARA/TPARA patches (#21248)

Browse Source
This commit is contained in:
Tanguy Pruvot 2021-03-05 00:34:38 +01:00 committed by Scott Lahteine
parent 10ec13b2c3
commit 6cfd190107
8 changed files with 74 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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)