From 35431d1bd39151935a9681354312f0bd23c70add Mon Sep 17 00:00:00 2001 From: Terje Io Date: Sun, 26 Jul 2026 08:21:33 +0200 Subject: [PATCH] Fix for M3 enabled laser turned off on feed hold when $63 option Disable laser during hold is off. Ref. issue #991. Improved handling of some VFS file system actions (unlink, mkdirv and rmdir), added support for truncate. Changed some kinematics API function signatures for readability/debugability. The ioports implementation has been changed to allow for consecutive pin numbers for external (expander based) I/O. See changelog for details. --- README.md | 2 +- changelog.md | 35 +++++++- errors.h | 3 +- grbllib.c | 2 +- ioports.c | 30 +++++-- ioports.h | 2 + kinematics.h | 38 -------- kinematics/asymmetric_ganging.c | 48 +++++++--- kinematics/corexy.c | 76 ++++++++-------- kinematics/delta.c | 154 ++++++++++++++++---------------- kinematics/interface.h | 49 ++++++++++ kinematics/polar.c | 60 ++++++------- kinematics/wall_plotter.c | 93 ++++++++++--------- machine_limits.c | 6 +- motion_control.c | 6 +- nuts_bolts.h | 2 +- spindle_control.c | 7 +- system.c | 4 +- vfs.c | 64 ++++++++++--- vfs.h | 3 + 20 files changed, 407 insertions(+), 277 deletions(-) delete mode 100644 kinematics.h create mode 100644 kinematics/interface.h diff --git a/README.md b/README.md index 078f423..3105624 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ ## grblHAL ## -Latest build date is 20260718, see the [changelog](changelog.md) for details. +Latest build date is 20260726, see the [changelog](changelog.md) for details. > [!NOTE] > A settings reset will be performed on an update of builds prior to 20241208. Backup and restore of settings is recommended. diff --git a/changelog.md b/changelog.md index 2d3fa61..9206c98 100644 --- a/changelog.md +++ b/changelog.md @@ -1,15 +1,46 @@ ## grblHAL changelog +Build 20260726 + +Core: + +* Fix for M3 enabled laser turned off on feed hold when $63 option _Disable laser during hold_ is off. Ref. issue [#991](https://github.com/grblHAL/core/issues/991). + +* Improved handling of some VFS file system actions \(`unlink`, `mkdirv` and `rmdir`\), added support for `truncate`. + +* Changed some kinematics API function signatures for readability/debugability. + +* For developers: the ioports implementation has been changed to allow for consecutive pin numbers for external \(expander based\) I/O +so that external pins can be assigned to a given expander for basic I/O functions. +Plugins should thus declare themself as external before registering and add a pin base, provided by the core, to their pin information output. +In addition they should allow multiple calls to their init function, with only the first acted upon, +to allow board code to sequence registration by implementing the weak function `board_ports_init()`. + +Drivers: + +* iMRXT1062, RP2040 and STM32F4xx: I/O expander code updated for core change. + +Plugins: + +* SDCard, YModem: no longer stores the file name to avoid potential buffer overflow. Files will be truncated to zero length on aborted transfers. + +* SDCard, filing systems: added support for creating and deleting subdirectories via `$FMD` and `$FRD`. +Based on PR [#15](https://github.com/grblHAL/Plugin_SD_card/pull/15) which was not accepted due to the core VFS had to be changed to properly support the new commands. + +* Misc: updated for core changes. + +--- + Build 20260719 Core: -* Updated VFS to correctly handle long working directory paths that would otherwise lead to buffer overflows. Ref. PR#986 which is a partial fix. +* Updated VFS to correctly handle long working directory paths that would otherwise lead to buffer overflows. Ref. PR [#986](https://github.com/grblHAL/core/pull/986) which is a partial fix. > [!NOTE] > The underlying file systems may impose their own limits to path lengths. -* Updated named O calls to allow name lengths only limited by available heap. Ref. PR#989 which likely would return an error on overly long names and would potentially execute incorrect code if not. +* Updated named O calls to allow name lengths only limited by available heap. Ref. PR [#989](https://github.com/grblHAL/core/pull/989) which likely would return an error on overly long names and would potentially execute incorrect code if not. Plugins: diff --git a/errors.h b/errors.h index bbbe9fa..befddc3 100644 --- a/errors.h +++ b/errors.h @@ -121,7 +121,8 @@ typedef enum { Status_AuxiliaryPortUnusable = 86, Status_ToolInSPindle = 87, Status_NoToolInSPindle = 88, - Status_StatusMax = Status_NoToolInSPindle, + Status_FileDeleteFailed = 89, + Status_StatusMax = Status_FileDeleteFailed, Status_UserException = 253, Status_Handled, // For internal use only Status_Unhandled // For internal use only diff --git a/grbllib.c b/grbllib.c index fcaa139..934beb3 100644 --- a/grbllib.c +++ b/grbllib.c @@ -42,7 +42,7 @@ #include "motion_control.h" #endif #ifdef KINEMATICS_API -#include "kinematics.h" +#include "kinematics/interface.h" #endif static void task_execute (sys_state_t state); diff --git a/ioports.c b/ioports.c index 909d9f0..77a4002 100644 --- a/ioports.c +++ b/ioports.c @@ -857,6 +857,19 @@ FLASHMEM static io_ports_list_t *insert_ports (void) return io_ports; } +FLASHMEM static uint8_t get_pin_base (io_port_type_t type, io_port_direction_t dir) +{ + uint8_t pin_base = 0; + io_ports_list_t *io_ports; + + if((io_ports = ports)) do { + if(io_ports->type == type && io_ports->ports_id && io_ports->ports_id->external) + pin_base += io_ports->ports_id->cfg[dir].n_ports; + } while((io_ports = io_ports->next)); + + return pin_base; +} + FLASHMEM static bool claim_hal (void) { io_port_t empty = {}; @@ -1062,13 +1075,17 @@ FLASHMEM bool ioports_add_analog (io_analog_t *analog) if((ports = insert_ports())) { ports->type = Port_Analog; - ports->ports_id = analog->ports; ports->hal.set_pin_description = analog->set_pin_description; ports->hal.get_pin_info = analog->get_pin_info; - if(analog->ports->out.n_ports) + if(analog->ports->out.n_ports) { ports->hal.analog_out = analog->analog_out; - if(analog->ports->in.n_ports) + analog->ports->out.pin_base = get_pin_base(Port_Analog, Port_Output); + } + if(analog->ports->in.n_ports) { ports->hal.wait_on_input = analog->wait_on_input; + analog->ports->out.pin_base = get_pin_base(Port_Analog, Port_Input); + } + ports->ports_id = analog->ports; } } @@ -1091,15 +1108,18 @@ FLASHMEM bool ioports_add_digital (io_digital_t *digital) if((io_ports = insert_ports())) { io_ports->type = Port_Digital; - io_ports->ports_id = digital->ports; io_ports->hal.set_pin_description = digital->set_pin_description; io_ports->hal.get_pin_info = digital->get_pin_info; - if(digital->ports->out.n_ports) + if(digital->ports->out.n_ports) { io_ports->hal.digital_out = digital->digital_out; + digital->ports->out.pin_base = get_pin_base(Port_Digital, Port_Output); + } if(digital->ports->in.n_ports) { io_ports->hal.wait_on_input = digital->wait_on_input; io_ports->hal.register_interrupt_handler = digital->register_interrupt_handler; + digital->ports->in.pin_base = get_pin_base(Port_Digital, Port_Input); } + io_ports->ports_id = digital->ports; } ioports_add_settings(NULL, NULL); diff --git a/ioports.h b/ioports.h index 04216c0..b107c34 100644 --- a/ioports.h +++ b/ioports.h @@ -242,12 +242,14 @@ typedef struct { uint8_t n_ports; uint8_t n_start; uint8_t idx_last; + uint8_t pin_base; #ifdef IOPORTS_KEEP_DEPRECATED uint8_t *map; //!< Deprecated - do not reference in new code! #endif } io_ports_detail_t; typedef struct io_ports_data { + bool external; union { io_ports_detail_t cfg[2]; struct { diff --git a/kinematics.h b/kinematics.h deleted file mode 100644 index d770512..0000000 --- a/kinematics.h +++ /dev/null @@ -1,38 +0,0 @@ -/* - kinematics.h - kinematics interface (API) - - Part of grblHAL - - Copyright (c) 2019-2023 Terje Io - - grblHAL is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - grblHAL is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with grblHAL. If not, see . -*/ - -#ifndef _KINEMATICS_H_ -#define _KINEMATICS_H_ - -typedef struct { - float *(*transform_steps_to_cartesian)(float *position, int32_t *steps); - float *(*transform_from_cartesian) (float *target, float *position); - float *(*segment_line) (float *target, float *position, plan_line_data_t *pl_data, bool init); // target is cartesian, position transformed - uint_fast8_t (*limits_get_axis_mask)(uint_fast8_t idx); - void (*limits_set_target_pos)(uint_fast8_t idx); - void (*limits_set_machine_positions)(axes_signals_t cycle); - bool (*homing_cycle_validate)(axes_signals_t cycle); - float (*homing_cycle_get_feedrate)(axes_signals_t axes, float rate, homing_mode_t mode); -} kinematics_t; - -extern kinematics_t kinematics; - -#endif diff --git a/kinematics/asymmetric_ganging.c b/kinematics/asymmetric_ganging.c index 2734506..734ce81 100644 --- a/kinematics/asymmetric_ganging.c +++ b/kinematics/asymmetric_ganging.c @@ -32,7 +32,7 @@ #include "../hal.h" #include "../settings.h" #include "../planner.h" -#include "../kinematics.h" +#include "interface.h" #ifdef ASYMMETRIC_AUTO_SQUARE #define PRIMARY_AXIS ASYMMETRIC_AUTO_SQUARE @@ -155,23 +155,23 @@ static axes_signals_t onGetGangedAxes (bool auto_squared) #endif // ASYMMETRIC_AUTO_SQUARE -static float *convert_array_steps_to_mpos (float *position, int32_t *steps) +static coord_data_t *convert_array_steps_to_mpos (coord_data_t *position, mpos_t *steps) { uint_fast8_t idx = N_AXIS; do { idx--; - position[idx] = steps[idx] / settings.axis[idx].steps_per_mm; + position->values[idx] = steps->values[idx] / settings.axis[idx].steps_per_mm; } while(idx); return position; } // Transform position from cartesian coordinate system to corexy coordinate system -static inline float *transform_from_cartesian (float *target, float *position) +static inline coord_data_t *transform_from_cartesian (coord_data_t *target, coord_data_t *position) { memcpy(target, position, sizeof(coord_data_t)); - target[GANGED_AXIS] = position[PRIMARY_AXIS]; + target->values[GANGED_AXIS] = position->values[PRIMARY_AXIS]; return target; } @@ -197,17 +197,17 @@ static void set_machine_positions (axes_signals_t cycle) } // called from mc_line() to segment lines if not overridden, default implementation for pass-through -static float *kinematics_segment_line (float *target, float *position, plan_line_data_t *pl_data, bool init) +static coord_data_t *kinematics_segment_line (coord_data_t *target, coord_data_t *position, plan_line_data_t *pl_data, bool init) { static uint_fast8_t iterations; static coord_data_t trsf; if(init) { iterations = 2; - transform_from_cartesian(trsf.values, target); + transform_from_cartesian(&trsf, target); } - return iterations-- == 0 ? NULL : trsf.values; + return iterations-- == 0 ? NULL : &trsf; } static float homing_cycle_get_feedrate (axes_signals_t cycle, float feedrate, homing_mode_t mode) @@ -215,6 +215,12 @@ static float homing_cycle_get_feedrate (axes_signals_t cycle, float feedrate, ho return feedrate; } +static bool validate_steps_mm (float value) +{ + // max diff 1% + return fabs(settings.axis[PRIMARY_AXIS].steps_per_mm - value) <= settings.axis[PRIMARY_AXIS].steps_per_mm / 100.0f; +} + static void onSettingsChanged (settings_t *settings, settings_changed_flags_t changed) { uint_fast8_t idx = sizeof(settings->homing.cycle) / sizeof(axes_signals_t); @@ -225,7 +231,12 @@ static void onSettingsChanged (settings_t *settings, settings_changed_flags_t ch memcpy(&settings->axis[GANGED_AXIS], &settings->axis[PRIMARY_AXIS], sizeof(axis_settings_t)); - settings->axis[GANGED_AXIS].steps_per_mm = steps_per_mm; + if(!changed.restore_defaults) { + if(validate_steps_mm(steps_per_mm)) + settings->axis[GANGED_AXIS].steps_per_mm = steps_per_mm; + else + report_message("Ganged axis step/mm is out of range, reset to default.", Message_Warning); + } do { if(settings->homing.cycle[--idx].bits & PRIMARY_AXIS_BIT) @@ -286,8 +297,23 @@ PROGMEM static const char label[] = { #endif }; +FLASHMEM static status_code_t set_steps_mm (setting_id_t setting, float value) +{ + if(!validate_steps_mm(value)) + return Status_SettingValueOutOfRange; + + settings.axis[GANGED_AXIS].steps_per_mm = value; + + return Status_OK; +} + +FLASHMEM static float get_steps_mm (setting_id_t setting) +{ + return settings.axis[GANGED_AXIS].steps_per_mm; +} + PROGMEM static const setting_detail_t axis_settings[] = { - { Setting_AxisStepsPerMM + GANGED_AXIS, Group_Axis0 + PRIMARY_AXIS, label, "step/mm", Format_Decimal, "#####0.000##", NULL, NULL, Setting_IsLegacy, &settings.axis[GANGED_AXIS].steps_per_mm, NULL, NULL }, + { Setting_AxisStepsPerMM + GANGED_AXIS, Group_Axis0 + PRIMARY_AXIS, label, "step/mm", Format_Decimal, "#####0.000##", NULL, NULL, Setting_IsLegacyFn, set_steps_mm, get_steps_mm, NULL }, }; static void report_options (bool newopt) @@ -295,7 +321,7 @@ static void report_options (bool newopt) on_report_options(newopt); if(!newopt) - hal.stream.write("[KINEMATICS:Asymmetric ganging v0.01]" ASCII_EOL); + hal.stream.write("[KINEMATICS:Asymmetric ganging v0.02]" ASCII_EOL); } // Initialize API pointers for xxx kinematics diff --git a/kinematics/corexy.c b/kinematics/corexy.c index ad54ee2..6d39fc0 100644 --- a/kinematics/corexy.c +++ b/kinematics/corexy.c @@ -3,7 +3,7 @@ Part of grblHAL - Copyright (c) 2019-2024 Terje Io + Copyright (c) 2019-2026 Terje Io Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC grblHAL is free software: you can redistribute it and/or modify @@ -29,7 +29,7 @@ #include "../hal.h" #include "../settings.h" #include "../planner.h" -#include "../kinematics.h" +#include "interface.h" // CoreXY motor assignments. DO NOT ALTER. // NOTE: If the A and B motor axis bindings are changed, this effects the CoreXY equations. @@ -40,54 +40,54 @@ static on_report_options_ptr on_report_options; static travel_limits_ptr check_travel_limits; // Returns x or y-axis "steps" based on CoreXY motor steps. -inline static int32_t corexy_convert_to_a_motor_steps (int32_t *steps) +inline static int32_t corexy_convert_to_a_motor_steps (mpos_t *steps) { - return (steps[A_MOTOR] + steps[B_MOTOR]) >> 1; + return (steps->values[A_MOTOR] + steps->values[B_MOTOR]) >> 1; } -inline static int32_t corexy_convert_to_b_motor_steps (int32_t *steps) +inline static int32_t corexy_convert_to_b_motor_steps (mpos_t *steps) { - return (steps[A_MOTOR] - steps[B_MOTOR]) >> 1; + return (steps->values[A_MOTOR] - steps->values[B_MOTOR]) >> 1; } // Returns machine position of axis 'idx'. Must be sent a 'step' array. -static float *corexy_convert_array_steps_to_mpos (float *position, int32_t *steps) +static coord_data_t *corexy_convert_array_steps_to_mpos (coord_data_t *position, mpos_t *steps) { uint_fast8_t idx; - position[X_AXIS] = corexy_convert_to_a_motor_steps(steps) / settings.axis[X_AXIS].steps_per_mm; - position[Y_AXIS] = corexy_convert_to_b_motor_steps(steps) / settings.axis[Y_AXIS].steps_per_mm; + position->x = corexy_convert_to_a_motor_steps(steps) / settings.axis[X_AXIS].steps_per_mm; + position->y = corexy_convert_to_b_motor_steps(steps) / settings.axis[Y_AXIS].steps_per_mm; for(idx = Z_AXIS; idx < N_AXIS; idx++) - position[idx] = steps[idx] / settings.axis[idx].steps_per_mm; + position->values[idx] = steps->values[idx] / settings.axis[idx].steps_per_mm; return position; } // Transform position from cartesian coordinate system to corexy coordinate system -static inline float *transform_from_cartesian (float *target, float *position) +static inline coord_data_t *transform_from_cartesian (coord_data_t *target, coord_data_t *position) { uint_fast8_t idx; - target[X_AXIS] = position[X_AXIS] + position[Y_AXIS]; - target[Y_AXIS] = position[X_AXIS] - position[Y_AXIS]; + target->x = position->x + position->y; + target->y = position->x - position->y; for(idx = Z_AXIS; idx < N_AXIS; idx++) - target[idx] = position[idx]; + target->values[idx] = position->values[idx]; return target; } // Transform position from motor (corexy) coordinate system to cartesian coordinate system -static inline float *transform_to_cartesian (float *target, float *position) +static inline coord_data_t *transform_to_cartesian (coord_data_t *target, coord_data_t *position) { uint_fast8_t idx; - target[X_AXIS] = (position[X_AXIS] + position[Y_AXIS]) * 0.5f; - target[Y_AXIS] = (position[X_AXIS] - position[Y_AXIS]) * 0.5f; + target->x = (position->x + position->y) * 0.5f; + target->y = (position->x - position->y) * 0.5f; for(idx = Z_AXIS; idx < N_AXIS; idx++) - target[idx] = position[idx]; + target->values[idx] = position->values[idx]; return target; } @@ -103,12 +103,12 @@ static void corexy_limits_set_target_pos (uint_fast8_t idx) // fn name? switch(idx) { case X_AXIS: - axis_position = corexy_convert_to_b_motor_steps(sys.position); + axis_position = corexy_convert_to_b_motor_steps((mpos_t *)sys.position); sys.position[A_MOTOR] = axis_position; sys.position[B_MOTOR] = -axis_position; break; case Y_AXIS: - sys.position[A_MOTOR] = sys.position[B_MOTOR] = corexy_convert_to_a_motor_steps(sys.position); + sys.position[A_MOTOR] = sys.position[B_MOTOR] = corexy_convert_to_a_motor_steps((mpos_t *)sys.position); break; default: sys.position[idx] = 0; @@ -123,11 +123,11 @@ static bool corexy_check_travel_limits (float *target, axes_signals_t axes, bool if(is_cartesian) return check_travel_limits(target, axes, true, envelope); - float cartesian_coords[N_AXIS]; + coord_data_t cartesian_coords; - transform_to_cartesian(cartesian_coords, target); + transform_to_cartesian(&cartesian_coords, (coord_data_t *)target); - return check_travel_limits(cartesian_coords, axes, true, envelope); + return check_travel_limits(cartesian_coords.values, axes, true, envelope); } // Set machine positions for homed limit switches. Don't update non-homed axes. @@ -140,11 +140,11 @@ static void corexy_limits_set_machine_positions (axes_signals_t cycle) do { if(cycle.mask & bit(--idx)) switch(idx) { case X_AXIS: - sys.position[A_MOTOR] = corexy_convert_to_b_motor_steps(sys.position); + sys.position[A_MOTOR] = corexy_convert_to_b_motor_steps((mpos_t *)sys.position); sys.position[B_MOTOR] = - sys.position[A_MOTOR]; break; case Y_AXIS: - sys.position[A_MOTOR] = corexy_convert_to_a_motor_steps(sys.position); + sys.position[A_MOTOR] = corexy_convert_to_a_motor_steps((mpos_t *)sys.position); sys.position[B_MOTOR] = sys.position[A_MOTOR]; break; default: @@ -163,12 +163,12 @@ static void corexy_limits_set_machine_positions (axes_signals_t cycle) : lroundf(-pulloff->values[idx] * settings.axis[idx].steps_per_mm); switch(idx) { case X_AXIS: - off_axis_position = corexy_convert_to_b_motor_steps(sys.position); + off_axis_position = corexy_convert_to_b_motor_steps((mpos_t *)sys.position); sys.position[A_MOTOR] = set_axis_position + off_axis_position; sys.position[B_MOTOR] = set_axis_position - off_axis_position; break; case Y_AXIS: - off_axis_position = corexy_convert_to_a_motor_steps(sys.position); + off_axis_position = corexy_convert_to_a_motor_steps((mpos_t *)sys.position); sys.position[A_MOTOR] = off_axis_position + set_axis_position; sys.position[B_MOTOR] = off_axis_position - set_axis_position; break; @@ -180,46 +180,46 @@ static void corexy_limits_set_machine_positions (axes_signals_t cycle) } while(idx); } -static inline float get_distance (float *p0, float *p1) +static inline float get_distance (coord_data_t *p0, coord_data_t *p1) { uint_fast8_t idx = N_AXIS; float distance = 0.0f; do { idx--; - distance += (p0[idx] - p1[idx]) * (p0[idx] - p1[idx]); + distance += (p0->values[idx] - p1->values[idx]) * (p0->values[idx] - p1->values[idx]); } while(idx); return sqrtf(distance); } // called from mc_line() to segment lines if not overridden, default implementation for pass-through -static float *kinematics_segment_line (float *target, float *position, plan_line_data_t *pl_data, bool init) +static coord_data_t *kinematics_segment_line (coord_data_t *target, coord_data_t *position, plan_line_data_t *pl_data, bool init) { static uint_fast8_t iterations; - static float trsf[N_AXIS]; + static coord_data_t trsf; if(init) { iterations = 2; - transform_from_cartesian(trsf, target); + transform_from_cartesian(&trsf, target); if(!pl_data->condition.rapid_motion) { uint_fast8_t idx; - float cpos[N_AXIS]; + coord_data_t cpos; - cpos[X_AXIS] = (position[X_AXIS] + position[Y_AXIS]) * .5f; - cpos[Y_AXIS] = (position[X_AXIS] - position[Y_AXIS]) * .5f; + cpos.x = (position->x + position->y) * .5f; + cpos.y = (position->x - position->y) * .5f; for(idx = Z_AXIS; idx < N_AXIS; idx++) - cpos[idx] = position[idx]; + cpos.values[idx] = position->values[idx]; - pl_data->feed_rate *= get_distance(trsf, position) / get_distance(target, cpos); + pl_data->feed_rate *= get_distance(&trsf, position) / get_distance(target, &cpos); } } - return iterations-- == 0 ? NULL : trsf; + return iterations-- == 0 ? NULL : &trsf; } static bool homing_cycle_validate (axes_signals_t cycle) diff --git a/kinematics/delta.c b/kinematics/delta.c index c71a878..37eff17 100644 --- a/kinematics/delta.c +++ b/kinematics/delta.c @@ -3,7 +3,7 @@ Part of grblHAL - Copyright (c) 2023-2025 Terje Io + Copyright (c) 2023-2026 Terje Io Transforms derived from mzavatsky at Trossen Robotics https://hypertriangle.com/~alex/delta-robot-tutorial/ get_cuboid_envelope() derived from javascript code in @@ -36,7 +36,7 @@ #include "../planner.h" #include "../motion_control.h" #include "../protocol.h" -#include "../kinematics.h" +#include "interface.h" #define A_MOTOR X_AXIS #define B_MOTOR Y_AXIS @@ -125,28 +125,28 @@ static bool delta_calcAngleYZ (float x0, float y0, float z0, float *theta) // 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) +static bool delta_calcInverse (coord_data_t *pos, coord_data_t *target) { - target[A_MOTOR] = target[B_MOTOR] = target[C_MOTOR] = 0.0f; + target->values[A_MOTOR] = target->values[B_MOTOR] = target->values[C_MOTOR] = 0.0f; - 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 + return delta_calcAngleYZ(pos->x, pos->y, pos->z, &target->values[A_MOTOR]) && + delta_calcAngleYZ(pos->x * COS120 + pos->y * SIN120, pos->y * COS120 - pos->x * SIN120, pos->z, &target->values[B_MOTOR]) && // rotate coords to +120 deg + delta_calcAngleYZ(pos->x * COS120 - pos->y * SIN120, pos->y * COS120 + pos->x * SIN120, pos->z, &target->values[C_MOTOR]); // rotate coords to -120 deg } // Returns machine position in mm converted from system position. -static float *transform_to_cartesian (float *target, float *position) +static coord_data_t *transform_to_cartesian (coord_data_t *target, coord_data_t *position) { - float y1 = -(machine.t + machine.cfg.rf * cosf(position[A_MOTOR])); - float z1 = -machine.cfg.rf * sinf(position[X_AXIS]); + float y1 = -(machine.t + machine.cfg.rf * cosf(position->values[A_MOTOR])); + float z1 = -machine.cfg.rf * sinf(position->x); - float y2 = (machine.t + machine.cfg.rf * cosf(position[B_MOTOR])) * SIN30; + float y2 = (machine.t + machine.cfg.rf * cosf(position->values[B_MOTOR])) * SIN30; float x2 = y2 * TAN60; - float z2 = -machine.cfg.rf * sinf(position[Y_AXIS]); + float z2 = -machine.cfg.rf * sinf(position->y); - float y3 = (machine.t + machine.cfg.rf * cosf(position[C_MOTOR])) * SIN30; + float y3 = (machine.t + machine.cfg.rf * cosf(position->values[C_MOTOR])) * SIN30; float x3 = -y3 * TAN60; - float z3 = -machine.cfg.rf * sinf(position[Z_AXIS]); + float z3 = -machine.cfg.rf * sinf(position->z); float dnm = (y2 - y1) * x3 -(y3 - y1) * x2; @@ -170,53 +170,53 @@ static float *transform_to_cartesian (float *target, float *position) // discriminant float d = b * b - 4.0f * a * c; if (d < 0.0f) - target[X_AXIS] = target[Y_AXIS] = target[Z_AXIS] = NAN; // non-existing point + target->x = target->y = target->z = NAN; // non-existing point else { - target[Z_AXIS] = -0.5f * (b + sqrtf(d)) / a; - target[X_AXIS] = (a1 * target[Z_AXIS] + b1) / dnm; - target[Y_AXIS] = (a2 * target[Z_AXIS] + b2) / dnm; + target->z = -0.5f * (b + sqrtf(d)) / a; + target->x = (a1 * target->z + b1) / dnm; + target->y = (a2 * target->z + b2) / dnm; } return target; } // Returns machine position in mm converted from system position steps. -static float *delta_convert_array_steps_to_mpos (float *position, int32_t *steps) +static coord_data_t *delta_convert_array_steps_to_mpos (coord_data_t *position, mpos_t *steps) { - float mpos[N_AXIS]; + coord_data_t mpos; uint_fast8_t idx = N_AXIS; do { idx--; - mpos[idx] = steps[idx] / settings.axis[idx].steps_per_mm; + mpos.values[idx] = steps->values[idx] / settings.axis[idx].steps_per_mm; } while(idx); - return transform_to_cartesian(position, mpos); + return transform_to_cartesian(position, &mpos); } // Transform absolute position from cartesian coordinate system to delta robot coordinate system -static float *transform_from_cartesian (float *target, float *position) +static coord_data_t *transform_from_cartesian (coord_data_t *target, coord_data_t *position) { - delta_calcInverse((coord_data_t *)position, target); + delta_calcInverse(position, target); return target; } -static inline float get_distance (float *p0, float *p1) +static inline float get_distance (coord_data_t *p0, coord_data_t *p1) { uint_fast8_t idx = Z_AXIS + 1; float distance = 0.0f; do { idx--; - distance += (p0[idx] - p1[idx]) * (p0[idx] - p1[idx]); + distance += (p0->values[idx] - p1->values[idx]) * (p0->values[idx] - p1->values[idx]); } while(idx); return sqrtf(distance); } // Delta robots needs long lines divided up -static float *delta_segment_line (float *target, float *position, plan_line_data_t *pl_data, bool init) +static coord_data_t *delta_segment_line (coord_data_t *target, coord_data_t *position, plan_line_data_t *pl_data, bool init) { static uint_fast16_t iterations; static bool segmented; @@ -228,20 +228,20 @@ static float *delta_segment_line (float *target, float *position, plan_line_data if(init) { jog_cancel = false; - memcpy(final_target.values, target, sizeof(final_target)); + memcpy(&final_target, target, sizeof(final_target)); - if(delta_calcInverse((coord_data_t *)target, mpos.values)) { + if(delta_calcInverse(target, &mpos)) { 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, &sys.work_envelope); } - transform_to_cartesian(segment_target.values, position); + transform_to_cartesian(&segment_target, position); - delta.x = target[X_AXIS] - segment_target.x; - delta.y = target[Y_AXIS] - segment_target.y; - delta.z = target[Z_AXIS] - segment_target.z; + delta.x = target->x - segment_target.x; + delta.y = target->y - segment_target.y; + delta.z = target->z - segment_target.z; distance = sqrtf(delta.x * delta.x + delta.y * delta.y + delta.z * delta.z); @@ -283,11 +283,11 @@ 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)) { + if(!delta_calcInverse(&segment_target, &mpos)) { memcpy(&mpos, &machine.last_pos, sizeof(coord_data_t)); iterations = 0; } else if(!pl_data->condition.rapid_motion && distance != 0.0f) { - float rate_multiplier = get_distance(mpos.values, machine.last_pos.values) / distance; + float rate_multiplier = get_distance(&mpos, &machine.last_pos) / distance; pl_data->feed_rate *= rate_multiplier; pl_data->rate_multiplier = 1.0f / rate_multiplier; } @@ -295,7 +295,7 @@ static float *delta_segment_line (float *target, float *position, plan_line_data memcpy(&machine.last_pos, &mpos, sizeof(coord_data_t)); } - return iterations == 0 || jog_cancel ? NULL : mpos.values; + return iterations == 0 || jog_cancel ? NULL : &mpos; } static void get_cuboid_envelope (void) @@ -303,7 +303,7 @@ static void get_cuboid_envelope (void) 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]; + coord_data_t pos; uint32_t idx, z; coord_data_t mpos, home = { .x = machine.cfg.home_angle, @@ -311,16 +311,16 @@ static void get_cuboid_envelope (void) .z = machine.cfg.home_angle }; struct { - float pos[N_AXIS]; + coord_data_t pos; } r[8]; - transform_to_cartesian(mpos.values, home.values); + transform_to_cartesian(&mpos, &home); machine.home_z = mpos.z; // find extents for(z = 0; z < settings.axis[X_AXIS].steps_per_mm * 2.0f * M_PI ; ++z) { - pos[0] = pos[1] = pos[2] = sr * (float)z; - transform_to_cartesian(mpos.values, pos); + pos.values[0] = pos.values[1] = pos.values[2] = sr * (float)z; + transform_to_cartesian(&mpos, &pos); if(!isnan(mpos.x)) { if(minz > mpos.z) minz = mpos.z; @@ -332,7 +332,7 @@ 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); + transform_to_cartesian(&mpos, &home); minz = mpos.z; } @@ -356,58 +356,58 @@ static void get_cuboid_envelope (void) mpos.x = sum; mpos.y = sum; mpos.z = middlez + sum; - if((ok = delta_calcInverse(&mpos, r[0].pos))) { + if((ok = delta_calcInverse(&mpos, &r[0].pos))) { mpos.y = -sum; - ok = delta_calcInverse(&mpos, r[1].pos); + ok = delta_calcInverse(&mpos, &r[1].pos); } if(ok) { mpos.x = -sum; - ok = delta_calcInverse(&mpos, r[2].pos); + ok = delta_calcInverse(&mpos, &r[2].pos); } if(ok) { mpos.y = sum; - ok = delta_calcInverse(&mpos, r[3].pos); + ok = delta_calcInverse(&mpos, &r[3].pos); } if(ok) { mpos.x = sum; mpos.z = middlez - sum; - ok = delta_calcInverse(&mpos, r[4].pos); + ok = delta_calcInverse(&mpos, &r[4].pos); } if(ok) { mpos.y = -sum; - ok = delta_calcInverse(&mpos, r[5].pos); + ok = delta_calcInverse(&mpos, &r[5].pos); } if(ok) { mpos.x = -sum; - ok = delta_calcInverse(&mpos, r[6].pos); + ok = delta_calcInverse(&mpos, &r[6].pos); } if(ok) { mpos.y = sum; - ok = delta_calcInverse(&mpos, r[7].pos); + ok = delta_calcInverse(&mpos, &r[7].pos); } if(!ok) { sum -= dist; dist *= 0.5f; } else for(idx = 0; idx < 8; ++idx) { - if(machine.min_angle[A_MOTOR] > r[idx].pos[A_MOTOR]) - machine.min_angle[A_MOTOR] = r[idx].pos[A_MOTOR]; - if(machine.max_angle[A_MOTOR] < r[idx].pos[A_MOTOR]) - machine.max_angle[A_MOTOR] = r[idx].pos[A_MOTOR]; - if(machine.min_angle[B_MOTOR] > r[idx].pos[B_MOTOR]) - machine.min_angle[B_MOTOR] = r[idx].pos[B_MOTOR]; - if(machine.max_angle[B_MOTOR] < r[idx].pos[B_MOTOR]) - machine.max_angle[B_MOTOR] = r[idx].pos[B_MOTOR]; - if(machine.min_angle[C_MOTOR] > r[idx].pos[C_MOTOR]) - machine.min_angle[C_MOTOR] = r[idx].pos[C_MOTOR]; - if(machine.max_angle[C_MOTOR] < r[idx].pos[C_MOTOR]) - machine.max_angle[C_MOTOR] = r[idx].pos[C_MOTOR]; + if(machine.min_angle[A_MOTOR] > r[idx].pos.values[A_MOTOR]) + machine.min_angle[A_MOTOR] = r[idx].pos.values[A_MOTOR]; + if(machine.max_angle[A_MOTOR] < r[idx].pos.values[A_MOTOR]) + machine.max_angle[A_MOTOR] = r[idx].pos.values[A_MOTOR]; + if(machine.min_angle[B_MOTOR] > r[idx].pos.values[B_MOTOR]) + machine.min_angle[B_MOTOR] = r[idx].pos.values[B_MOTOR]; + if(machine.max_angle[B_MOTOR] < r[idx].pos.values[B_MOTOR]) + machine.max_angle[B_MOTOR] = r[idx].pos.values[B_MOTOR]; + if(machine.min_angle[C_MOTOR] > r[idx].pos.values[C_MOTOR]) + machine.min_angle[C_MOTOR] = r[idx].pos.values[C_MOTOR]; + if(machine.max_angle[C_MOTOR] < r[idx].pos.values[C_MOTOR]) + machine.max_angle[C_MOTOR] = r[idx].pos.values[C_MOTOR]; } } while(original_dist > sum && dist > 0.1f); @@ -429,8 +429,8 @@ static void get_cuboid_envelope (void) home.x = home.y = 0; home.z = sys.work_envelope.max.z; - if(delta_calcInverse(&home, pos)) - machine.home_angle_cuboid = pos[A_MOTOR]; + if(delta_calcInverse(&home, &pos)) + machine.home_angle_cuboid = pos.values[A_MOTOR]; machine.min_angle[A_MOTOR] = machine.min_angle[B_MOTOR] = machine.min_angle[C_MOTOR] = delta_settings.flags.home_to_cuboid_top ? machine.home_angle_cuboid : machine.cfg.home_angle; @@ -438,28 +438,28 @@ 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[A_MOTOR] = pos[B_MOTOR] = pos[C_MOTOR] = 0.0f; - transform_to_cartesian(r[0].pos, pos); - pos[A_MOTOR] = sr; - transform_to_cartesian(r[1].pos, pos); + pos.values[A_MOTOR] = pos.values[B_MOTOR] = pos.values[C_MOTOR] = 0.0f; + transform_to_cartesian(&r[0].pos, &pos); + pos.values[A_MOTOR] = sr; + transform_to_cartesian(&r[1].pos, &pos); - float x = r[0].pos[A_MOTOR] - r[1].pos[A_MOTOR]; - float y = r[0].pos[B_MOTOR] - r[1].pos[B_MOTOR]; + float x = r[0].pos.values[A_MOTOR] - r[1].pos.values[A_MOTOR]; + float y = r[0].pos.values[B_MOTOR] - r[1].pos.values[B_MOTOR]; machine.resolution = sqrtf(x * x + y * y); // use as segment length (/ 2)? } -static float *get_homing_target (float *target, float *position) +static coord_data_t *get_homing_target (coord_data_t *target, coord_data_t *position) { uint_fast8_t idx = Z_AXIS + 1; do { idx--; if(homing_mode == HomingMode_Pulloff) - target[idx] = machine.home_pulloff = position[X_AXIS] / machine.resolution / settings.axis[X_AXIS].steps_per_mm; + target->values[idx] = machine.home_pulloff = position->x / machine.resolution / settings.axis[X_AXIS].steps_per_mm; else if(homing_mode == HomingMode_Locate) - target[idx] = position[X_AXIS] / machine.resolution / settings.axis[X_AXIS].steps_per_mm; + target->values[idx] = position->x / machine.resolution / settings.axis[X_AXIS].steps_per_mm; else - target[idx] = bit_istrue(settings.homing.dir_mask.value, bit(idx)) ? -M_PI : M_PI; // 0.5 revolution + target->values[idx] = bit_istrue(settings.homing.dir_mask.value, bit(idx)) ? -M_PI : M_PI; // 0.5 revolution } while(idx); return target; @@ -588,7 +588,7 @@ static bool delta_check_travel_limits (float *target, axes_signals_t axes, bool #endif if(!is_cartesian) { - if(isnan(transform_to_cartesian(pos.values, target)[A_MOTOR])) + if(isnan(transform_to_cartesian(&pos, (coord_data_t *)target)->values[A_MOTOR])) return false; } @@ -600,7 +600,7 @@ static bool delta_check_travel_limits (float *target, axes_signals_t axes, bool if(!is_cartesian) memcpy(&pos.values, target, sizeof(coord_data_t)); - else if(delta_calcInverse((coord_data_t *)target, pos.values) || !pos_ok(&pos)) + else if(delta_calcInverse((coord_data_t *)target, &pos) || !pos_ok(&pos)) return false; if(sys.homed.mask) do { @@ -658,7 +658,7 @@ static void delta_apply_travel_limits (float *target, float *position, work_enve 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) && pos_ok(&pos); + ok = delta_calcInverse((coord_data_t *)target, &pos) && pos_ok(&pos); if(dist > machine.resolution) dist *= 0.5f; diff --git a/kinematics/interface.h b/kinematics/interface.h new file mode 100644 index 0000000..e32cc37 --- /dev/null +++ b/kinematics/interface.h @@ -0,0 +1,49 @@ +/* + kinematics.h - kinematics interface (API) + + Part of grblHAL + + Copyright (c) 2019-2026 Terje Io + + grblHAL is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + grblHAL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with grblHAL. If not, see . +*/ + +#ifndef _KINEMATICS_H_ +#define _KINEMATICS_H_ + +#include "../nuts_bolts.h" + +typedef coord_data_t *(*transform_steps_to_cartesian_ptr)(coord_data_t *position, mpos_t *steps); +typedef coord_data_t *(*transform_from_cartesian_ptr) (coord_data_t *target, coord_data_t *position); +typedef coord_data_t *(*segment_line_ptr) (coord_data_t *target, coord_data_t *position, plan_line_data_t *pl_data, bool init); // target is cartesian, position transformed +typedef uint_fast8_t (*limits_get_axis_mask_ptr)(uint_fast8_t idx); +typedef void (*limits_set_target_pos_ptr)(uint_fast8_t idx); +typedef void (*limits_set_machine_positions_ptr)(axes_signals_t cycle); +typedef bool (*homing_cycle_validate_ptr)(axes_signals_t cycle); +typedef float (*homing_cycle_get_feedrate_ptr)(axes_signals_t axes, float rate, homing_mode_t mode); + +typedef struct { + transform_steps_to_cartesian_ptr transform_steps_to_cartesian; + transform_from_cartesian_ptr transform_from_cartesian; + segment_line_ptr segment_line; // target is cartesian, position transformed + limits_get_axis_mask_ptr limits_get_axis_mask; + limits_set_target_pos_ptr limits_set_target_pos; + limits_set_machine_positions_ptr limits_set_machine_positions; + homing_cycle_validate_ptr homing_cycle_validate; + homing_cycle_get_feedrate_ptr homing_cycle_get_feedrate; +} kinematics_t; + +extern kinematics_t kinematics; + +#endif diff --git a/kinematics/polar.c b/kinematics/polar.c index 82b7a59..927fd41 100644 --- a/kinematics/polar.c +++ b/kinematics/polar.c @@ -31,7 +31,7 @@ #include "../hal.h" #include "../settings.h" #include "../planner.h" -#include "../kinematics.h" +#include "interface.h" #define RADIUS_AXIS X_AXIS #define POLAR_AXIS Y_AXIS @@ -57,22 +57,22 @@ inline static float abs_angle (float ang) } // Returns machine position in mm converted from system position steps. -static float *transform_to_cartesian (float *target, float *position) +static coord_data_t *transform_to_cartesian (coord_data_t *target, coord_data_t *position) { uint_fast8_t idx = N_AXIS; do { switch(--idx) { case X_AXIS: - target[X_AXIS] = cosf(position[POLAR_AXIS] * RADDEG) * position[RADIUS_AXIS]; + target->x = cosf(position->values[POLAR_AXIS] * RADDEG) * position->values[RADIUS_AXIS]; break; case Y_AXIS: - target[Y_AXIS] = sinf(position[POLAR_AXIS] * RADDEG) * position[RADIUS_AXIS]; + target->y = sinf(position->values[POLAR_AXIS] * RADDEG) * position->values[RADIUS_AXIS]; break; default: - target[idx] = position[idx]; // unchanged + target->values[idx] = position->values[idx]; // unchanged break; } } while(idx); @@ -81,63 +81,63 @@ static float *transform_to_cartesian (float *target, float *position) } // Returns machine position in mm converted from system position steps. -static float *polar_convert_array_steps_to_mpos (float *position, int32_t *steps) +static coord_data_t *polar_convert_array_steps_to_mpos (coord_data_t *position, mpos_t *steps) { coord_data_t cpos; uint_fast8_t idx = N_AXIS; do { idx--; - cpos.values[idx] = steps[idx] / settings.axis[idx].steps_per_mm; + cpos.values[idx] = steps->values[idx] / settings.axis[idx].steps_per_mm; } while(idx); - return transform_to_cartesian(position, cpos.values); + return transform_to_cartesian(position, &cpos); } // Transform absolute position from cartesian coordinate system to polar coordinate system -static float *transform_from_cartesian (float *target, float *position) +static coord_data_t *transform_from_cartesian (coord_data_t *target, coord_data_t *position) { float delta_ang; // the difference from the last and next angle uint_fast8_t idx = N_AXIS - 1; do { - target[idx] = position[idx]; + target->values[idx] = position->values[idx]; } while(--idx > Y_AXIS); - target[RADIUS_AXIS] = hypot_f(position[X_AXIS], position[Y_AXIS]); - if (target[RADIUS_AXIS] == 0.0f) { - target[POLAR_AXIS] = last_pos.values[POLAR_AXIS]; // don't care about angle at center + target->values[RADIUS_AXIS] = hypot_f(position->x, position->y); + if (target->values[RADIUS_AXIS] == 0.0f) { + target->values[POLAR_AXIS] = last_pos.values[POLAR_AXIS]; // don't care about angle at center } else { - target[POLAR_AXIS] = atan2f(position[Y_AXIS], position[X_AXIS]) * DEGRAD; + target->values[POLAR_AXIS] = atan2f(position->y, position->x) * DEGRAD; // no negative angles...we want the absolute angle not -90, use 270 - target[POLAR_AXIS] = abs_angle(target[POLAR_AXIS]); + target->values[POLAR_AXIS] = abs_angle(target->values[POLAR_AXIS]); } - delta_ang = target[POLAR_AXIS] - abs_angle(last_pos.values[POLAR_AXIS]); + delta_ang = target->values[POLAR_AXIS] - abs_angle(last_pos.values[POLAR_AXIS]); // if the delta is above 180 degrees it means we are crossing the 0 degree line if (fabs(delta_ang) <= 180.0f) - target[POLAR_AXIS] = last_pos.values[POLAR_AXIS] + delta_ang; + target->values[POLAR_AXIS] = last_pos.values[POLAR_AXIS] + delta_ang; else - target[POLAR_AXIS] = last_pos.values[POLAR_AXIS] + (delta_ang > 0.0f ? - (360.0f - delta_ang) : delta_ang + 360.0f); + target->values[POLAR_AXIS] = last_pos.values[POLAR_AXIS] + (delta_ang > 0.0f ? - (360.0f - delta_ang) : delta_ang + 360.0f); return target; } -static inline float get_distance (float *p0, float *p1) +static inline float get_distance (coord_data_t *p0, coord_data_t *p1) { uint_fast8_t idx = N_AXIS; float distance = 0.0f; do { idx--; - distance += (p0[idx] - p1[idx]) * (p0[idx] - p1[idx]); + distance += (p0->values[idx] - p1->values[idx]) * (p0->values[idx] - p1->values[idx]); } while(idx); return sqrtf(distance); } // Polar is circular in motion, so long lines must be divided up -static float *polar_segment_line (float *target, float *position, plan_line_data_t *pl_data, bool init) +static coord_data_t *polar_segment_line (coord_data_t *target, coord_data_t *position, plan_line_data_t *pl_data, bool init) { static uint_fast16_t iterations; static bool segmented; @@ -151,13 +151,13 @@ static float *polar_segment_line (float *target, float *position, plan_line_data jog_cancel = false; r_offset = gc_get_offset(RADIUS_AXIS, false) * 2.0f; //?? - memcpy(final_target.values, target, sizeof(final_target)); + memcpy(&final_target, target, sizeof(coord_data_t)); - transform_to_cartesian(segment_target.values, position); + transform_to_cartesian(&segment_target, position); - delta.x = target[X_AXIS] - segment_target.x; - delta.y = target[Y_AXIS] - segment_target.y; - delta.z = target[Z_AXIS] - segment_target.z; + delta.x = target->x - segment_target.x; + delta.y = target->y - segment_target.y; + delta.z = target->z - segment_target.z; distance = sqrtf(delta.x * delta.x + delta.y * delta.y + delta.z * delta.z); @@ -193,11 +193,11 @@ static float *polar_segment_line (float *target, float *position, plan_line_data memcpy(&segment_target, &final_target, sizeof(coord_data_t)); segment_target.values[RADIUS_AXIS] -= r_offset; - transform_from_cartesian(cpos.values, segment_target.values); + transform_from_cartesian(&cpos, &segment_target); segment_target.values[RADIUS_AXIS] += r_offset; if(!pl_data->condition.rapid_motion && segmented) { - float rate_multiplier = get_distance(last_pos.values, cpos.values) / distance; + float rate_multiplier = get_distance(&last_pos, &cpos) / distance; rate_multiplier = rate_multiplier == 0.0f ? 1.0 : (rate_multiplier < 0.5f ? 0.5f : rate_multiplier); pl_data->feed_rate *= rate_multiplier; pl_data->rate_multiplier = 1.0f / rate_multiplier; @@ -206,7 +206,7 @@ static float *polar_segment_line (float *target, float *position, plan_line_data memcpy(&last_pos, &cpos, sizeof(coord_data_t)); } - return iterations == 0 || jog_cancel ? NULL : cpos.values; + return iterations == 0 || jog_cancel ? NULL : &cpos; } static uint_fast8_t polar_limits_get_axis_mask (uint_fast8_t idx) @@ -258,7 +258,7 @@ static void onProgramCompleted (program_flow_t program_flow, bool check_mode) coord_data_t cpos; memset(last_pos.values, 0, sizeof(coord_data_t)); - transform_from_cartesian(cpos.values, gc_state.position); + transform_from_cartesian(&cpos, (coord_data_t *)gc_state.position); memcpy(&last_pos, &cpos, sizeof(coord_data_t)); sys.position[POLAR_AXIS] = lroundf(last_pos.values[POLAR_AXIS] * settings.axis[POLAR_AXIS].steps_per_mm); diff --git a/kinematics/wall_plotter.c b/kinematics/wall_plotter.c index 2c83370..e80dfa0 100644 --- a/kinematics/wall_plotter.c +++ b/kinematics/wall_plotter.c @@ -34,7 +34,7 @@ #include "../hal.h" #include "../settings.h" #include "../planner.h" -#include "../kinematics.h" +#include "interface.h" #define A_MOTOR X_AXIS // Must be X_AXIS #define B_MOTOR Y_AXIS // Must be Y_AXIS @@ -63,34 +63,34 @@ static settings_changed_ptr settings_changed; // Returns machine position in mm converted from system position steps. // TODO: perhaps change to double precision here - float calculation results in errors of a couple of micrometers. -static float *wp_convert_array_steps_to_mpos (float *position, int32_t *steps) +static coord_data_t *wp_convert_array_steps_to_mpos (coord_data_t *position, mpos_t *steps) { coord_t len; - len.a = (float)steps[A_MOTOR] / settings.axis[A_MOTOR].steps_per_mm; - len.b = (float)steps[B_MOTOR] / settings.axis[B_MOTOR].steps_per_mm; + len.a = (float)steps->values[A_MOTOR] / settings.axis[A_MOTOR].steps_per_mm; + len.b = (float)steps->values[B_MOTOR] / settings.axis[B_MOTOR].steps_per_mm; - position[X_AXIS] = (machine.width_pow + len.a * len.a - len.b * len.b) / (2.0f * machine.width_mm); - len.a = machine.width_mm - position[X_AXIS]; - position[Y_AXIS] = sqrtf(len.b * len.b - len.a * len.a ); - position[Z_AXIS] = steps[Z_AXIS] / settings.axis[Z_AXIS].steps_per_mm; + position->x = (machine.width_pow + len.a * len.a - len.b * len.b) / (2.0f * machine.width_mm); + len.a = machine.width_mm - position->x; + position->y = sqrtf(len.b * len.b - len.a * len.a ); + position->z = (float)steps->z / settings.axis[Z_AXIS].steps_per_mm; return position; } // Returns machine position in mm converted from system position steps. // TODO: perhaps change to double precision here - float calculation results in errors of a couple of micrometers. -static float *transform_to_cartesian (float *target, float *position) +static coord_data_t *transform_to_cartesian (coord_data_t *target, coord_data_t *position) { coord_t len; - len.a = position[A_MOTOR]; - len.b = position[B_MOTOR]; + len.a = position->values[A_MOTOR]; + len.b = position->values[B_MOTOR]; - target[X_AXIS] = (machine.width_pow + len.a * len.a - len.b * len.b) / (2.0f * machine.width_mm); - len.a = machine.width_mm - target[X_AXIS]; - target[Y_AXIS] = sqrtf(len.b * len.b - len.a * len.a ); - target[Z_AXIS] = position[Z_AXIS]; + target->x = (machine.width_pow + len.a * len.a - len.b * len.b) / (2.0f * machine.width_mm); + len.a = machine.width_mm - target->x; + target->y = sqrtf(len.b * len.b - len.a * len.a ); + target->z = position->z; return target; } @@ -98,49 +98,48 @@ static float *transform_to_cartesian (float *target, float *position) // Wall plotter calculation only. Returns x or y-axis "steps" based on wall plotter motor steps. // A length = sqrt( X^2 + Y^2 ) // B length = sqrt( (MACHINE_WIDTH - X)^2 + Y^2 ) -inline static float wp_convert_to_a_motor_steps (float *target) +inline static float wp_convert_to_a_motor_steps (coord_data_t *target) { - return sqrtf(target[A_MOTOR] * target[A_MOTOR] + target[B_MOTOR] * target[B_MOTOR]); + return sqrtf(target->values[A_MOTOR] * target->values[A_MOTOR] + target->values[B_MOTOR] * target->values[B_MOTOR]); } -inline static float wp_convert_to_b_motor_steps (float *target) +inline static float wp_convert_to_b_motor_steps (coord_data_t *target) { - float xpos = machine.width_mm - target[A_MOTOR]; + float xpos = machine.width_mm - target->values[A_MOTOR]; - return sqrtf(xpos * xpos + target[B_MOTOR] * target[B_MOTOR]); + return sqrtf(xpos * xpos + target->values[B_MOTOR] * target->values[B_MOTOR]); } // Transform absolute position from cartesian coordinate system to wall plotter coordinate system -static float *transform_from_cartesian (float *target, float *position) +static coord_data_t *transform_from_cartesian (coord_data_t *target, coord_data_t *position) { uint_fast8_t idx = N_AXIS - 1; do { - target[idx] = position[idx]; + target->values[idx] = position->values[idx]; } while(--idx > Y_AXIS); - target[A_MOTOR] = wp_convert_to_a_motor_steps(position); - target[B_MOTOR] = wp_convert_to_b_motor_steps(position); + target->values[A_MOTOR] = wp_convert_to_a_motor_steps(position); + target->values[B_MOTOR] = wp_convert_to_b_motor_steps(position); return target; } - -static inline float get_distance (float *p0, float *p1) +static inline float get_distance (coord_data_t *p0, coord_data_t *p1) { - uint_fast8_t idx = Z_AXIS; + uint_fast8_t idx = N_AXIS; float distance = 0.0f; do { idx--; - distance += (p0[idx] - p1[idx]) * (p0[idx] - p1[idx]); + distance += (p0->values[idx] - p1->values[idx]) * (p0->values[idx] - p1->values[idx]); } while(idx); return sqrtf(distance); } // Wall plotter is circular in motion, so long lines must be divided up -static float *wp_segment_line (float *target, float *position, plan_line_data_t *pl_data, bool init) +static coord_data_t *wp_segment_line (coord_data_t *target, coord_data_t *position, plan_line_data_t *pl_data, bool init) { static uint_fast16_t iterations; static bool segmented; @@ -155,11 +154,11 @@ static float *wp_segment_line (float *target, float *position, plan_line_data_t memcpy(final_target.values, target, sizeof(final_target)); - transform_to_cartesian(segment_target.values, position); + transform_to_cartesian(&segment_target, position); - delta.x = target[X_AXIS] - segment_target.x; - delta.y = target[Y_AXIS] - segment_target.y; - delta.z = target[Z_AXIS] - segment_target.z; + delta.x = target->x - segment_target.x; + delta.y = target->y - segment_target.y; + delta.z = target->z - segment_target.z; float distance = sqrtf(delta.x * delta.x + delta.y * delta.y); @@ -193,10 +192,10 @@ static float *wp_segment_line (float *target, float *position, plan_line_data_t } else memcpy(&segment_target, &final_target, sizeof(coord_data_t)); - transform_from_cartesian(cpos.values, segment_target.values); + transform_from_cartesian(&cpos, &segment_target); } - return iterations == 0 || jog_cancel ? NULL : cpos.values; + return iterations == 0 || jog_cancel ? NULL : &cpos; } @@ -208,20 +207,20 @@ static uint_fast8_t wp_limits_get_axis_mask (uint_fast8_t idx) static void wp_limits_set_target_pos (uint_fast8_t idx) // fn name? { - float xy[2]; + coord_data_t xy; int32_t axis_position; - xy[X_AXIS] = sys.position[X_AXIS] / settings.axis[X_AXIS].steps_per_mm; - xy[Y_AXIS] = sys.position[Y_AXIS] / settings.axis[Y_AXIS].steps_per_mm; + xy.x = sys.position[X_AXIS] / settings.axis[X_AXIS].steps_per_mm; + xy.y = sys.position[Y_AXIS] / settings.axis[Y_AXIS].steps_per_mm; switch(idx) { case X_AXIS: - axis_position = wp_convert_to_b_motor_steps(xy); + axis_position = wp_convert_to_b_motor_steps(&xy); sys.position[A_MOTOR] = axis_position; sys.position[B_MOTOR] = -axis_position; break; case Y_AXIS: - sys.position[A_MOTOR] = sys.position[B_MOTOR] = wp_convert_to_a_motor_steps(xy); + sys.position[A_MOTOR] = sys.position[B_MOTOR] = wp_convert_to_a_motor_steps(&xy); break; default: sys.position[idx] = 0; @@ -234,21 +233,21 @@ static void wp_limits_set_target_pos (uint_fast8_t idx) // fn name? // NOTE: settings.max_travel[] is stored as a negative value. static void wp_limits_set_machine_positions (axes_signals_t cycle) { - float xy[2]; + coord_data_t xy; uint_fast8_t idx = N_AXIS; - xy[X_AXIS] = sys.position[X_AXIS] / settings.axis[X_AXIS].steps_per_mm; - xy[Y_AXIS] = sys.position[Y_AXIS] / settings.axis[Y_AXIS].steps_per_mm; + xy.x = sys.position[X_AXIS] / settings.axis[X_AXIS].steps_per_mm; + xy.y = sys.position[Y_AXIS] / settings.axis[Y_AXIS].steps_per_mm; if(settings.homing.flags.force_set_origin) { if (cycle.mask & bit(--idx)) do { switch(--idx) { case X_AXIS: - sys.position[A_MOTOR] = wp_convert_to_b_motor_steps(xy); + sys.position[A_MOTOR] = wp_convert_to_b_motor_steps(&xy); sys.position[B_MOTOR] = - sys.position[A_MOTOR]; break; case Y_AXIS: - sys.position[A_MOTOR] = wp_convert_to_a_motor_steps(xy); + sys.position[A_MOTOR] = wp_convert_to_a_motor_steps(&xy); sys.position[B_MOTOR] = sys.position[A_MOTOR]; break; default: @@ -267,12 +266,12 @@ static void wp_limits_set_machine_positions (axes_signals_t cycle) : lroundf(-pulloff->values[idx] * settings.axis[idx].steps_per_mm); switch(idx) { case X_AXIS: - off_axis_position = wp_convert_to_b_motor_steps(xy); + off_axis_position = wp_convert_to_b_motor_steps(&xy); sys.position[A_MOTOR] = set_axis_position + off_axis_position; sys.position[B_MOTOR] = set_axis_position - off_axis_position; break; case Y_AXIS: - off_axis_position = wp_convert_to_a_motor_steps(xy); + off_axis_position = wp_convert_to_a_motor_steps(&xy); sys.position[A_MOTOR] = off_axis_position + set_axis_position; sys.position[B_MOTOR] = off_axis_position - set_axis_position; break; diff --git a/machine_limits.c b/machine_limits.c index 688478e..804b1be 100644 --- a/machine_limits.c +++ b/machine_limits.c @@ -33,7 +33,7 @@ #include "tool_change.h" #include "state_machine.h" #ifdef KINEMATICS_API -#include "kinematics.h" +#include "kinematics/interface.h" #endif #include "config.h" @@ -203,7 +203,7 @@ FLASHMEM static bool limits_pull_off (axes_signals_t axis, coord_data_t *distanc // Bypass mc_line(). Directly plan homing motion. #ifdef KINEMATICS_API coord_data_t k_target; - plan_buffer_line(kinematics.transform_from_cartesian(k_target.values, target.values), &plan_data); + plan_buffer_line(kinematics.transform_from_cartesian(&k_target, &target)->values, &plan_data); #else plan_buffer_line(target.values, &plan_data); #endif @@ -386,7 +386,7 @@ FLASHMEM static bool homing_cycle (axes_signals_t cycle, axes_signals_t auto_squ #ifdef KINEMATICS_API coord_data_t k_target; - plan_buffer_line(kinematics.transform_from_cartesian(k_target.values, target.values), &plan_data); // Bypass mc_line(). Directly plan homing motion.; + plan_buffer_line(kinematics.transform_from_cartesian(&k_target, &target)->values, &plan_data); // Bypass mc_line(). Directly plan homing motion.; #else plan_buffer_line(target.values, &plan_data); // Bypass mc_line(). Directly plan homing motion. #endif diff --git a/motion_control.c b/motion_control.c index fd60fce..a2921f7 100644 --- a/motion_control.c +++ b/motion_control.c @@ -37,7 +37,7 @@ #include "motion_control.h" #include "tool_change.h" #ifdef KINEMATICS_API -#include "kinematics.h" +#include "kinematics/interface.h" #endif #if ENABLE_BACKLASH_COMPENSATION @@ -84,7 +84,7 @@ bool mc_line (float *target, plan_line_data_t *pl_data) #ifdef KINEMATICS_API float feed_rate = pl_data->feed_rate; pl_data->rate_multiplier = 1.0f; - target = kinematics.segment_line(target, plan_get_position(), pl_data, true); + target = kinematics.segment_line((coord_data_t *)target, (coord_data_t *)plan_get_position(), pl_data, true)->values; #endif // If enabled, check for soft limit violations. Placed here all line motions are picked up @@ -110,7 +110,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((coord_data_t *)target, NULL, pl_data, false)) { #endif #if ENABLE_BACKLASH_COMPENSATION diff --git a/nuts_bolts.h b/nuts_bolts.h index 02d608c..2633083 100644 --- a/nuts_bolts.h +++ b/nuts_bolts.h @@ -326,7 +326,7 @@ typedef struct { } coord_system_data_t; typedef union { - int32_t value[N_AXIS]; + int32_t values[N_AXIS]; struct { int32_t x; int32_t y; diff --git a/spindle_control.c b/spindle_control.c index 92bb226..99762f3 100644 --- a/spindle_control.c +++ b/spindle_control.c @@ -845,9 +845,10 @@ FLASHMEM bool spindle_restore (spindle_ptrs_t *spindle, spindle_state_t state, f { bool ok; - if(spindle->cap.laser) // When in laser mode, ignore spindle spin-up delay. Set to turn on laser when cycle starts. - ok = (sys.step_control.update_spindle_rpm = _spindle_set_state(spindle, state, 0.0f, 0)); - else if(!(ok = spindle_check_state(spindle, state) && spindle->param->rpm == rpm)) + if(spindle->cap.laser) { // When in laser mode, ignore spindle spin-up delay. Set to turn on laser when cycle starts. + if(!(ok = !settings.flags.disable_laser_during_hold)) + ok = (sys.step_control.update_spindle_rpm = _spindle_set_state(spindle, state, 0.0f, 0)); + } else if(!(ok = spindle_check_state(spindle, state) && spindle->param->rpm == rpm)) ok = spindle_set_state_wait(spindle, state, rpm, delay_ms); return ok; diff --git a/system.c b/system.c index 9f6048b..8e072e9 100644 --- a/system.c +++ b/system.c @@ -30,7 +30,7 @@ #include "state_machine.h" #include "machine_limits.h" #ifdef KINEMATICS_API -#include "kinematics.h" +#include "kinematics/interface.h" #endif static uint8_t n_axis = N_AXIS; @@ -1240,7 +1240,7 @@ __NOTE:__ If motor steps and machine position are not in the same coordinate fra void system_convert_array_steps_to_mpos (float *position, int32_t *steps) { #ifdef KINEMATICS_API - kinematics.transform_steps_to_cartesian(position, steps); + kinematics.transform_steps_to_cartesian((coord_data_t *)position, (mpos_t *)steps); #else uint_fast8_t idx = N_AXIS; do { diff --git a/vfs.c b/vfs.c index 8e88aee..01b9d4d 100644 --- a/vfs.c +++ b/vfs.c @@ -23,6 +23,7 @@ #include #include +#include #include "hal.h" #include "vfs.h" @@ -31,6 +32,8 @@ #undef feof #endif +typedef void (*vfs_ptr)(void); + static vfs_mount_t *get_rootfs (void); static inline vfs_mount_t *path_is_mount_dir (const char *path) @@ -318,6 +321,24 @@ static const char *get_filename (vfs_mount_t *mount, const char *filename) return filename; } +static vfs_mount_t *mount_modifiable (const char *path, size_t op_fn) +{ + vfs_mount_t *mount = get_mount((path = parse_path(path))); + + if(mount == NULL || mount->mode.hidden) + vfs_errno = EFAULT; + else if(mount->mode.read_only) + vfs_errno = EROFS; + else if(*((vfs_ptr *)((uint8_t *)mount->vfs + op_fn)) == NULL) + vfs_errno = EPERM; + else if(!strncmp(mount->path, path, strlen(path))) + vfs_errno = EISDIR; + else + vfs_errno = 0; + + return vfs_errno ? NULL : mount; +} + vfs_file_t *vfs_open (const char *filename, const char *mode) { vfs_file_t *file = NULL; @@ -381,6 +402,13 @@ int vfs_seek (vfs_file_t *file, size_t offset) return ((vfs_t *)(file->fs))->fseek(file, offset); } +int vfs_truncate (vfs_file_t *file, size_t offset) +{ + vfs_errno = ((vfs_t *)(file->fs))->ftruncate ? 0 : EPERM; + + return vfs_errno ? -1 : ((vfs_t *)(file->fs))->ftruncate(file, offset); +} + bool vfs_eof (vfs_file_t *file) { vfs_errno = 0; @@ -406,36 +434,44 @@ int vfs_rename (const char *from, const char *to) int vfs_unlink (const char *filename) { - int ret; + int ret = -1; + vfs_mount_t *mount; // TODO: test for dir? - vfs_mount_t *mount = get_mount(filename); // TODO: test for dir? - - if((ret = mount ? mount->vfs->funlink(get_filename(mount, filename)) : -1) != -1 && vfs.on_fs_changed && !mount->mode.hidden) - vfs.on_fs_changed(mount->vfs); + if((mount = mount_modifiable(filename, offsetof(vfs_t, funlink)))) { + if((ret = mount->vfs->funlink(get_filename(mount, filename))) != -1 && vfs.on_fs_changed) + vfs.on_fs_changed(mount->vfs); + } return ret; } int vfs_mkdir (const char *path) { - int ret; + int ret = -1; + vfs_mount_t *mount; - vfs_mount_t *mount = get_mount(path); - - if((ret = mount ? mount->vfs->fmkdir(get_filename(mount, path)) : -1) != -1 && vfs.on_fs_changed && !mount->mode.hidden) - vfs.on_fs_changed(mount->vfs); + if((mount = mount_modifiable(path, offsetof(vfs_t, fmkdir)))) { + if((ret = mount->vfs->fmkdir(get_filename(mount, path))) != -1 && vfs.on_fs_changed) + vfs.on_fs_changed(mount->vfs); + } return ret; } int vfs_rmdir (const char *path) { - int ret; + int ret = -1; + vfs_mount_t *mount; - vfs_mount_t *mount = get_mount(path); + if((mount = mount_modifiable(path, offsetof(vfs_t, frmdir)))) { - if((ret = mount ? mount->vfs->frmdir(get_filename(mount, path)) : -1) != -1 && vfs.on_fs_changed && !mount->mode.hidden) - vfs.on_fs_changed(mount->vfs); + vfs_stat_t st; + + if(vfs_stat(path, &st) == 0 && !st.st_mode.directory) + vfs_errno = ENOTDIR; + else if((ret = mount->vfs->frmdir(get_filename(mount, path))) != -1 && vfs.on_fs_changed) + vfs.on_fs_changed(mount->vfs); + } return ret; } diff --git a/vfs.h b/vfs.h index 53b63e0..248e14d 100644 --- a/vfs.h +++ b/vfs.h @@ -127,6 +127,7 @@ typedef size_t (*vfs_write_ptr)(const void *buffer, size_t size, size_t count, v typedef void (*vfs_close_ptr)(vfs_file_t *file); typedef size_t (*vfs_ftell_ptr)(vfs_file_t *file); typedef int (*vfs_fseek_ptr)(vfs_file_t *file, size_t offset); +typedef int (*vfs_ftruncate_ptr)(vfs_file_t *file, size_t length); typedef bool (*vfs_eof_ptr)(vfs_file_t *file); typedef int (*vfs_rename_ptr)(const char *from, const char *to); typedef int (*vfs_unlink_ptr)(const char *filename); @@ -155,6 +156,7 @@ typedef struct vfs_write_ptr fwrite; vfs_ftell_ptr ftell; vfs_fseek_ptr fseek; + vfs_ftruncate_ptr ftruncate; vfs_eof_ptr feof; vfs_rename_ptr frename; vfs_unlink_ptr funlink; @@ -235,6 +237,7 @@ size_t vfs_write (const void *buffer, size_t size, size_t count, vfs_file_t *fil int vfs_puts (const char *s, vfs_file_t *file); size_t vfs_tell (vfs_file_t *file); int vfs_seek (vfs_file_t *file, size_t offset); +int vfs_truncate (vfs_file_t *file, size_t length); bool vfs_eof (vfs_file_t *file); int vfs_rename (const char *from, const char *to); int vfs_unlink (const char *filename);