diff --git a/changelog.md b/changelog.md index 87a583a..d8a581f 100644 --- a/changelog.md +++ b/changelog.md @@ -1,10 +1,30 @@ ## grblHAL changelog +Build 20230913 + +Core: + +* Added faster soft limits check for arcs when work envelope is a cuboid, fixed some related obscure bugs. New/modified core entry points for soft limit checking. + +* More delta robot tuning, added setting warning in `$DELTA` output if some settings are inconsistent such as steps/rad, acceleration etc. + +* Fix for issue [#361](https://github.com/grblHAL/core/issues/361), `HOME` status is not reported or reported late when auto reporting is enabled with setting `$481`. + +Drivers: + +* iMXRT1062: fix for MCP3221 ADC regression, [issue #68](https://github.com/grblHAL/iMXRT1062/issues/68#). + +* STM32F4xx: SPI pin definition typo, [issue #139](https://github.com/grblHAL/STM32F4xx/issues/139). + +* RP2040: workaround for [issue #74](https://github.com/grblHAL/RP2040/issues/74), odd TMC driver addressing. + +* Remaining drivers updated for [improved handling of limit inputs](#20230903): + Build 20230907 Core: -* Delta robot kinematics tuning: soft limits checks, extended $DELTA command++. Still work in progress \(getting closer to working version\). +* Delta robot kinematics tuning: soft limits checks, extended `$DELTA` command++. Still work in progress \(getting closer to working version\). Plugins: diff --git a/core_handlers.h b/core_handlers.h index 8082f37..20c2cff 100644 --- a/core_handlers.h +++ b/core_handlers.h @@ -78,7 +78,9 @@ 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 bool (*travel_limits_ptr)(float *target, axes_signals_t axes, bool is_cartesian); +typedef bool (*arc_limits_ptr)(coord_data_t *target, coord_data_t *position, point_2d_t center, float radius, plane_t plane, int32_t turns); + typedef void (*jog_limits_ptr)(float *target, float *position); typedef bool (*home_machine_ptr)(axes_signals_t cycle, axes_signals_t auto_square); @@ -175,6 +177,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; + arc_limits_ptr check_arc_travel_limits; jog_limits_ptr apply_jog_limits; enqueue_gcode_ptr enqueue_gcode; enqueue_realtime_command_ptr enqueue_realtime_command; diff --git a/gcode.c b/gcode.c index 32624d2..76a70f7 100644 --- a/gcode.c +++ b/gcode.c @@ -2634,7 +2634,7 @@ status_code_t gc_execute_block (char *block) float h_x2_div_d = 4.0f * gc_block.values.r * gc_block.values.r - x * x - y * y; if (h_x2_div_d < 0.0f) - FAIL(Status_GcodeArcRadiusError); // [Arc radius error] + FAIL(Status_GcodeArcRadiusError); // [Arc radius error] TODO: this will fail due to limited float precision... // Finish computing h_x2_div_d. h_x2_div_d = -sqrtf(h_x2_div_d) / hypot_f(x, y); // == -(h * 2 / d) @@ -2845,6 +2845,7 @@ status_code_t gc_execute_block (char *block) // Initialize planner data struct for motion blocks. plan_line_data_t plan_data; memset(&plan_data, 0, sizeof(plan_line_data_t)); // Zero plan_data struct + plan_data.condition.target_validated = plan_data.condition.target_valid = sys.soft_limits.mask == 0; // Intercept jog commands and complete error checking for valid jog commands and execute. // NOTE: G-code parser state is not updated, except the position to ensure sequential jog @@ -3339,16 +3340,16 @@ status_code_t gc_execute_block (char *block) case MotionMode_CcwArc: // fail if spindle synchronized motion? mc_arc(gc_block.values.xyz, &plan_data, gc_state.position, gc_block.values.ijk, gc_block.values.r, - plane, gc_parser_flags.arc_is_clockwise ? gc_block.arc_turns : - gc_block.arc_turns); + plane, gc_parser_flags.arc_is_clockwise ? -gc_block.arc_turns : gc_block.arc_turns); break; case MotionMode_CubicSpline: { - point_2d cp1 = { + point_2d_t cp1 = { .x = gc_state.position[X_AXIS] + gc_block.values.ijk[X_AXIS], .y = gc_state.position[Y_AXIS] + gc_block.values.ijk[Y_AXIS] }; - point_2d cp2 = { + point_2d_t cp2 = { .x = gc_block.values.xyz[X_AXIS] + gc_state.modal.spline_pq[X_AXIS], .y = gc_block.values.xyz[Y_AXIS] + gc_state.modal.spline_pq[Y_AXIS] }; @@ -3358,11 +3359,11 @@ status_code_t gc_execute_block (char *block) case MotionMode_QuadraticSpline: { - point_2d cp1 = { + point_2d_t cp1 = { .x = gc_state.position[X_AXIS] + (gc_block.values.ijk[X_AXIS] * 2.0f) / 3.0f, .y = gc_state.position[Y_AXIS] + (gc_block.values.ijk[Y_AXIS] * 2.0f) / 3.0f }; - point_2d cp2 = { + point_2d_t cp2 = { .x = gc_block.values.xyz[X_AXIS] + ((gc_state.position[X_AXIS] + gc_block.values.ijk[X_AXIS] - gc_block.values.xyz[X_AXIS]) * 2.0f) / 3.0f, .y = gc_block.values.xyz[Y_AXIS] + ((gc_state.position[Y_AXIS] + gc_block.values.ijk[Y_AXIS] - gc_block.values.xyz[Y_AXIS]) * 2.0f) / 3.0f }; diff --git a/gcode.h b/gcode.h index 4a56654..054b7f2 100644 --- a/gcode.h +++ b/gcode.h @@ -357,10 +357,13 @@ typedef struct { } coord_system_t; //! Axis index to plane assignment. -typedef struct { - uint8_t axis_0; - uint8_t axis_1; - uint8_t axis_linear; +typedef union { + uint8_t axis[3]; + struct { + uint8_t axis_0; + uint8_t axis_1; + uint8_t axis_linear; + }; } plane_t; /*! \brief G- and M-code parameter values diff --git a/grbl.h b/grbl.h index 985ed4e..4fe71ef 100644 --- a/grbl.h +++ b/grbl.h @@ -42,7 +42,7 @@ #else #define GRBL_VERSION "1.1f" #endif -#define GRBL_BUILD 20230907 +#define GRBL_BUILD 20230913 #define GRBL_URL "https://github.com/grblHAL" diff --git a/kinematics/delta.c b/kinematics/delta.c index c0725a3..520f617 100644 --- a/kinematics/delta.c +++ b/kinematics/delta.c @@ -81,6 +81,7 @@ typedef struct { float fe; float rf_sqr; // bicep length ^ 2 float re_sqr; // forearm length ^ 2 + bool settings_warning; delta_settings_t cfg; } delta_properties_t; @@ -95,11 +96,16 @@ static on_set_axis_setting_unit_ptr on_set_axis_setting_unit; static on_setting_get_description_ptr on_setting_get_description; static on_realtime_report_ptr on_realtime_report; static jog_limits_ptr apply_jog_limits; +#if N_AXIS > 3 +static travel_limits_ptr check_travel_limits; +#endif +static arc_limits_ptr check_arc_travel_limits; +static settings_changed_ptr settings_changed; static nvs_address_t nvs_address; // inverse kinematics // helper functions, calculates angle position[X_AXIS] (for YZ-pane) -static int delta_calcAngleYZ (float x0, float y0, float z0, float *theta) +static bool delta_calcAngleYZ (float x0, float y0, float z0, float *theta) { y0 -= machine.fe; // shift center to edge // z = a + b*y @@ -108,29 +114,25 @@ static int delta_calcAngleYZ (float x0, float y0, float z0, float *theta) // discriminant 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 + return false; // non-existing 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 / (machine.y1 - yj)) + ((yj > machine.y1) ? M_PI : 0.0f); - return 0; + return true; // *theta >= machine.cfg.home_angle; //?? } -// inverse kinematics: (x0, y0, z0) -> (position[X_AXIS], position[Y_AXIS], position[Z_AXIS]) -// returned status: 0=OK, -1=non-existing position -static int delta_calcInverse (coord_data_t *mpos, float *position) +// inverse kinematics: cartesian position (pos) -> (target[A_MOTOR], target[B_MOTOR], target[C_MOTOR]) +// returns false for non-existing position +static bool delta_calcInverse (coord_data_t *pos, float *target) { - int status; + target[A_MOTOR] = target[B_MOTOR] = target[C_MOTOR] = 0.0f; - position[A_MOTOR] = position[B_MOTOR] = position[C_MOTOR] = 0.0f; - - if((status = delta_calcAngleYZ(mpos->x, mpos->y, mpos->z, &position[A_MOTOR])) == 0 && - (status = delta_calcAngleYZ(mpos->x * COS120 + mpos->y * SIN120, mpos->y * COS120 - mpos->x * SIN120, mpos->z, &position[B_MOTOR])) == 0) // rotate coords to +120 deg - status = delta_calcAngleYZ(mpos->x * COS120 - mpos->y * SIN120, mpos->y * COS120 + mpos->x * SIN120, mpos->z, &position[C_MOTOR]); // rotate coords to -120 deg - - return status; + return delta_calcAngleYZ(pos->x, pos->y, pos->z, &target[A_MOTOR]) && + delta_calcAngleYZ(pos->x * COS120 + pos->y * SIN120, pos->y * COS120 - pos->x * SIN120, pos->z, &target[B_MOTOR]) && // rotate coords to +120 deg + delta_calcAngleYZ(pos->x * COS120 - pos->y * SIN120, pos->y * COS120 + pos->x * SIN120, pos->z, &target[C_MOTOR]); // rotate coords to -120 deg } // Returns machine position in mm converted from system position. @@ -229,10 +231,12 @@ static float *delta_segment_line (float *target, float *position, plan_line_data jog_cancel = false; memcpy(final_target.values, target, sizeof(final_target)); - if(delta_calcInverse((coord_data_t *)target, mpos.values) == 0) { + if(delta_calcInverse((coord_data_t *)target, mpos.values)) { - if(!pl_data->condition.target_validated && (pl_data->condition.target_validated = settings.limits.flags.soft_enabled)) - pl_data->condition.target_valid = grbl.check_travel_limits(mpos.values, false); + if(!pl_data->condition.target_validated) { + pl_data->condition.target_validated = On; + pl_data->condition.target_valid = grbl.check_travel_limits(mpos.values, sys.soft_limits, false); + } transform_to_cartesian(segment_target.values, position); @@ -261,7 +265,7 @@ static float *delta_segment_line (float *target, float *position, plan_line_data } else { // out of bounds, report? or should never happen? iterations = 1; - pl_data->condition.target_valid = !settings.limits.flags.soft_enabled; + pl_data->condition.target_valid = sys.soft_limits.mask == 0; pl_data->condition.target_validated = On; memcpy(&final_target, position, sizeof(coord_data_t)); } @@ -280,12 +284,10 @@ static float *delta_segment_line (float *target, float *position, plan_line_data } else memcpy(&segment_target, &final_target, sizeof(coord_data_t)); - if(delta_calcInverse(&segment_target, mpos.values) != 0) { + if(!delta_calcInverse(&segment_target, mpos.values)) { memcpy(&mpos, &machine.last_pos, sizeof(coord_data_t)); iterations = 0; - } - - if(!pl_data->condition.rapid_motion && distance != 0.0f) { + } else if(!pl_data->condition.rapid_motion && distance != 0.0f) { 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; @@ -355,40 +357,40 @@ static void get_cuboid_envelope (void) mpos.x = sum; mpos.y = sum; mpos.z = middlez + sum; - if((ok = delta_calcInverse(&mpos, r[0].pos)) == 0) { + if((ok = delta_calcInverse(&mpos, r[0].pos))) { mpos.y = -sum; - ok = delta_calcInverse(&mpos, r[1].pos) == 0; + ok = delta_calcInverse(&mpos, r[1].pos); } if(ok) { mpos.x = -sum; - ok = delta_calcInverse(&mpos, r[2].pos) == 0; + ok = delta_calcInverse(&mpos, r[2].pos); } if(ok) { mpos.y = sum; - ok = delta_calcInverse(&mpos, r[3].pos) == 0; + ok = delta_calcInverse(&mpos, r[3].pos); } if(ok) { mpos.x = sum; mpos.z = middlez - sum; - ok = delta_calcInverse(&mpos, r[4].pos) == 0; + ok = delta_calcInverse(&mpos, r[4].pos); } if(ok) { mpos.y = -sum; - ok = delta_calcInverse(&mpos, r[5].pos) == 0; + ok = delta_calcInverse(&mpos, r[5].pos); } if(ok) { mpos.x = -sum; - ok = delta_calcInverse(&mpos, r[6].pos) == 0; + ok = delta_calcInverse(&mpos, r[6].pos); } if(ok) { mpos.y = sum; - ok = delta_calcInverse(&mpos, r[7].pos) == 0; + ok = delta_calcInverse(&mpos, r[7].pos); } if(!ok) { @@ -428,7 +430,7 @@ static void get_cuboid_envelope (void) home.x = home.y = 0; home.z = sys.work_envelope.max.z; - if(delta_calcInverse(&home, pos) == 0) + if(delta_calcInverse(&home, pos)) machine.home_angle_cuboid = pos[A_MOTOR]; machine.min_angle[A_MOTOR] = machine.min_angle[B_MOTOR] = machine.min_angle[C_MOTOR] = @@ -437,9 +439,9 @@ static void get_cuboid_envelope (void) 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; + pos[A_MOTOR] = pos[B_MOTOR] = pos[C_MOTOR] = 0.0f; transform_to_cartesian(r[0].pos, pos); - pos[0] = sr; + pos[A_MOTOR] = sr; transform_to_cartesian(r[1].pos, pos); float x = r[0].pos[A_MOTOR] - r[1].pos[A_MOTOR]; @@ -575,12 +577,17 @@ static inline bool pos_ok (coord_data_t *pos) } // 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) +static bool delta_check_travel_limits (float *target, axes_signals_t axes, bool is_cartesian) { bool failed = false; uint_fast8_t idx = N_AXIS; coord_data_t pos; +#if N_AXIS > 3 + if((axes.mask & ~0b111) && !check_travel_limits(target, (axes_signals_t){ axes.mask & ~0b111 }, is_cartesian)) + return false; +#endif + if(!is_cartesian) { if(isnanf(transform_to_cartesian(pos.values, target)[A_MOTOR])) return false; @@ -652,7 +659,7 @@ static void delta_apply_jog_limits (float *target, float *position) 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); + ok = delta_calcInverse((coord_data_t *)target, pos.values) && pos_ok(&pos); if(dist > machine.resolution) dist *= 0.5f; @@ -762,14 +769,31 @@ static void delta_settings_changed (settings_t *settings, settings_changed_flags 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.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; + // Force check of all arc segments if work envelope not limited to cuboid. + grbl.check_arc_travel_limits = delta_settings.flags.limit_to_cuboid ? check_arc_travel_limits : NULL; + get_cuboid_envelope(); } +static void core_settings_changed (settings_t *settings, settings_changed_flags_t changed) +{ + if(settings_changed) + settings_changed(settings, changed); + + machine.settings_warning = settings->axis[X_AXIS].steps_per_mm != settings->axis[Y_AXIS].steps_per_mm || + settings->axis[X_AXIS].steps_per_mm != settings->axis[Z_AXIS].steps_per_mm || + settings->axis[X_AXIS].acceleration != settings->axis[Y_AXIS].acceleration || + settings->axis[X_AXIS].acceleration != settings->axis[Z_AXIS].acceleration || + settings->axis[X_AXIS].max_rate != settings->axis[Y_AXIS].max_rate || + settings->axis[X_AXIS].max_rate != settings->axis[Z_AXIS].max_rate || + (settings->homing.flags.enabled && (settings->homing.cycle[0].mask & 0b111) != 0b111); +} + static void delta_settings_save (void) { hal.nvs.memcpy_to_nvs(nvs_address, (uint8_t *)&delta_settings, sizeof(delta_settings_t), true); @@ -820,7 +844,7 @@ static void report_options (bool newopt) on_report_options(newopt); if(!newopt) - hal.stream.write("[KINEMATICS:Delta v0.03]" ASCII_EOL); + hal.stream.write("[KINEMATICS:Delta v0.04]" ASCII_EOL); } static status_code_t delta_info (sys_state_t state, char *args) @@ -834,6 +858,8 @@ static status_code_t delta_info (sys_state_t state, char *args) } while(idx); hal.stream.write("Delta robot:" ASCII_EOL); + if(machine.settings_warning) + hal.stream.write(" WARNING: Review settings!" ASCII_EOL); hal.stream.write(" Rectangular cuboid envelope:" ASCII_EOL " X: "); hal.stream.write(ftoa(sys.work_envelope.min.x, 3)); hal.stream.write(" to "); @@ -905,10 +931,17 @@ 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; + +#if N_AXIS > 3 + check_travel_limits = grbl.check_travel_limits; +#endif grbl.check_travel_limits = delta_check_travel_limits; + check_arc_travel_limits = grbl.check_arc_travel_limits; + on_report_options = grbl.on_report_options; grbl.on_report_options = report_options; @@ -924,6 +957,9 @@ void delta_robot_init (void) on_homing_completed = grbl.on_homing_completed; grbl.on_homing_completed = delta_homing_complete; + settings_changed = hal.settings_changed; + hal.settings_changed = core_settings_changed; + on_setting_get_description = grbl.on_setting_get_description; grbl.on_setting_get_description = delta_setting_get_description; diff --git a/kinematics/polar.c b/kinematics/polar.c index 5eaf800..d7cc8f5 100644 --- a/kinematics/polar.c +++ b/kinematics/polar.c @@ -212,7 +212,6 @@ static void polar_limits_set_target_pos (uint_fast8_t idx) // fn name? } } - // Set machine positions for homed limit switches. Don't update non-homed axes. // NOTE: settings.max_travel[] is stored as a negative value. static void polar_limits_set_machine_positions (axes_signals_t cycle) @@ -229,7 +228,14 @@ static void report_options (bool newopt) on_report_options(newopt); if(!newopt) - hal.stream.write("[KINEMATICS:Polar v0.00]" ASCII_EOL); + hal.stream.write("[KINEMATICS:Polar v0.01]" ASCII_EOL); +} + +static bool polar_homing_cycle (axes_signals_t cycle, axes_signals_t auto_square) +{ + report_message("Homing is not implemented!", Message_Warning); + + return false; } // Initialize API pointers for Wall Plotter kinematics @@ -242,6 +248,7 @@ void polar_init (void) kinematics.transform_steps_to_cartesian = polar_convert_array_steps_to_mpos; kinematics.segment_line = polar_segment_line; + grbl.home_machine = polar_homing_cycle; grbl.on_jog_cancel = cancel_jog; on_report_options = grbl.on_report_options; diff --git a/kinematics/wall_plotter.c b/kinematics/wall_plotter.c index 0ac6830..759907c 100644 --- a/kinematics/wall_plotter.c +++ b/kinematics/wall_plotter.c @@ -293,6 +293,13 @@ static void report_options (bool newopt) hal.stream.write("[KINEMATICS:WallPlotter v2.00]" ASCII_EOL); } +static bool wp_homing_cycle (axes_signals_t cycle, axes_signals_t auto_square) +{ + report_message("Homing is not implemented!", Message_Warning); + + return false; +} + // Initialize API pointers for Wall Plotter kinematics void wall_plotter_init (void) { @@ -316,6 +323,7 @@ void wall_plotter_init (void) kinematics.transform_steps_to_cartesian = wp_convert_array_steps_to_mpos; kinematics.segment_line = wp_segment_line; + grbl.home_machine = wp_homing_cycle; grbl.on_jog_cancel = cancel_jog; on_report_options = grbl.on_report_options; diff --git a/machine_limits.c b/machine_limits.c index d562f3c..0b16f63 100644 --- a/machine_limits.c +++ b/machine_limits.c @@ -545,9 +545,9 @@ status_code_t limits_go_home (axes_signals_t cycle) void limits_soft_check (float *target, planner_cond_t condition) { #ifdef KINEMATICS_API - if(condition.target_validated ? !condition.target_valid : !grbl.check_travel_limits(target, false)) { + if(condition.target_validated ? !condition.target_valid : !grbl.check_travel_limits(target, sys.soft_limits, false)) { #else - if(condition.target_validated ? !condition.target_valid : !grbl.check_travel_limits(target, true)) { + if(condition.target_validated ? !condition.target_valid : !grbl.check_travel_limits(target, sys.soft_limits, true)) { #endif sys.flags.soft_limit = On; @@ -595,20 +595,117 @@ static float get_homing_rate (axes_signals_t cycle, homing_mode_t mode) } // Checks and reports if target array exceeds machine travel limits. Returns false if check failed. -static bool check_travel_limits (float *target, bool is_cartesian) +static bool check_travel_limits (float *target, axes_signals_t axes, bool is_cartesian) { bool failed = false; uint_fast8_t idx = N_AXIS; - if(is_cartesian && sys.homed.mask) do { + if(is_cartesian && (sys.homed.mask & axes.mask)) do { idx--; - if(bit_istrue(sys.homed.mask, bit(idx)) && settings.axis[idx].max_travel < -0.0f) + if(bit_istrue(sys.homed.mask, bit(idx)) && bit_istrue(axes.mask, bit(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; } +// Checks and reports if the arc exceeds machine travel limits. Returns false if check failed. +// NOTE: needs the work envelope to be a cuboid! +static bool check_arc_travel_limits (coord_data_t *target, coord_data_t *position, point_2d_t center, float radius, plane_t plane, int32_t turns) +{ + typedef union { + uint_fast8_t value; + struct { + uint_fast8_t pos_y : 1, + neg_x : 1, + neg_y : 1, + pos_x : 1; + }; + } arc_x_t; + + static const axes_signals_t xyz = { .x = On, .y = On, .z = On }; + + if((sys.soft_limits.mask & xyz.mask) == 0) + return grbl.check_travel_limits(target->values, sys.soft_limits, true); + + arc_x_t x = {0}; + point_2d_t start, end; + + // Set arc start and end points centered at 0,0 and convert CW arcs to CCW. + if(turns > 0) { // CCW + start.x = position->values[plane.axis_0] - center.x; + start.y = position->values[plane.axis_1] - center.y; + end.x = target->values[plane.axis_0] - center.x; + end.y = target->values[plane.axis_1] - center.y; + } else { // CW + start.x = target->values[plane.axis_0] - center.x; + start.y = target->values[plane.axis_1] - center.y; + end.x = position->values[plane.axis_0] - center.x; + end.y = position->values[plane.axis_1] - center.y; + } + + if(labs(turns > 1)) + x.value = 0b1111; // Crosses all + else if(start.y >= 0.0f) { + if(start.x > 0.0f) { // Starts in Q1 + if(end.y >= 0.0f) { + if(end.x <= 0.0f) + x.value = 0b0001; // Ends in Q2 + else if(end.x >= start.x) + x.value = 0b1111; // Ends in Q1, crosses all + } else if(end.x <= 0.0f) + x.value = 0b0011; // Ends in Q3 + else + x.value = 0b0111; // Ends in Q4 + } else { // Starts in Q2 + if(end.y >= 0.0f) { + if(end.x > 0.0f) + x.value = 0b1110; // Ends in Q1 + else if(end.x >= start.x) + x.value = 0b1111; // Ends in Q2, crosses all + } else if(end.x <= 0.0f) + x.value = 0b0010; // Ends in Q3 + else + x.value = 0b0110; // Ends in Q4 + } + } else if(start.x < 0.0f) { // Starts in Q3 + if(end.y < 0.0f) { + if(end.x > 0.0f) + x.value = 0b0100; // Ends in Q4 + else if(end.x <= start.x) + x.value = 0b1111; // Ends in Q3, crosses all + } else if(end.x > 0.0f) + x.value = 0b1100; // Ends in Q1 + else + x.value = 0b1101; // Ends in Q2 + } else { // Starts in Q4 + if(end.y < 0.0f) { + if(end.x < 0.0f) + x.value = 0b1011; // Ends in Q3 + else if(end.x <= start.x) + x.value = 0b1111; // Ends in Q4, crosses all + } else if(end.x > 0.0f) + x.value = 0b1000; // Ends in Q1 + else + x.value = 0b1001; // Ends in Q2 + } + + coord_data_t corner1, corner2; + + memcpy(&corner1, turns > 0 ? position : target, sizeof(coord_data_t)); + corner1.values[plane.axis_0] = x.neg_x ? center.x - radius : min(position->values[plane.axis_0], target->values[plane.axis_0]); + corner1.values[plane.axis_1] = x.neg_y ? center.y - radius : max(position->values[plane.axis_1], target->values[plane.axis_1]); + + if(!grbl.check_travel_limits(corner1.values, sys.soft_limits, true)) + return false; + + memcpy(&corner2, turns > 0 ? target : position, sizeof(coord_data_t)); + corner2.values[plane.axis_0] = x.pos_x ? center.x + radius : max(position->values[plane.axis_0], target->values[plane.axis_0]); + corner2.values[plane.axis_1] = x.pos_y ? center.y + radius : min(position->values[plane.axis_1], target->values[plane.axis_1]); + + return grbl.check_travel_limits(corner2.values, sys.soft_limits, true); +} + // 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) @@ -683,6 +780,7 @@ void limits_init (void) { hal.homing.get_feedrate = get_homing_rate; grbl.check_travel_limits = check_travel_limits; + grbl.check_arc_travel_limits = check_arc_travel_limits; grbl.apply_jog_limits = apply_jog_limits; grbl.home_machine = homing_cycle; } diff --git a/motion_control.c b/motion_control.c index 1b36baf..3449b9d 100644 --- a/motion_control.c +++ b/motion_control.c @@ -85,7 +85,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. - if(settings.limits.flags.soft_enabled) + if(!(pl_data->condition.target_validated && pl_data->condition.target_valid)) limits_soft_check(target, pl_data->condition); // If in check gcode mode, prevent motion by blocking planner. Soft limits still work. @@ -106,7 +106,7 @@ bool mc_line (float *target, plan_line_data_t *pl_data) // parser and planner are separate from the system machine positions, this is doable. #ifdef KINEMATICS_API - while(kinematics.segment_line(target, NULL, pl_data, false)) { + while(kinematics.segment_line(target, NULL, pl_data, false)) { #endif #if ENABLE_BACKLASH_COMPENSATION @@ -190,37 +190,59 @@ bool mc_line (float *target, plan_line_data_t *pl_data) } pl_data->feed_rate = feed_rate; - } + } // while(kinematics.segment_line() + + pl_data->feed_rate = feed_rate; #endif } return !ABORTED; } - // Execute an arc in offset mode format. position == current xyz, target == target xyz, -// offset == offset from current xyz, axis_X defines circle plane in tool space, axis_linear is -// the direction of helical travel, radius == circle radius, isclockwise boolean. Used -// for vector transformation direction. +// offset == offset from current xyz, plane.axis_X defines circle plane in tool space, plane.axis_linear is +// the direction of helical travel, radius == circle radius, turns > 0 for CCW arcs. Used +// for vector transformation direction and number of full turns to add (abs(turns) - 1). // The arc is approximated by generating a huge number of tiny, linear segments. The chordal tolerance // of each segment is configured in settings.arc_tolerance, which is defined to be the maximum normal // distance from segment to the circle when the end points both lie on the circle. void mc_arc (float *target, plan_line_data_t *pl_data, float *position, float *offset, float radius, plane_t plane, int32_t turns) { - double center_axis0 = (double)position[plane.axis_0] + (double)offset[plane.axis_0]; - double center_axis1 = (double)position[plane.axis_1] + (double)offset[plane.axis_1]; - double r_axis0 = -(double)offset[plane.axis_0]; // Radius vector from center to current location - double r_axis1 = -(double)offset[plane.axis_1]; - double rt_axis0 = (double)target[plane.axis_0] - center_axis0; - double rt_axis1 = (double)target[plane.axis_1] - center_axis1; + typedef union { + double values[2]; + struct { + double x; + double y; + }; + } point_2dd_t; + + point_2dd_t rv = { // Radius vector from center to current location + .x = -(double)offset[plane.axis_0], + .y = -(double)offset[plane.axis_1] + }; + point_2dd_t center = { + .x = (double)position[plane.axis_0] - rv.x, + .y = (double)position[plane.axis_1] - rv.y + }; + point_2dd_t rt = { + .x = (double)target[plane.axis_0] - center.x, + .y = (double)target[plane.axis_1] - center.y + }; // CCW angle between position and target from circle center. Only one atan2() trig computation required. - float angular_travel = (float)atan2(r_axis0 * rt_axis1 - r_axis1 * rt_axis0, r_axis0 * rt_axis0 + r_axis1 * rt_axis1); + float angular_travel = (float)atan2(rv.x * rt.y - rv.y * rt.x, rv.x * rt.x + rv.y * rt.y); if (turns > 0) { // Correct atan2 output per direction - if (angular_travel >= -ARC_ANGULAR_TRAVEL_EPSILON) - angular_travel -= 2.0f * M_PI; - } else if (angular_travel <= ARC_ANGULAR_TRAVEL_EPSILON) - angular_travel += 2.0f * M_PI; + if (angular_travel <= ARC_ANGULAR_TRAVEL_EPSILON) + angular_travel += 2.0f * M_PI; + } else if (angular_travel >= -ARC_ANGULAR_TRAVEL_EPSILON) + angular_travel -= 2.0f * M_PI; + + if(!pl_data->condition.target_validated && grbl.check_arc_travel_limits) { + pl_data->condition.target_validated = On; + pl_data->condition.target_valid = grbl.check_arc_travel_limits((coord_data_t *)target, (coord_data_t *)position, + (point_2d_t){ .x = (float)center.x, .y = (float)center.y }, + radius, plane, turns); + } if(labs(turns) > 1) { @@ -265,7 +287,7 @@ void mc_arc (float *target, plan_line_data_t *pl_data, float *position, float *o // (2x) settings.arc_tolerance. For 99% of users, this is just fine. If a different arc segment fit // is desired, i.e. least-squares, midpoint on arc, just change the mm_per_arc_segment calculation. // For the intended uses of Grbl, this value shouldn't exceed 2000 for the strictest of cases. - uint16_t segments = (uint16_t)floorf(fabsf(0.5f * angular_travel * radius) / sqrtf(settings.arc_tolerance * (2.0f * radius - settings.arc_tolerance))); + uint_fast16_t segments = (uint_fast16_t)floorf(fabsf(0.5f * angular_travel * radius) / sqrtf(settings.arc_tolerance * (2.0f * radius - settings.arc_tolerance))); if (segments) { @@ -329,24 +351,24 @@ void mc_arc (float *target, plan_line_data_t *pl_data, float *position, float *o for (i = 1; i < segments; i++) { // Increment (segments-1). if (count < N_ARC_CORRECTION) { - // Apply vector rotation matrix. ~40 usec - r_axisi = r_axis0 * sin_T + r_axis1 * cos_T; - r_axis0 = r_axis0 * cos_T - r_axis1 * sin_T; - r_axis1 = r_axisi; + // Apply vector rotation matrix. + r_axisi = rv.x * sin_T + rv.y * cos_T; + rv.x = rv.x * cos_T - rv.y * sin_T; + rv.y = r_axisi; count++; } else { // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments. // Compute exact location by applying transformation matrix from initial radius vector(=-offset). cos_Ti = cosf(i * theta_per_segment); sin_Ti = sinf(i * theta_per_segment); - r_axis0 = -offset[plane.axis_0] * cos_Ti + offset[plane.axis_1] * sin_Ti; - r_axis1 = -offset[plane.axis_0] * sin_Ti - offset[plane.axis_1] * cos_Ti; + rv.x = -offset[plane.axis_0] * cos_Ti + offset[plane.axis_1] * sin_Ti; + rv.y = -offset[plane.axis_0] * sin_Ti - offset[plane.axis_1] * cos_Ti; count = 0; } // Update arc_target location - position[plane.axis_0] = center_axis0 + r_axis0; - position[plane.axis_1] = center_axis1 + r_axis1; + position[plane.axis_0] = center.x + rv.x; + position[plane.axis_1] = center.y + rv.y; #if N_AXIS > 3 idx = N_AXIS; do { @@ -767,7 +789,7 @@ status_code_t mc_jog_execute (plan_line_data_t *pl_data, parser_block_t *gc_bloc if(settings.limits.flags.jog_soft_limited) 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)) + else if(sys.soft_limits.mask && !grbl.check_travel_limits(gc_block->values.xyz, sys.soft_limits, true)) return Status_TravelExceeded; // Valid jog command. Plan, set state, and execute. @@ -853,10 +875,10 @@ status_code_t mc_homing_cycle (axes_signals_t cycle) } #endif - state_set(STATE_HOMING); // Set homing system state. + state_set(STATE_HOMING); // Set homing system state. #if COMPATIBILITY_LEVEL == 0 - protocol_enqueue_realtime_command(CMD_STATUS_REPORT); // Force a status report and - delay_sec(0.1f, DelayMode_Dwell); // delay a bit to get it sent (or perhaps wait a bit for a request?) + system_set_exec_state_flag(EXEC_STATUS_REPORT); // Force a status report and + delay_sec(0.1f, DelayMode_Dwell); // delay a bit to get it sent (or perhaps wait a bit for a request?) #endif // Turn off spindle and coolant (and update parser state) if(spindle_is_on()) @@ -997,7 +1019,7 @@ gc_probe_t mc_probe_cycle (float *target, plan_line_data_t *pl_data, gc_parser_f do { idx--; - if(position.values[idx] != target[idx]) + if(fabsf(target[idx] - position.values[idx]) > TOLERANCE_EQUAL) bit_true(axes.mask, bit(idx)); } while(idx--); diff --git a/nuts_bolts.h b/nuts_bolts.h index 29f5ebe..868bdb6 100644 --- a/nuts_bolts.h +++ b/nuts_bolts.h @@ -148,7 +148,7 @@ typedef union { float x; float y; }; -} point_2d; +} point_2d_t; #pragma pack(push, 1) diff --git a/planner.c b/planner.c index d8b887b..474aee0 100644 --- a/planner.c +++ b/planner.c @@ -692,6 +692,7 @@ void plan_data_init (plan_line_data_t *plan_data) { memset(plan_data, 0, sizeof(plan_line_data_t)); plan_data->spindle.hal = gc_state.spindle.hal ? gc_state.spindle.hal : spindle_get(0); + plan_data->condition.target_validated = plan_data->condition.target_valid = sys.soft_limits.mask == 0; #ifdef KINEMATICS_API plan_data->rate_multiplier = 1.0; #endif diff --git a/plugins_init.h b/plugins_init.h index 48d3a11..88b858a 100644 --- a/plugins_init.h +++ b/plugins_init.h @@ -78,6 +78,11 @@ lb_clusters_init(); #endif +#if PROBE_PROTECT_ENABLE + extern void probe_protect_init (void); + probe_protect_init(); +#endif + #if WEBUI_ENABLE extern void webui_init (void); webui_init(); diff --git a/settings.c b/settings.c index 5332ca2..762570a 100644 --- a/settings.c +++ b/settings.c @@ -1013,6 +1013,19 @@ static status_code_t set_probe_disable_pullup (setting_id_t id, uint_fast16_t in return Status_OK; } +static void tmp_set_soft_limits (void) +{ + sys.soft_limits.mask = 0; + + if(settings.limits.flags.soft_enabled) { + uint_fast8_t idx = N_AXIS; + do { + if(settings.axis[--idx].max_travel < -0.0f) + bit_true(sys.soft_limits.mask, bit(idx)); + } while(idx); + } +} + static status_code_t set_soft_limits_enable (setting_id_t id, uint_fast16_t int_value) { if(int_value && !settings.homing.flags.enabled) @@ -1020,6 +1033,8 @@ static status_code_t set_soft_limits_enable (setting_id_t id, uint_fast16_t int_ settings.limits.flags.soft_enabled = int_value != 0; + tmp_set_soft_limits(); + return Status_OK; } @@ -1374,6 +1389,7 @@ static status_code_t set_axis_setting (setting_id_t setting, float value) system_raise_alarm(Alarm_HomingRequired); grbl.report.feedback_message(Message_HomingCycleRequired); } + tmp_set_soft_limits(); break; case Setting_AxisBacklash: @@ -2749,6 +2765,8 @@ void settings_init (void) xbar_set_homing_source(); + tmp_set_soft_limits(); + if(spindle_get_count() == 0) spindle_add_null(); diff --git a/settings.h b/settings.h index 57cab55..3a2b1d9 100644 --- a/settings.h +++ b/settings.h @@ -651,8 +651,24 @@ typedef struct { limit_settings_flags_t flags; axes_signals_t invert; axes_signals_t disable_pullup; +// axes_signals_t soft_enabled; // TODO: add per axis soft limits, replace soft_enabled flag } limit_settings_t; +typedef union { + uint8_t value; + uint8_t mask; + struct { + uint8_t g55 :1, + g56 :1, + g57 :1, + g58 :1, + g59 :1, + g59_1 :1, + g59_2 :1, + g59_3 :1; + }; +} offset_lock_t; + typedef union { uint8_t value; uint8_t mask; @@ -718,6 +734,7 @@ typedef struct { safety_door_settings_t safety_door; position_pid_t position; // Used for synchronized motion ioport_signals_t ioport; + // offset_lock_t offset_lock; // TODO: add in next settings version. } settings_t; typedef enum { diff --git a/state_machine.c b/state_machine.c index dd783b5..a6b4dd5 100644 --- a/state_machine.c +++ b/state_machine.c @@ -178,7 +178,7 @@ static bool initiate_hold (uint_fast16_t new_state) park.flags.value = 0; } - sys.suspend = true; + sys.suspend = !sys.flags.soft_limit; pending_state = sys_state == STATE_JOG ? new_state : STATE_IDLE; return sys_state == STATE_CYCLE; @@ -579,8 +579,12 @@ static void state_await_hold (uint_fast16_t rt_exec) } if (!handler_changed) { - sys.holding_state = Hold_Complete; - stateHandler = state_await_resume; + if(sys.flags.soft_limit) + state_set(STATE_IDLE); + else { + sys.holding_state = Hold_Complete; + stateHandler = state_await_resume; + } } } } diff --git a/system.h b/system.h index c4624bd..92453b6 100644 --- a/system.h +++ b/system.h @@ -307,6 +307,7 @@ typedef struct system { bool mpg_mode; //!< To be moved to system_flags_t signal_event_t last_event; //!< Last signal events (control and limits signal). int32_t position[N_AXIS]; //!< Real-time machine (aka home) position vector in steps. + axes_signals_t soft_limits; //!< temporary, will be removed when available in settings. //@} } system_t;