Mirrors_Framework/grblHAL
1
0
Fork 0
mirror of https://github.com/grblHAL/core.git synced 2026-02-06 09:02:33 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

The method for constraining continuous multi-axis jogs (XYZ) (when enabled by `$40=1\) will now use clipping of the motion vector to the work envelope.

Browse Source
This commit is contained in:
Terje Io
2023-09-05 15:42:46 +02:00
parent 5c21425adb
commit e0fa78cb87
11 changed files with 273 additions and 120 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
README.md
View File

@@ -13,7 +13,7 @@ It has been written to complement grblHAL and has features such as proper keyboa
---
Latest build date is 20230903, see the [changelog](changelog.md) for details.
Latest build date is 20230905, see the [changelog](changelog.md) for details.
__NOTE:__ A settings reset will be performed on an update of builds earlier than 20230125. Backup and restore of settings is recommended.
__IMPORTANT!__ A new setting has been introduced for ganged axes motors in build 20211121.
I have only bench tested this for a couple of drivers, correct function should be verified after updating by those who have more than three motors configured.
@@ -92,4 +92,4 @@ G/M-codes not supported by [legacy Grbl](https://github.com/gnea/grbl/wiki) are
Some [plugins](https://github.com/grblHAL/plugins) implements additional M-codes.
---
2023-06-07
2023-09-05

18
changelog.md
View File

@@ -1,5 +1,23 @@
## grblHAL changelog
<a name="20230825"/>Build 20230905
Core:
* The method for constraining continuous multi-axis jogs \(XYZ\) \(when enabled by `$40=1`\) will now use clipping of the motion vector to the work envelope instead of limiting the target to min/max separately for each axis.
The signature of the new core handler introduced in build 20230903 for constraining jog motions has been changed to accomodate this.
* Delta robot kinematics tuning, added new setting for max allowed arm angle. Constraining jog movements to the full work envelope may now work \(I do not have a machine to test with\).
Still work in progress.
Drivers:
ESP32: fix for Modbus RTU serial port board definitions.
STM32F4xx: removed unused code.
---
<a name="20230825"/>Build 20230903
Core:

3
core_handlers.h
View File

@@ -79,6 +79,7 @@ typedef struct {
typedef bool (*enqueue_gcode_ptr)(char *data);
typedef bool (*protocol_enqueue_realtime_command_ptr)(char c);
typedef bool (*travel_limits_ptr)(float *target, bool is_cartesian);
typedef void (*jog_limits_ptr)(float *target, float *position);
typedef bool (*home_machine_ptr)(axes_signals_t cycle, axes_signals_t auto_square);
typedef void (*on_parser_init_ptr)(parser_state_t *gc_state);
@@ -174,7 +175,7 @@ typedef struct {
// core entry points - set up by core before driver_init() is called.
home_machine_ptr home_machine;
travel_limits_ptr check_travel_limits;
travel_limits_ptr apply_jog_limits;
jog_limits_ptr apply_jog_limits;
enqueue_gcode_ptr enqueue_gcode;
enqueue_realtime_command_ptr enqueue_realtime_command;
on_macro_execute_ptr on_macro_execute;

2
gcode.c
View File

@@ -2866,7 +2866,7 @@ status_code_t gc_execute_block (char *block)
plan_data.condition.coolant = gc_state.modal.coolant;
plan_data.condition.is_rpm_rate_adjusted = gc_state.is_rpm_rate_adjusted || (gc_state.modal.spindle.state.ccw && gc_state.spindle.hal->cap.laser);
if ((status_code_t)(int_value = (uint_fast16_t)mc_jog_execute(&plan_data, &gc_block)) == Status_OK)
if ((status_code_t)(int_value = (uint_fast16_t)mc_jog_execute(&plan_data, &gc_block, gc_state.position)) == Status_OK)
memcpy(gc_state.position, gc_block.values.xyz, sizeof(gc_state.position));
return (status_code_t)int_value;

2
grbl.h
View File

@@ -42,7 +42,7 @@
#else
#define GRBL_VERSION "1.1f"
#endif
#define GRBL_BUILD 20230903
#define GRBL_BUILD 20230905
#define GRBL_URL "https://github.com/grblHAL"

243
kinematics/delta.c
View File

@@ -60,6 +60,7 @@ typedef struct {
float b; // base to floor
float sl; // max segment length TODO: replace with calculated value?
float home_angle;
float max_angle;
delta_settings_flags_t flags;
} delta_settings_t;
@@ -70,39 +71,45 @@ typedef struct {
float envelope_midz;
float min_angle[3];
float max_angle[3];
coord_data_t last_pos;
float t;
float y1;
float y1_sqr;
float fe;
float rf_sqr; // bicep length ^ 2
float re_sqr; // forearm length ^ 2
delta_settings_t cfg;
} delta_properties_t;
static bool jog_cancel = false;
static coord_data_t last_pos = {0};
static delta_settings_t delta_settings;
static delta_properties_t machine;
static delta_properties_t machine = {0};
static homing_mode_t homing_mode;
static on_report_options_ptr on_report_options;
static on_homing_completed_ptr on_homing_completed;
static on_report_command_help_ptr on_report_command_help;
static on_set_axis_setting_unit_ptr on_set_axis_setting_unit;
static on_setting_get_description_ptr on_setting_get_description;
static jog_limits_ptr apply_jog_limits;
static nvs_address_t nvs_address;
// inverse kinematics
// helper functions, calculates angle position[X_AXIS] (for YZ-pane)
int delta_calcAngleYZ (float x0, float y0, float z0, float *theta)
{
float y1 = -0.5f * TAN30 * machine.cfg.f; // f/2 * tg 30
y0 -= 0.5f * TAN30 * machine.cfg.e; // shift center to edge
y0 -= machine.fe; // shift center to edge
// z = a + b*y
float a = (x0 * x0 + y0 * y0 + z0 * z0 + machine.cfg.rf * machine.cfg.rf - machine.cfg.re * machine.cfg.re - y1 * y1) / (2.0f * z0);
float b = (y1 - y0) / z0;
float a = (x0 * x0 + y0 * y0 + z0 * z0 + machine.rf_sqr - machine.re_sqr - machine.y1_sqr) / (2.0f * z0);
float b = (machine.y1 - y0) / z0;
// discriminant
float d = -(a + b * y1) * (a + b * y1) + machine.cfg.rf * (b * b * machine.cfg.rf + machine.cfg.rf);
float d = -(a + b * machine.y1) * (a + b * machine.y1) + machine.cfg.rf * (b * b * machine.cfg.rf + machine.cfg.rf);
if (d < 0.0f)
return -1; // non-existing point
float yj = (y1 - a * b - sqrt(d)) / (b * b + 1); // choosing outer point
float yj = (machine.y1 - a * b - sqrt(d)) / (b * b + 1); // choosing outer point
float zj = a + b * yj;
// *theta = 180.0f * atanf(-zj / (y1 - yj)) / M_PI + ((yj > y1) ? 180.0f : 0.0f);
*theta = atanf(-zj / (y1 - yj)) + ((yj > y1) ? M_PI : 0.0f);
*theta = atanf(-zj / (machine.y1 - yj)) + ((yj > machine.y1) ? M_PI : 0.0f);
return 0;
}
@@ -125,21 +132,14 @@ int delta_calcInverse (coord_data_t *mpos, float *position)
// Returns machine position in mm converted from system position.
static float *transform_to_cartesian (float *target, float *position)
{
float t = (machine.cfg.f - machine.cfg.e) * TAN30_2;
/* float dtr = M_PI / 180.0f;
position[X_AXIS] *= dtr;
position[Y_AXIS] *= dtr;
position[Z_AXIS] *= dtr;
*/
float y1 = -(t + machine.cfg.rf * cosf(position[X_AXIS]));
float y1 = -(machine.t + machine.cfg.rf * cosf(position[X_AXIS]));
float z1 = -machine.cfg.rf * sinf(position[X_AXIS]);
float y2 = (t + machine.cfg.rf * cosf(position[Y_AXIS])) * SIN30;
float y2 = (machine.t + machine.cfg.rf * cosf(position[Y_AXIS])) * SIN30;
float x2 = y2 * TAN60;
float z2 = -machine.cfg.rf * sinf(position[Y_AXIS]);
float y3 = (t + machine.cfg.rf * cosf(position[Z_AXIS])) * SIN30;
float y3 = (machine.t + machine.cfg.rf * cosf(position[Z_AXIS])) * SIN30;
float x3 = -y3 * TAN60;
float z3 = -machine.cfg.rf * sinf(position[Z_AXIS]);
@@ -160,7 +160,7 @@ static float *transform_to_cartesian (float *target, float *position)
// a*z^2 + b*z + c = 0
float a = a1 * a1 + a2 * a2 + dnm * dnm;
float b = 2.0f * (a1 * b1 + a2 * (b2 - y1 * dnm) - z1 * dnm * dnm);
float c = (b2 - y1 * dnm) * (b2 - y1 * dnm) + b1 * b1 + dnm * dnm * (z1 * z1 - machine.cfg.re * machine.cfg.re);
float c = (b2 - y1 * dnm) * (b2 - y1 * dnm) + b1 * b1 + dnm * dnm * (z1 * z1 - machine.re_sqr);
// discriminant
float d = b * b - 4.0f * a * c;
@@ -235,7 +235,7 @@ static float *delta_segment_line (float *target, float *position, plan_line_data
distance = sqrtf(delta.x * delta.x + delta.y * delta.y + delta.z * delta.z);
if((segmented = distance > machine.cfg.sl && !(delta.x == 0.0f && delta.y == 0.0f && delta.z == 0.0f))) {
if((segmented = distance > machine.cfg.sl)) {
idx = N_AXIS;
iterations = (uint_fast16_t)ceilf(distance / machine.cfg.sl);
@@ -272,17 +272,17 @@ static float *delta_segment_line (float *target, float *position, plan_line_data
memcpy(&segment_target, &final_target, sizeof(coord_data_t));
if(delta_calcInverse(&segment_target, mpos.values) != 0) {
memcpy(&mpos, &last_pos, sizeof(coord_data_t));
memcpy(&mpos, &machine.last_pos, sizeof(coord_data_t));
iterations = 0;
}
if(!pl_data->condition.rapid_motion && distance != 0.0f) {
float rate_multiplier = get_distance(mpos.values, last_pos.values) / distance;
float rate_multiplier = get_distance(mpos.values, machine.last_pos.values) / distance;
pl_data->feed_rate *= rate_multiplier;
pl_data->rate_multiplier = 1.0 / rate_multiplier;
}
memcpy(&last_pos, &mpos, sizeof(coord_data_t));
memcpy(&machine.last_pos, &mpos, sizeof(coord_data_t));
}
return iterations == 0 || jog_cancel ? NULL : mpos.values;
@@ -290,9 +290,8 @@ static float *delta_segment_line (float *target, float *position, plan_line_data
static void get_cuboid_envelope (void)
{
float maxx = -machine.cfg.e - machine.cfg.f - machine.cfg.re - machine.cfg.rf;
float maxz = maxx;
float minz = -maxx;
float maxz = -machine.cfg.e - machine.cfg.f - machine.cfg.re - machine.cfg.rf;
float minz = -maxz;
float sr = 1.0f / settings.axis[X_AXIS].steps_per_mm; // Steps/rev -> rad/step, XYZ motors should have the same setting!
float pos[N_AXIS];
uint32_t z;
@@ -302,7 +301,6 @@ static void get_cuboid_envelope (void)
.z = machine.cfg.home_angle
};
struct {
int32_t res;
float pos[N_AXIS];
} r[8];
@@ -322,6 +320,11 @@ static void get_cuboid_envelope (void)
}
maxz = machine.home_z;
if(machine.cfg.max_angle != 0.0f) {
home.x = home.y = home.z = machine.cfg.max_angle;
transform_to_cartesian(mpos.values, home.values);
minz = mpos.z;
}
float btf = -machine.cfg.b;
if(minz < btf)
@@ -329,6 +332,7 @@ static void get_cuboid_envelope (void)
if(maxz < btf)
maxz = btf;
bool ok;
float middlez = (maxz + minz) * 0.5f;
float original_dist = (maxz - middlez);
float dist = original_dist * 0.5f;
@@ -339,35 +343,46 @@ static void get_cuboid_envelope (void)
do {
sum += dist;
mpos.x = sum;
mpos.y = sum;
mpos.z = middlez + sum;
r[0].res = delta_calcInverse(&mpos, r[0].pos);
if((ok = delta_calcInverse(&mpos, r[0].pos)) == 0) {
mpos.y = -sum;
ok = delta_calcInverse(&mpos, r[1].pos) == 0;
}
mpos.y = -sum;
r[1].res = delta_calcInverse(&mpos, r[1].pos);
if(ok) {
mpos.x = -sum;
ok = delta_calcInverse(&mpos, r[2].pos) == 0;
}
mpos.x = -sum;
r[2].res = delta_calcInverse(&mpos, r[2].pos);
if(ok) {
mpos.y = sum;
ok = delta_calcInverse(&mpos, r[3].pos) == 0;
}
mpos.y = sum;
r[3].res = delta_calcInverse(&mpos, r[3].pos);
if(ok) {
mpos.x = sum;
mpos.z = middlez - sum;
ok = delta_calcInverse(&mpos, r[4].pos) == 0;
}
mpos.x = sum;
mpos.z = middlez - sum;
r[4].res = delta_calcInverse(&mpos, r[4].pos);
if(ok) {
mpos.y = -sum;
ok = delta_calcInverse(&mpos, r[5].pos) == 0;
}
mpos.y = -sum;
r[5].res = delta_calcInverse(&mpos, r[5].pos);
if(ok) {
mpos.x = -sum;
ok = delta_calcInverse(&mpos, r[6].pos) == 0;
}
mpos.x = -sum;
r[6].res = delta_calcInverse(&mpos, r[6].pos);
if(ok) {
mpos.y = sum;
ok = delta_calcInverse(&mpos, r[7].pos) == 0;
}
mpos.y = sum;
r[7].res = delta_calcInverse(&mpos, r[7].pos);
if(r[0].res || r[1].res || r[2].res || r[3].res || r[4].res || r[5].res || r[6].res || r[7].res) {
if(!ok) {
sum -= dist;
dist *= 0.5f;
} else {
@@ -389,12 +404,21 @@ static void get_cuboid_envelope (void)
}
} while(original_dist > sum && dist > 0.1f);
sys.work_envelope.min[X_AXIS] = -sum;
sys.work_envelope.min[Y_AXIS] = -sum;
sys.work_envelope.min[Z_AXIS] = middlez - sum;
sys.work_envelope.max[X_AXIS] = sum;
sys.work_envelope.max[Y_AXIS] = sum;
sys.work_envelope.max[Z_AXIS] = middlez + sum;
sys.work_envelope.min.x = -sum;
sys.work_envelope.min.y = -sum;
sys.work_envelope.min.z = middlez - sum;
sys.work_envelope.max.x = sum;
sys.work_envelope.max.y = sum;
sys.work_envelope.max.z = middlez + sum - settings.homing.pulloff;
home.x = home.y = 0;
home.z = sys.work_envelope.max.z;
if(delta_calcInverse(&home, pos) == 0)
machine.cfg.home_angle = pos[A_MOTOR];
machine.min_angle[A_MOTOR] = machine.min_angle[B_MOTOR] = machine.min_angle[C_MOTOR] = machine.cfg.home_angle;
if(machine.cfg.max_angle != 0.0f && machine.cfg.max_angle < machine.max_angle[A_MOTOR])
machine.max_angle[A_MOTOR] = machine.max_angle[B_MOTOR] = machine.max_angle[C_MOTOR] = machine.cfg.max_angle;
// resolution
pos[0] = pos[1] = pos[2] = 0.0f;
@@ -505,8 +529,8 @@ static void delta_homing_complete (bool success)
sys.home_position[0] = .0f;
sys.home_position[1] = .0f;
sys.home_position[2] = machine.cfg.flags.home_to_cuboid_top
? sys.work_envelope.max[Z_AXIS]
: machine.home_z; // + pulloff
? sys.work_envelope.max.z
: machine.home_z - settings.homing.pulloff;
if(machine.cfg.flags.home_to_cuboid_top)
protocol_enqueue_rt_command(delta_go_home);
@@ -530,12 +554,19 @@ static bool is_target_inside_cuboid (float *target, bool is_cartesian)
if(is_cartesian && sys.homed.mask) do {
idx--;
if(bit_istrue(sys.homed.mask, bit(idx)) && settings.axis[idx].max_travel < -0.0f)
failed = target[idx] < sys.work_envelope.min[idx] || target[idx] > sys.work_envelope.max[idx];
failed = target[idx] < sys.work_envelope.min.values[idx] || target[idx] > sys.work_envelope.max.values[idx];
} while(!failed && idx);
return is_cartesian && !failed;
}
static inline bool pos_ok (coord_data_t *pos)
{
return !(pos->x < machine.min_angle[A_MOTOR] || pos->x > machine.max_angle[A_MOTOR] ||
pos->y < machine.min_angle[B_MOTOR] || pos->y > machine.max_angle[B_MOTOR] ||
pos->z < machine.min_angle[C_MOTOR] || pos->z > machine.max_angle[B_MOTOR]);
}
// Checks and reports if target array exceeds machine travel limits. Returns false if check failed.
static bool delta_check_travel_limits (float *target, bool is_cartesian)
{
@@ -554,7 +585,7 @@ static bool delta_check_travel_limits (float *target, bool is_cartesian)
if(machine.cfg.flags.limit_to_cuboid)
return false;
if(is_cartesian && delta_calcInverse((coord_data_t *)target, pos.values))
if(is_cartesian && (delta_calcInverse((coord_data_t *)target, pos.values) || !pos_ok(&pos)))
return false;
if(!is_cartesian)
@@ -564,7 +595,7 @@ static bool delta_check_travel_limits (float *target, bool is_cartesian)
idx--;
if(bit_istrue(sys.homed.mask, bit(idx)) && settings.axis[idx].max_travel < -0.0f) {
if(idx > Z_AXIS)
failed = target[idx] < sys.work_envelope.min[idx] || target[idx] > sys.work_envelope.max[idx];
failed = target[idx] < sys.work_envelope.min.values[idx] || target[idx] > sys.work_envelope.max.values[idx];
else
failed = pos.values[idx] < machine.min_angle[idx] || pos.values[idx] > machine.max_angle[idx];
}
@@ -573,28 +604,65 @@ static bool delta_check_travel_limits (float *target, bool is_cartesian)
return !failed;
}
static bool delta_apply_jog_limits (float *target, bool is_cartesian)
static void delta_apply_jog_limits (float *target, float *position)
{
uint_fast8_t idx = N_AXIS;
if(sys.homed.mask == 0)
return;
if(machine.cfg.flags.limit_to_cuboid) {
if(sys.homed.mask) do {
idx--;
if(bit_istrue(sys.homed.mask, bit(idx)) && settings.axis[idx].max_travel < -0.0f)
target[idx] = max(min(target[idx], sys.work_envelope.max[idx]), sys.work_envelope.min[idx]);
} while(idx);
if(machine.cfg.flags.limit_to_cuboid)
apply_jog_limits(target, position);
return true;
} else if(is_target_inside_cuboid(target, is_cartesian))
return true;
else if(!is_target_inside_cuboid(target, true)) {
coord_data_t mpos;
coord_data_t pos;
if(delta_calcInverse((coord_data_t *)target, mpos.values)) {
// find and apply limits...
// if(delta_calcInverse((coord_data_t *)target, pos.values)) {
bool ok = false;
uint_fast8_t idx = N_AXIS;
coord_data_t delta;
do {
idx--;
delta.values[idx] = target[idx] - position[idx];
} while(idx);
float dist, length = sqrtf(delta.x * delta.x + delta.y * delta.y + delta.z * delta.z);
delta.x /= length;
delta.y /= length;
delta.z /= length;
length = length * 0.5f;
dist = 0.0;
do {
if(ok)
length += dist;
else
length -= dist;
target[X_AXIS] = position[X_AXIS] + delta.x * length;
target[Y_AXIS] = position[Y_AXIS] + delta.y * length;
target[Z_AXIS] = position[Z_AXIS] + delta.z * length;
ok = delta_calcInverse((coord_data_t *)target, pos.values) == 0 && pos_ok(&pos);
if(dist > machine.resolution)
dist *= 0.5f;
else if(!ok)
dist += 0.1f;
else
break;
} while(!ok || dist > machine.resolution);
if(!ok)
memcpy(target, position, sizeof(coord_data_t));
hal.stream.write("");
// }
}
return true;
}
static const char *delta_set_axis_setting_unit (setting_id_t setting_id, uint_fast8_t axis_idx)
@@ -640,6 +708,7 @@ static const setting_detail_t kinematics_settings[] = {
{ Setting_Kinematics5, Group_Kinematics, "Base to floor", "mm", Format_Decimal, "###0.000", NULL, NULL, Setting_NonCore, &delta_settings.b, NULL, NULL },
{ Setting_Kinematics6, Group_Kinematics, "Home position", "deg", Format_Decimal, "-#0.000", "-90", "90", Setting_NonCore, &delta_settings.home_angle, NULL, NULL },
{ Setting_Kinematics7, Group_Kinematics, "Flags...", NULL, Format_Bitfield, "Home to cuboid top,Limit to cuboid", NULL, NULL, Setting_NonCore, &delta_settings.flags, NULL, NULL },
{ Setting_Kinematics8, Group_Kinematics, "Max angle", "deg", Format_Decimal, "-#0.000", "-90", "135", Setting_NonCore, &delta_settings.max_angle, NULL, NULL },
};
#ifndef NO_SETTINGS_DESCRIPTIONS
@@ -649,7 +718,9 @@ static const setting_descr_t kinematics_settings_descr[] = {
{ Setting_Kinematics1, "Forearm length is the length of the connecting rod in millimeters." },
{ Setting_Kinematics2, "Bicep length is the length of the driven arm in millimeters." },
{ Setting_Kinematics3, "Base radius is the radius in millimeters from the center of the drive platform to the bicep drive axle." },
{ Setting_Kinematics4, "End effector radius is the distance from the center of the end effector to the connection points of the main connecting rods." }
{ Setting_Kinematics4, "End effector radius is the distance from the center of the end effector to the connection points of the main connecting rods." },
{ Setting_Kinematics6, "Home position in degrees from biceps paralell to the floor." },
{ Setting_Kinematics8, "Max travel in degrees from biceps paralell to the floor. Set to 0 to use calculated max." }
};
#endif
@@ -659,6 +730,15 @@ static void delta_settings_changed (settings_t *settings, settings_changed_flags
memcpy(&machine.cfg, &delta_settings, sizeof(delta_settings_t));
machine.cfg.home_angle = delta_settings.home_angle * RADDEG;
machine.cfg.max_angle = delta_settings.max_angle * RADDEG;
machine.t = (machine.cfg.f - machine.cfg.e) * TAN30_2;
machine.y1 = -0.5f * TAN30 * machine.cfg.f; // f/2 * tg 30
machine.y1_sqr = machine.y1 * machine.y1;
machine.fe = 0.5f * TAN30 * machine.cfg.e; // shift center to edge
machine.rf_sqr = machine.cfg.rf * machine.cfg.rf;
machine.re_sqr = machine.cfg.re * machine.cfg.re;
get_cuboid_envelope();
}
@@ -720,17 +800,17 @@ static status_code_t delta_info (sys_state_t state, char *args)
{
hal.stream.write("Delta robot:" ASCII_EOL);
hal.stream.write(" Rectangular cuboid envelope:" ASCII_EOL " X: ");
hal.stream.write(ftoa(sys.work_envelope.min[X_AXIS], 3));
hal.stream.write(ftoa(sys.work_envelope.min.x, 3));
hal.stream.write(" to ");
hal.stream.write(ftoa(sys.work_envelope.max[X_AXIS], 3));
hal.stream.write(ftoa(sys.work_envelope.max.x, 3));
hal.stream.write(" mm" ASCII_EOL " Y: ");
hal.stream.write(ftoa(sys.work_envelope.min[Y_AXIS], 3));
hal.stream.write(ftoa(sys.work_envelope.min.y, 3));
hal.stream.write(" to ");
hal.stream.write(ftoa(sys.work_envelope.max[Y_AXIS], 3));
hal.stream.write(ftoa(sys.work_envelope.max.y, 3));
hal.stream.write(" mm" ASCII_EOL " Z: ");
hal.stream.write(ftoa(sys.work_envelope.min[Z_AXIS], 3));
hal.stream.write(ftoa(sys.work_envelope.min.z, 3));
hal.stream.write(" to ");
hal.stream.write(ftoa(sys.work_envelope.max[Z_AXIS], 3));
hal.stream.write(ftoa(sys.work_envelope.max.z, 3));
hal.stream.write(" mm" ASCII_EOL " Mid Z:" ASCII_EOL " ");
hal.stream.write(ftoa(machine.envelope_midz, 3));
hal.stream.write(" mm" ASCII_EOL " Resolution:" ASCII_EOL " ");
@@ -783,6 +863,7 @@ void delta_robot_init (void)
kinematics.homing_cycle_get_feedrate = homing_cycle_get_feedrate;
grbl.on_jog_cancel = cancel_jog;
apply_jog_limits = grbl.apply_jog_limits;
grbl.apply_jog_limits = delta_apply_jog_limits;
grbl.check_travel_limits = delta_check_travel_limits;

105
machine_limits.c
View File

@@ -38,18 +38,6 @@
#include "config.h"
// This is the Limit Pin Change Interrupt, which handles the hard limit feature. A bouncing
// limit switch can cause a lot of problems, like false readings and multiple interrupt calls.
// If a switch is triggered at all, something bad has happened and treat it as such, regardless
// if a limit switch is being disengaged. It's impossible to reliably tell the state of a
// bouncing pin because the microcontroller does not retain any state information when
// detecting a pin change. If we poll the pins in the ISR, you can miss the correct reading if the
// switch is bouncing.
// NOTE: Do not attach an e-stop to the limit pins, because this interrupt is disabled during
// homing cycles and will not respond correctly. Upon user request or need, there may be a
// special pinout for an e-stop, but it is generally recommended to just directly connect
// your e-stop switch to the microcontroller reset pin, since it is the most correct way to do this.
// Merge (bitwise or) all limit switch inputs.
ISR_CODE axes_signals_t ISR_FUNC(limit_signals_merge)(limit_signals_t signals)
{
@@ -84,6 +72,13 @@ ISR_CODE static axes_signals_t ISR_FUNC(homing_signals_select)(home_signals_t si
return state;
}
// This is the Limit Pin Change Interrupt, which handles the hard limit feature. A bouncing
// limit switch can cause a lot of problems, like false readings and multiple interrupt calls.
// If a switch is triggered at all, something bad has happened and treat it as such, regardless
// if a limit switch is being disengaged. It's impossible to reliably tell the state of a
// bouncing pin because the microcontroller does not retain any state information when
// detecting a pin change. If we poll the pins in the ISR, you can miss the correct reading if the
// switch is bouncing.
ISR_CODE void ISR_FUNC(limit_interrupt_handler)(limit_signals_t state) // DEFAULT: Limit pin change interrupt process.
{
// Ignore limit switches if already in an alarm state or in-process of executing an alarm.
@@ -122,18 +117,18 @@ void limits_set_work_envelope (void)
if(settings.homing.flags.force_set_origin) {
if(bit_isfalse(settings.homing.dir_mask.value, bit(idx))) {
sys.work_envelope.min[idx] = settings.axis[idx].max_travel + pulloff;
sys.work_envelope.max[idx] = 0.0f;
sys.work_envelope.min.values[idx] = settings.axis[idx].max_travel + pulloff;
sys.work_envelope.max.values[idx] = 0.0f;
} else {
sys.work_envelope.min[idx] = 0.0f;
sys.work_envelope.max[idx] = - (settings.axis[idx].max_travel + pulloff);
sys.work_envelope.min.values[idx] = 0.0f;
sys.work_envelope.max.values[idx] = - (settings.axis[idx].max_travel + pulloff);
}
} else {
sys.work_envelope.min[idx] = settings.axis[idx].max_travel + pulloff;
sys.work_envelope.max[idx] = - pulloff;
sys.work_envelope.min.values[idx] = settings.axis[idx].max_travel + pulloff;
sys.work_envelope.max.values[idx] = - pulloff;
}
} else
sys.work_envelope.min[idx] = sys.work_envelope.max[idx] = 0.0f;
sys.work_envelope.min.values[idx] = sys.work_envelope.max.values[idx] = 0.0f;
} while(idx);
}
@@ -607,24 +602,80 @@ static bool check_travel_limits (float *target, bool is_cartesian)
if(is_cartesian && sys.homed.mask) do {
idx--;
if(bit_istrue(sys.homed.mask, bit(idx)) && settings.axis[idx].max_travel < -0.0f)
failed = target[idx] < sys.work_envelope.min[idx] || target[idx] > sys.work_envelope.max[idx];
failed = target[idx] < sys.work_envelope.min.values[idx] || target[idx] > sys.work_envelope.max.values[idx];
} while(!failed && idx);
return is_cartesian && !failed;
}
// Limits jog commands to be within machine limits, homed axes only.
static bool apply_jog_limits (float *target, bool is_cartesian)
// Derived from code by Dimitrios Matthes & Vasileios Drakopoulos
// https://www.mdpi.com/1999-4893/16/4/201
static void clip_3d_target (coord_data_t *position, coord_data_t *target, work_envelope_t *envelope)
{
uint_fast8_t idx = N_AXIS;
float a = target->x - position->x;
float b = target->y - position->y;
float c = target->z - position->z;
if(sys.homed.mask) do {
if(target->x < envelope->min.x) {
target->y = b / a * (envelope->min.x - position->x) + position->y;
target->z = c / a * (envelope->min.x - position->x) + position->z;
target->x = envelope->min.x;
} else if(target->x > envelope->max.x) {
target->y = b / a * (envelope->max.x - position->x) + position->y;
target->z = c / a * (envelope->max.x - position->x) + position->z;
target->x = envelope->max.x;
}
if(target->y < envelope->min.y) {
target->x = a / b * (envelope->min.y - position->y) + position->x;
target->z = c / b * (envelope->min.y - position->y) + position->z;
target->y = envelope->min.y;
} else if(target->y > envelope->max.y) {
target->x = a / b * (envelope->max.y - position->y) + position->x;
target->z = c / b * (envelope->max.y - position->y) + position->z;
target->y = envelope->max.y;
}
if(target->z < envelope->min.z) {
target->x = a / c * (envelope->min.z - position->z) + position->x;
target->y = b / c * (envelope->min.z - position->z) + position->y;
target->z = envelope->min.z;
} else if(target->z > envelope->max.z) {
target->x = a / c * (envelope->max.z - position->z) + position->x;
target->y = b / c * (envelope->max.z - position->z) + position->y;
target->z = envelope->max.z;
}
}
// Limits jog commands to be within machine limits, homed axes only.
static void apply_jog_limits (float *target, float *position)
{
if(sys.homed.mask == 0)
return;
uint_fast8_t idx;
if((sys.homed.mask & 0b111) == 0b111) {
uint_fast8_t n_axes = 0;
idx = Z_AXIS + 1;
do {
idx--;
if(fabs(target[idx] - position[idx]) > 0.001f)
n_axes++;
} while(idx && n_axes < 2);
if(n_axes > 1)
clip_3d_target((coord_data_t *)position, (coord_data_t *)target, &sys.work_envelope);
}
idx = N_AXIS;
do {
idx--;
if(bit_istrue(sys.homed.mask, bit(idx)) && settings.axis[idx].max_travel < -0.0f)
target[idx] = max(min(target[idx], sys.work_envelope.max[idx]), sys.work_envelope.min[idx]);
target[idx] = max(min(target[idx], sys.work_envelope.max.values[idx]), sys.work_envelope.min.values[idx]);
} while(idx);
return is_cartesian;
}
void limits_init (void)

7
motion_control.c
View File

@@ -87,7 +87,7 @@ bool mc_line (float *target, plan_line_data_t *pl_data)
// If enabled, check for soft limit violations. Placed here all line motions are picked up
// from everywhere in Grbl.
// NOTE: Block jog motions. Jogging is a special case and soft limits are handled independently.
if (!pl_data->condition.jog_motion && settings.limits.flags.soft_enabled)
if (!pl_data->condition.target_validated && settings.limits.flags.soft_enabled)
limits_soft_check(target);
// If in check gcode mode, prevent motion by blocking planner. Soft limits still work.
@@ -756,17 +756,18 @@ void mc_thread (plan_line_data_t *pl_data, float *position, gc_thread_data *thre
}
// Sets up valid jog motion received from g-code parser, checks for soft-limits, and executes the jog.
status_code_t mc_jog_execute (plan_line_data_t *pl_data, parser_block_t *gc_block)
status_code_t mc_jog_execute (plan_line_data_t *pl_data, parser_block_t *gc_block, float *position)
{
// Initialize planner data struct for jogging motions.
// NOTE: Spindle and coolant are allowed to fully function with overrides during a jog.
pl_data->feed_rate = gc_block->values.f;
pl_data->condition.no_feed_override = On;
pl_data->condition.jog_motion = On;
pl_data->condition.target_validated = On;
pl_data->line_number = gc_block->values.n;
if(settings.limits.flags.jog_soft_limited)
grbl.apply_jog_limits(gc_block->values.xyz, true);
grbl.apply_jog_limits(gc_block->values.xyz, position);
else if (settings.limits.flags.soft_enabled && !grbl.check_travel_limits(gc_block->values.xyz, true))
return Status_TravelExceeded;

2
motion_control.h
View File

@@ -52,7 +52,7 @@ void mc_canned_drill (motion_mode_t motion, float *target, plan_line_data_t *pl_
void mc_thread (plan_line_data_t *pl_data, float *position, gc_thread_data *thread, bool feed_hold_disabled);
// Sets up valid jog motion received from g-code parser, checks for soft-limits, and executes the jog.
status_code_t mc_jog_execute(plan_line_data_t *pl_data, parser_block_t *gc_block);
status_code_t mc_jog_execute(plan_line_data_t *pl_data, parser_block_t *gc_block, float *position);
// Dwell for a specific number of seconds
void mc_dwell(float seconds);

3
planner.h
View File

@@ -35,7 +35,8 @@ typedef union {
inverse_time :1,
is_rpm_rate_adjusted :1,
is_laser_ppi_mode :1,
unassigned :8;
target_validated :1,
unassigned :7;
coolant_state_t coolant;
};
} planner_cond_t;

4
system.h
View File

@@ -258,8 +258,8 @@ typedef struct {
} signal_event_t;
typedef struct {
float min[N_AXIS];
float max[N_AXIS];
coord_data_t min;
coord_data_t max;
} work_envelope_t;
//! Global system variables struct.