diff --git a/README.md b/README.md
index a18bc6a..f7f34f1 100644
--- a/README.md
+++ b/README.md
@@ -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
diff --git a/changelog.md b/changelog.md
index b1c55ab..8fdac51 100644
--- a/changelog.md
+++ b/changelog.md
@@ -1,5 +1,23 @@
## grblHAL changelog
+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.
+
+---
+
Build 20230903
Core:
diff --git a/core_handlers.h b/core_handlers.h
index 41a80b0..8082f37 100644
--- a/core_handlers.h
+++ b/core_handlers.h
@@ -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;
diff --git a/gcode.c b/gcode.c
index b76825e..32624d2 100644
--- a/gcode.c
+++ b/gcode.c
@@ -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;
diff --git a/grbl.h b/grbl.h
index 79cc82a..f9e26af 100644
--- a/grbl.h
+++ b/grbl.h
@@ -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"
diff --git a/kinematics/delta.c b/kinematics/delta.c
index df8e5c3..61ccfa5 100644
--- a/kinematics/delta.c
+++ b/kinematics/delta.c
@@ -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;
diff --git a/machine_limits.c b/machine_limits.c
index b7f6803..074fe79 100644
--- a/machine_limits.c
+++ b/machine_limits.c
@@ -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)
diff --git a/motion_control.c b/motion_control.c
index c9d60b0..67de681 100644
--- a/motion_control.c
+++ b/motion_control.c
@@ -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;
diff --git a/motion_control.h b/motion_control.h
index aed5483..d6f5608 100644
--- a/motion_control.h
+++ b/motion_control.h
@@ -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);
diff --git a/planner.h b/planner.h
index 522a92f..aede5bc 100644
--- a/planner.h
+++ b/planner.h
@@ -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;
diff --git a/system.h b/system.h
index a6000e1..c4624bd 100644
--- a/system.h
+++ b/system.h
@@ -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.