From b55cca15f953414e99b4dc6c70c4e9fc1eb4fbb4 Mon Sep 17 00:00:00 2001 From: Terje Io Date: Sun, 12 Jun 2022 20:07:20 +0200 Subject: [PATCH] Changed kinematics API and implementations (corexy and wallplotter) to allow backlash compensation. Ref [ESP32 issue 25](https://github.com/grblHAL/ESP32/issues/25). Fixed feed rate handling for corexy kinematics. Ref issue #147. Fixed tool table/tool change bugs. Ref. [ioSender issue 228](https://github.com/terjeio/ioSender/issues/228). --- README.md | 2 +- alarms.c | 5 +- alarms.h | 3 +- changelog.md | 20 ++++++++ config.h | 56 +++++++++++---------- core_handlers.h | 2 + corexy.c | 97 ++++++++++++++++++++++++++---------- defaults.h | 4 ++ gcode.c | 8 ++- grbl.h | 2 +- grbllib.c | 17 ------- kinematics.h | 10 ++-- limits.c | 26 ++++++++-- motion_control.c | 53 +++++++++++++++++--- planner.c | 26 ++++++---- planner.h | 5 +- protocol.c | 2 + settings.c | 44 +++++++++++++++++ settings.h | 3 +- system.c | 8 ++- tool_change.c | 2 + wall_plotter.c | 124 +++++++++++++++++++++++++++++++++-------------- 22 files changed, 378 insertions(+), 141 deletions(-) diff --git a/README.md b/README.md index d98d10d..79b0204 100644 --- a/README.md +++ b/README.md @@ -11,7 +11,7 @@ It has been written to complement grblHAL and has features such as proper keyboa --- -Latest build date is 20220325, see the [changelog](changelog.md) for details. +Latest build date is 20220612, see the [changelog](changelog.md) for details. __NOTE:__ A settings reset will be performed on an update for versions earlier than 20211122. Backup and restore of settings is recommended. __IMPORTANT!__ A new setting has been introduced for ganged axes motors in version 20211121. I have only bench tested this for a couple of drivers, correct function should be verified after updating by those who have more than three motors configured. diff --git a/alarms.c b/alarms.c index 59816a6..048088c 100644 --- a/alarms.c +++ b/alarms.c @@ -3,7 +3,7 @@ Part of grblHAL - Copyright (c) 2017-2021 Terje Io + Copyright (c) 2017-2022 Terje Io Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC Copyright (c) 2009-2011 Simen Svale Skogsrud @@ -43,7 +43,8 @@ PROGMEM static const alarm_detail_t alarm_detail[] = { { Alarm_Spindle, "Spindle at speed timeout. Clear before continuing." }, { Alarm_HomingFailAutoSquaringApproach, "Homing fail. Could not find second limit switch for auto squared axis within search distances. Try increasing max travel, decreasing pull-off distance, or check wiring." }, { Alarm_SelftestFailed, "Power on selftest (POS) failed." }, - { Alarm_MotorFault, "Motor fault." } + { Alarm_MotorFault, "Motor fault." }, + { Alarm_HomingFail, "Homing fail. Bad configuration." } }; static alarm_details_t details = { diff --git a/alarms.h b/alarms.h index bcc0f74..fad3446 100644 --- a/alarms.h +++ b/alarms.h @@ -44,7 +44,8 @@ typedef enum { Alarm_HomingFailAutoSquaringApproach = 15, Alarm_SelftestFailed = 16, Alarm_MotorFault = 17, - Alarm_AlarmMax = Alarm_MotorFault + Alarm_HomingFail = 18, + Alarm_AlarmMax = Alarm_HomingFail } alarm_code_t; typedef struct { diff --git a/changelog.md b/changelog.md index f076a7b..dbdc9c9 100644 --- a/changelog.md +++ b/changelog.md @@ -1,5 +1,25 @@ ## grblHAL changelog +20220612: + +Core: + +* Changed kinematics API and implementations (corexy and wallplotter) to allow backlash compensation. Ref [ESP32 issue 25](https://github.com/grblHAL/ESP32/issues/25). + +* Fixed feed rate handling for corexy kinematics. Ref issue #147. + +* Fixed tool table/tool change bugs. Ref. [ioSender issue 228](https://github.com/terjeio/ioSender/issues/228). + +Drivers: + +* iMXRT1062 : Fix for issue #38. + +* STM32F4xx : Added missing code guard for boards not having a spindle direction pin. Improved UART channel assignment handling. + +* STM32F7xx : Improved UART channel assignment handling. + +* ESP32 : Added missing file \(corexy.c\) to filelist, fixed incorrect URL in readme. + 20220416: Drivers: diff --git a/config.h b/config.h index 90e600b..d848f94 100644 --- a/config.h +++ b/config.h @@ -74,7 +74,7 @@ __NOTE:__ if switching to a level > 1 please reset non-volatile storage with \a // Enable CoreXY kinematics. Use ONLY with CoreXY machines. // IMPORTANT: If homing is enabled, you must reconfigure the homing cycle #defines above to -// #define HOMING_CYCLE_0 X_AXIS_BIT and #define HOMING_CYCLE_1 Y_AXIS_BIT +//#define HOMING_CYCLE_0 X_AXIS_BIT and #define HOMING_CYCLE_1 Y_AXIS_BIT // NOTE: This configuration option alters the motion of the X and Y axes to principle of operation // defined at (http://corexy.com/theory.html). Motors are assumed to positioned and wired exactly as // described, if not, motions may move in strange directions. Grbl requires the CoreXY A and B motors @@ -113,8 +113,8 @@ __NOTE:__ these definitions are only referenced in this file. Do __NOT__ change! // ADVANCED CONFIGURATION OPTIONS: // Enables code for debugging purposes. Not for general use and always in constant flux. -// #define DEBUG // Uncomment to enable. Default disabled. -// #define DEBUGOUT 0 // Uncomment to claim serial port with given instance number and add HAL entry point for debug output. +//#define DEBUG // Uncomment to enable. Default disabled. +//#define DEBUGOUT 0 // Uncomment to claim serial port with given instance number and add HAL entry point for debug output. // Some status report data isn't necessary for realtime, only intermittently, because the values don't // change often. The following macros configures how many times a status report needs to be called before @@ -172,7 +172,7 @@ __NOTE:__ these definitions are only referenced in this file. Do __NOT__ change! // value. This also ensures that a planned motion always completes and accounts for any floating-point // round-off errors. Although not recommended, a lower value than 1.0 mm/min will likely work in smaller // machines, perhaps to 0.1mm/min, but your success may vary based on multiple factors. -// #define MINIMUM_FEED_RATE 1.0f // (mm/min) +//#define MINIMUM_FEED_RATE 1.0f // (mm/min) // Number of arc generation iterations by small angle approximation before exact arc trajectory // correction with expensive sin() and cos() calculations. This parameter maybe decreased if there @@ -208,7 +208,7 @@ __NOTE:__ these definitions are only referenced in this file. Do __NOT__ change! // available RAM, like when re-compiling for MCU with ample amounts of RAM. Or decrease if the MCU begins to // crash due to the lack of available RAM or if the CPU is having trouble keeping up with planning // new incoming motions as they are executed. -// #define BLOCK_BUFFER_SIZE 36 // Uncomment to override default in planner.h. +//#define BLOCK_BUFFER_SIZE 36 // Uncomment to override default in planner.h. // Governs the size of the intermediary step segment buffer between the step execution algorithm // and the planner blocks. Each segment is set of steps executed at a constant velocity over a @@ -216,11 +216,11 @@ __NOTE:__ these definitions are only referenced in this file. Do __NOT__ change! // block velocity profile is traced exactly. The size of this buffer governs how much step // execution lead time there is for other Grbl processes have to compute and do their thing // before having to come back and refill this buffer, currently at ~50msec of step moves. -// #define SEGMENT_BUFFER_SIZE 10 // Uncomment to override default in stepper.h. +//#define SEGMENT_BUFFER_SIZE 10 // Uncomment to override default in stepper.h. // Configures the position after a probing cycle during Grbl's check mode. Disabled sets // the position to the probe target, when enabled sets the position to the start position. -// #define SET_CHECK_MODE_PROBE_TO_START // Default disabled. Uncomment to enable. +//#define SET_CHECK_MODE_PROBE_TO_START // Default disabled. Uncomment to enable. // Force Grbl to check the state of the hard limit switches when the processor detects a pin // change inside the hard limit ISR routine. By default, Grbl will trigger the hard limits @@ -230,7 +230,7 @@ __NOTE:__ these definitions are only referenced in this file. Do __NOT__ change! // that the switches don't bounce, we recommend enabling this option. This will help prevent // triggering a hard limit when the machine disengages from the switch. // NOTE: This option has no effect if SOFTWARE_DEBOUNCE is enabled. -// #define HARD_LIMIT_FORCE_STATE_CHECK // Default disabled. Uncomment to enable. +//#define HARD_LIMIT_FORCE_STATE_CHECK // Default disabled. Uncomment to enable. // Adjusts homing cycle search and locate scalars. These are the multipliers used by Grbl's // homing cycle to ensure the limit switches are engaged and cleared through each phase of @@ -239,8 +239,8 @@ __NOTE:__ these definitions are only referenced in this file. Do __NOT__ change! // uses the homing pull-off distance setting times the LOCATE_SCALAR to pull-off and re-engage // the limit switch. // NOTE: Both of these values must be greater than 1.0 to ensure proper function. -// #define HOMING_AXIS_SEARCH_SCALAR 1.5f // Uncomment to override defaults in limits.c. -// #define HOMING_AXIS_LOCATE_SCALAR 10.0f // Uncomment to override defaults in limits.c. +//#define HOMING_AXIS_SEARCH_SCALAR 1.5f // Uncomment to override defaults in limits.c. +//#define HOMING_AXIS_LOCATE_SCALAR 10.0f // Uncomment to override defaults in limits.c. // Enable the '$RST=*', '$RST=$', and '$RST=#' non-volatile storage restore commands. There are cases where // these commands may be undesirable. Simply comment the desired macro to disable it. @@ -268,7 +268,7 @@ __NOTE:__ these definitions are only referenced in this file. Do __NOT__ change! // NOTE: If disabled and to ensure Grbl can never alter the build info line, you'll also need to enable // the SETTING_RESTORE_ALL macro above and remove SETTINGS_RESTORE_BUILD_INFO from the mask. // NOTE: See the included grblWrite_BuildInfo.ino example file to write this string seperately. -// #define DISABLE_BUILD_INFO_WRITE_COMMAND // '$I=' Default enabled. Uncomment to disable. +//#define DISABLE_BUILD_INFO_WRITE_COMMAND // '$I=' Default enabled. Uncomment to disable. // Enables and configures Grbl's sleep mode feature. If the spindle or coolant are powered and Grbl // is not actively moving or receiving any commands, a sleep timer will start. If any data or commands @@ -372,11 +372,11 @@ __NOTE:__ these definitions are only referenced in this file. Do __NOT__ change! // instead of ground. // WARNING: When the pull-ups are disabled, this might require additional wiring with pull-down resistors! // Please check driver code and/or documentation. -// #define DISABLE_LIMIT_BITS_PULL_UP_MASK AXES_BITMASK -// #define DISABLE_LIMIT_BITS_PULL_UP_MASK (X_AXIS_BIT|Y_AXIS_BIT) -// #define DISABLE_CONTROL_PINS_PULL_UP_MASK SIGNALS_BITMASK -// #define DISABLE_CONTROL_PINS_PULL_UP_MASK (SIGNALS_SAFETYDOOR_BIT|SIGNALS_RESET_BIT) -// #define DISABLE_PROBE_BIT_PULL_UP +//#define DISABLE_LIMIT_BITS_PULL_UP_MASK AXES_BITMASK +//#define DISABLE_LIMIT_BITS_PULL_UP_MASK (X_AXIS_BIT|Y_AXIS_BIT) +//#define DISABLE_CONTROL_PINS_PULL_UP_MASK SIGNALS_BITMASK +//#define DISABLE_CONTROL_PINS_PULL_UP_MASK (SIGNALS_SAFETYDOOR_BIT|SIGNALS_RESET_BIT) +//#define DISABLE_PROBE_BIT_PULL_UP // If your machine has two limits switches wired in parallel to one axis, you will need to enable // this feature. Since the two switches are sharing a single pin, there is no way for Grbl to tell @@ -394,9 +394,13 @@ __NOTE:__ these definitions are only referenced in this file. Do __NOT__ change! //#define ALLOW_FEED_OVERRIDE_DURING_PROBE_CYCLES // Default disabled. Uncomment to enable. // Inverts logic of the stepper enable signal(s). +#if COMPATIBILITY_LEVEL <= 2 // NOTE: Not universally available for individual axes - check driver documentation. // Specify at least X_AXIS_BIT if a common enable signal is used. -// #define INVERT_ST_ENABLE_MASK (X_AXIS_BIT|Y_AXIS_BIT|Z_AXIS_BIT) // Default disabled. Uncomment to enable. +//#define INVERT_ST_ENABLE_MASK (X_AXIS_BIT|Y_AXIS_BIT|Z_AXIS_BIT) // Default disabled. Uncomment to enable. +#else +//#define INVERT_ST_ENABLE_MASK 1 // Default disabled. Uncomment to enable. +#endif // Mask to be OR'ed with stepper disable signal(s). Axes configured will not be disabled. // NOTE: Not universally available for individual axes - check driver documentation. // Specify at least X_AXIS_BIT if a common enable signal is used. @@ -414,20 +418,20 @@ __NOTE:__ these definitions are only referenced in this file. Do __NOT__ change! // normally-open (NO) switches on the specified pins, rather than the default normally-closed (NC) switches. // NOTE: The first option will invert all control pins. The second option is an example of // inverting only a few pins. See the start of this file for other signal definitions. -// #define INVERT_CONTROL_PIN_MASK SIGNALS_BITMASK // Default disabled. Uncomment to enable. -// #define INVERT_CONTROL_PIN_MASK (SIGNALS_SAFETYDOOR_BIT|SIGNALS_RESET_BIT) // Default disabled. Uncomment to enable. -// #define INVERT_LIMIT_BIT_MASK AXES_BITMASK // Default disabled. Uncomment to enable. Uncomment to enable. -// #define INVERT_LIMIT_BIT_MASK (X_AXIS_BIT|Y_AXIS_BIT) // Default disabled. Uncomment to enable. +//#define INVERT_CONTROL_PIN_MASK SIGNALS_BITMASK // Default disabled. Uncomment to enable. +//#define INVERT_CONTROL_PIN_MASK (SIGNALS_SAFETYDOOR_BIT|SIGNALS_RESET_BIT) // Default disabled. Uncomment to enable. +//#define INVERT_LIMIT_BIT_MASK AXES_BITMASK // Default disabled. Uncomment to enable. Uncomment to enable. +//#define INVERT_LIMIT_BIT_MASK (X_AXIS_BIT|Y_AXIS_BIT) // Default disabled. Uncomment to enable. // For inverting the probe pin use DEFAULT_INVERT_PROBE_BIT in defaults.h // Inverts the selected spindle output signals from active high to active low. Useful for some pre-built electronic boards. -// #define INVERT_SPINDLE_ENABLE_PIN 1 // Default disabled. Uncomment to enable. -// #define INVERT_SPINDLE_CCW_PIN 1 // Default disabled. Uncomment to enable. NOTE: not supported by all drivers. -// #define INVERT_SPINDLE_PWM_PIN 1 // Default disabled. Uncomment to enable. NOTE: not supported by all drivers. +//#define INVERT_SPINDLE_ENABLE_PIN 1 // Default disabled. Uncomment to enable. +//#define INVERT_SPINDLE_CCW_PIN 1 // Default disabled. Uncomment to enable. NOTE: not supported by all drivers. +//#define INVERT_SPINDLE_PWM_PIN 1 // Default disabled. Uncomment to enable. NOTE: not supported by all drivers. // Inverts the selected coolant signals from active high to active low. Useful for some pre-built electronic boards. -// #define INVERT_COOLANT_FLOOD_PIN 1 // Default disabled. Uncomment to enable. -// #define INVERT_COOLANT_MIST_PIN 1 // Default disabled. Note: not supported by all drivers. +//#define INVERT_COOLANT_FLOOD_PIN 1 // Default disabled. Uncomment to enable. +//#define INVERT_COOLANT_MIST_PIN 1 // Default disabled. Note: not supported by all drivers. // Used by variable spindle output only. This forces the PWM output to a minimum duty cycle when enabled. diff --git a/core_handlers.h b/core_handlers.h index 08e99f0..e43b907 100644 --- a/core_handlers.h +++ b/core_handlers.h @@ -84,6 +84,7 @@ typedef bool (*on_probe_fixture_ptr)(tool_data_t *tool, bool at_g59_3, bool on); typedef bool (*on_probe_start_ptr)(axes_signals_t axes, float *target, plan_line_data_t *pl_data); typedef void (*on_probe_completed_ptr)(void); typedef void (*on_toolchange_ack_ptr)(void); +typedef void (*on_jog_cancel_ptr)(sys_state_t state); typedef bool (*on_spindle_select_ptr)(spindle_id_t spindle_id); typedef status_code_t (*on_unknown_sys_command_ptr)(sys_state_t state, char *line); // return Status_Unhandled. typedef status_code_t (*on_user_command_ptr)(char *line); @@ -116,6 +117,7 @@ typedef struct { on_probe_start_ptr on_probe_start; on_probe_completed_ptr on_probe_completed; on_toolchange_ack_ptr on_toolchange_ack; // Called from interrupt context. + on_jog_cancel_ptr on_jog_cancel; // Called from interrupt context. on_laser_ppi_enable_ptr on_laser_ppi_enable; on_spindle_select_ptr on_spindle_select; // core entry points - set up by core before driver_init() is called. diff --git a/corexy.c b/corexy.c index 2780e41..7f917d2 100644 --- a/corexy.c +++ b/corexy.c @@ -3,7 +3,7 @@ Part of grblHAL - Copyright (c) 2019 Terje Io + Copyright (c) 2019-2022 Terje Io Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC Grbl is free software: you can redistribute it and/or modify @@ -24,6 +24,8 @@ #ifdef COREXY +#include + #include "settings.h" #include "planner.h" #include "kinematics.h" @@ -45,37 +47,27 @@ inline static int32_t corexy_convert_to_b_motor_steps (int32_t *steps) } // Returns machine position of axis 'idx'. Must be sent a 'step' array. -static void corexy_convert_array_steps_to_mpos (float *position, int32_t *steps) +static float *corexy_convert_array_steps_to_mpos (float *position, int32_t *steps) { 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[Z_AXIS] = steps[Z_AXIS] / settings.axis[Z_AXIS].steps_per_mm; + + return position; } -// Transform absolute position from cartesian coordinate system (mm) to corexy coordinate system (step) -static void corexy_target_to_steps (int32_t *target_steps, float *target) +// Transform position from cartesian coordinate system to corexy coordinate system +static inline float *transform_from_cartesian (float *target, float *position) { - uint_fast8_t idx = N_AXIS; - int32_t a_steps, b_steps; + uint_fast8_t idx; - do { - switch(--idx) { - case X_AXIS: - a_steps = lroundf(target[idx] * settings.axis[idx].steps_per_mm); - break; - - case Y_AXIS: - b_steps = lroundf(target[idx] * settings.axis[idx].steps_per_mm); - break; - - default: - target_steps[idx] = lroundf(target[idx] * settings.axis[idx].steps_per_mm); - break; - } - } while(idx); + target[X_AXIS] = position[X_AXIS] + position[Y_AXIS]; + target[Y_AXIS] = position[X_AXIS] - position[Y_AXIS]; + + for(idx = Z_AXIS; idx < N_AXIS; idx++) + target[idx] = position[idx]; - target_steps[A_MOTOR] = a_steps + b_steps; - target_steps[B_MOTOR] = a_steps - b_steps; + return target; } static uint_fast8_t corexy_limits_get_axis_mask (uint_fast8_t idx) @@ -151,6 +143,57 @@ static void corexy_limits_set_machine_positions (axes_signals_t cycle) } while(idx); } +static inline float get_distance (float *p0, float *p1) +{ + uint_fast8_t idx = N_AXIS; + float distance = 0.0f; + + do { + idx--; + distance += (p0[idx] - p1[idx]) * (p0[idx] - p1[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 uint_fast8_t iterations; + static float trsf[N_AXIS]; + + if(init) { + + iterations = 2; + + transform_from_cartesian(trsf, target); + + if(!pl_data->condition.rapid_motion) { + + uint_fast8_t idx; + float cpos[N_AXIS]; + + cpos[X_AXIS] = (position[X_AXIS] + position[Y_AXIS]) * .5f; + cpos[Y_AXIS] = (position[X_AXIS] - position[Y_AXIS]) * .5f; + for(idx = Z_AXIS; idx < N_AXIS; idx++) + cpos[idx] = position[idx]; + + pl_data->feed_rate *= get_distance(trsf, position) / get_distance(target, cpos); + } + } + + return iterations-- == 0 ? NULL : trsf; +} + +static bool homing_cycle_validate (axes_signals_t cycle) +{ + return (cycle.mask & (X_AXIS_BIT|Y_AXIS_BIT)) == 0 || cycle.mask < 3; +} + +static float homing_cycle_get_feedrate (float feedrate, axes_signals_t cycle) +{ + return feedrate * sqrtf(2.0f); +} // Initialize API pointers for CoreXY kinematics void corexy_init (void) @@ -158,9 +201,11 @@ void corexy_init (void) kinematics.limits_set_target_pos = corexy_limits_set_target_pos; kinematics.limits_get_axis_mask = corexy_limits_get_axis_mask; kinematics.limits_set_machine_positions = corexy_limits_set_machine_positions; - kinematics.plan_target_to_steps = corexy_target_to_steps; - kinematics.convert_array_steps_to_mpos = corexy_convert_array_steps_to_mpos; + kinematics.transform_from_cartesian = transform_from_cartesian; + kinematics.transform_steps_to_cartesian = corexy_convert_array_steps_to_mpos; + kinematics.segment_line = kinematics_segment_line; + kinematics.homing_cycle_validate = homing_cycle_validate; + kinematics.homing_cycle_get_feedrate = homing_cycle_get_feedrate; } #endif - diff --git a/defaults.h b/defaults.h index 5843f91..b152712 100644 --- a/defaults.h +++ b/defaults.h @@ -573,7 +573,11 @@ #endif #ifndef INVERT_ST_ENABLE_MASK +#if COMPATIBILITY_LEVEL <= 2 #define INVERT_ST_ENABLE_MASK AXES_BITMASK +#else +#define INVERT_ST_ENABLE_MASK 0 +#endif #endif #ifndef INVERT_LIMIT_BIT_MASK diff --git a/gcode.c b/gcode.c index 0673b1c..9fb57d2 100644 --- a/gcode.c +++ b/gcode.c @@ -216,7 +216,9 @@ void gc_set_tool_offset (tool_offset_mode_t mode, uint_fast8_t idx, int32_t offs idx--; tlo_changed |= gc_state.tool_length_offset[idx] != 0.0f; gc_state.tool_length_offset[idx] = 0.0f; - gc_state.tool->offset[idx] = 0; +#ifndef N_TOOLS + gc_state.tool->offset[idx] = 0.0f; +#endif } while(idx); break; @@ -225,7 +227,9 @@ void gc_set_tool_offset (tool_offset_mode_t mode, uint_fast8_t idx, int32_t offs float new_offset = offset / settings.axis[idx].steps_per_mm; tlo_changed |= gc_state.tool_length_offset[idx] != new_offset; gc_state.tool_length_offset[idx] = new_offset; - gc_state.tool->offset[idx] = offset; +#ifndef N_TOOLS + gc_state.tool->offset[idx] = new_offset; +#endif } break; diff --git a/grbl.h b/grbl.h index 0b9c854..eb020fb 100644 --- a/grbl.h +++ b/grbl.h @@ -34,7 +34,7 @@ #else #define GRBL_VERSION "1.1f" #endif -#define GRBL_BUILD 20220325 +#define GRBL_BUILD 20220612 // The following symbols are set here if not already set by the compiler or in config.h // Do NOT change here! diff --git a/grbllib.c b/grbllib.c index 448fd73..44be9ef 100644 --- a/grbllib.c +++ b/grbllib.c @@ -70,22 +70,7 @@ grbl_hal_t hal; static driver_startup_t driver = { .ok = 0xFF }; #ifdef KINEMATICS_API - kinematics_t kinematics; - -// called from mc_line() to segment lines if not overridden, default implementation for pass-through -static bool kinematics_segment_line (float *target, plan_line_data_t *pl_data, bool init) -{ - static uint_fast8_t iterations; - - if(init) - iterations = 2; - else - iterations--; - - return iterations != 0; -} - #endif void dummy_bool_handler (bool arg) @@ -154,8 +139,6 @@ int grbl_enter (void) #ifdef KINEMATICS_API memset(&kinematics, 0, sizeof(kinematics_t)); - - kinematics.segment_line = kinematics_segment_line; // default to no segmentation #endif driver.init = driver_init(); diff --git a/kinematics.h b/kinematics.h index 16d08e7..fdc553d 100644 --- a/kinematics.h +++ b/kinematics.h @@ -3,7 +3,7 @@ Part of grblHAL - Copyright (c) 2019 Terje Io + Copyright (c) 2019-2022 Terje Io Grbl is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -23,12 +23,14 @@ #define _KINEMATICS_H_ typedef struct { - void (*convert_array_steps_to_mpos)(float *position, int32_t *steps); - void (*plan_target_to_steps) (int32_t *target_steps, float *target); - bool (*segment_line) (float *target, plan_line_data_t *pl_data, bool init); + 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)(float feedrate, axes_signals_t cycle); } kinematics_t; extern kinematics_t kinematics; diff --git a/limits.c b/limits.c index 5a26f0e..e15978d 100644 --- a/limits.c +++ b/limits.c @@ -173,7 +173,12 @@ static bool limits_pull_off (axes_signals_t axis, float distance) plan_data.condition.coolant = gc_state.modal.coolant; memcpy(&plan_data.spindle, &gc_state.spindle, sizeof(spindle_t)); +#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.; +#else plan_buffer_line(target.values, &plan_data); // Bypass mc_line(). Directly plan homing motion. +#endif sys.step_control.flags = 0; // Clear existing flags and sys.step_control.execute_sys_motion = On; // set to execute homing motion. @@ -239,7 +244,7 @@ static bool limits_homing_cycle (axes_signals_t cycle, axes_signals_t auto_squar int32_t initial_trigger_position = 0, autosquare_fail_distance = 0; uint_fast8_t n_cycle = (2 * settings.homing.locate_cycles + 1); uint_fast8_t step_pin[N_AXIS], n_active_axis, dual_motor_axis = 0; - float target[N_AXIS]; + coord_data_t target; float max_travel = 0.0f, homing_rate = settings.homing.seek_rate; bool approach = true, autosquare_check = false; axes_signals_t axislock, homing_state; @@ -286,7 +291,7 @@ static bool limits_homing_cycle (axes_signals_t cycle, axes_signals_t auto_squar do { // Initialize and declare variables needed for homing routine. - system_convert_array_steps_to_mpos(target, sys.position); + system_convert_array_steps_to_mpos(target.values, sys.position); axislock = (axes_signals_t){0}; n_active_axis = 0; @@ -303,9 +308,9 @@ static bool limits_homing_cycle (axes_signals_t cycle, axes_signals_t auto_squar #endif // Set target direction based on cycle mask and homing cycle approach state. if (bit_istrue(settings.homing.dir_mask.value, bit(idx))) - target[idx] = approach ? - max_travel : max_travel; + target.values[idx] = approach ? - max_travel : max_travel; else - target[idx] = approach ? max_travel : - max_travel; + target.values[idx] = approach ? max_travel : - max_travel; // Apply axislock to the step port pins active in this cycle. axislock.mask |= step_pin[idx]; @@ -314,6 +319,11 @@ static bool limits_homing_cycle (axes_signals_t cycle, axes_signals_t auto_squar sys.homing_axis_lock.mask = axislock.mask; +#ifdef KINEMATICS_API + if(kinematics.homing_cycle_get_feedrate) + homing_rate = kinematics.homing_cycle_get_feedrate(homing_rate, cycle); +#endif + if(grbl.on_homing_rate_set) grbl.on_homing_rate_set(cycle, homing_rate, !approach); @@ -321,7 +331,13 @@ static bool limits_homing_cycle (axes_signals_t cycle, axes_signals_t auto_squar // Perform homing cycle. Planner buffer should be empty, as required to initiate the homing cycle. plan_data.feed_rate = homing_rate; // Set current homing rate. - plan_buffer_line(target, &plan_data); // 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); // Bypass mc_line(). Directly plan homing motion.; +#else + plan_buffer_line(target.values, &plan_data); // Bypass mc_line(). Directly plan homing motion. +#endif sys.step_control.flags = 0; sys.step_control.execute_sys_motion = On; // Set to execute homing motion and clear existing flags. diff --git a/motion_control.c b/motion_control.c index d01fa7a..fcc103a 100644 --- a/motion_control.c +++ b/motion_control.c @@ -94,6 +94,11 @@ void mc_sync_backlash_position (void) bool mc_line (float *target, plan_line_data_t *pl_data) { +#ifdef KINEMATICS_API + float feed_rate = pl_data->feed_rate; + target = kinematics.segment_line(target, plan_get_position(), pl_data, true); +#endif + // If enabled, check for soft limit violations. Placed here all line motions are picked up // from everywhere in Grbl. // NOTE: Block jog motions. Jogging is a special case and soft limits are handled independently. @@ -117,6 +122,10 @@ bool mc_line (float *target, plan_line_data_t *pl_data) // doesn't update the machine position values. Since the position values used by the g-code // 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)) { +#endif + #ifdef ENABLE_BACKLASH_COMPENSATION if(backlash_enabled.mask) { @@ -169,11 +178,6 @@ bool mc_line (float *target, plan_line_data_t *pl_data) #endif // Backlash comp -#ifdef KINEMATICS_API - kinematics.segment_line(target, pl_data, true); - - while(kinematics.segment_line(target, pl_data, false)) { -#endif // If the buffer is full: good! That means we are well ahead of the robot. // Remain in this loop until there is room in the buffer. do { @@ -192,7 +196,18 @@ bool mc_line (float *target, plan_line_data_t *pl_data) // Forces a buffer sync while in M3 laser mode only. hal.spindle.set_state(pl_data->condition.spindle, pl_data->spindle.rpm); } + #ifdef KINEMATICS_API + if(pl_data->condition.jog_motion) { + sys_state_t state = state_get(); + if ((state == STATE_IDLE || state == STATE_TOOL_CHANGE) && plan_get_current_block() != NULL) { // Check if there is a block to execute. + state_set(STATE_JOG); + st_prep_buffer(); + st_wake_up(); // NOTE: Manual start. No state machine required. + } + } + + pl_data->feed_rate = feed_rate; } #endif } @@ -217,7 +232,6 @@ void mc_arc (float *target, plan_line_data_t *pl_data, float *position, float *o float r_axis1 = -offset[plane.axis_1]; float rt_axis0 = target[plane.axis_0] - center_axis0; float rt_axis1 = target[plane.axis_1] - center_axis1; - // CCW angle between position and target from circle center. Only one atan2() trig computation required. float angular_travel = atan2f(r_axis0 * rt_axis1 - r_axis1 * rt_axis0, r_axis0 * rt_axis0 + r_axis1 * rt_axis1); @@ -292,7 +306,7 @@ void mc_arc (float *target, plan_line_data_t *pl_data, float *position, float *o float sin_Ti; float cos_Ti; float r_axisi; - uint32_t i, count = 0; + uint_fast16_t i, count = 0; for (i = 1; i < segments; i++) { // Increment (segments-1). @@ -741,12 +755,15 @@ status_code_t mc_jog_execute (plan_line_data_t *pl_data, parser_block_t *gc_bloc // Valid jog command. Plan, set state, and execute. mc_line(gc_block->values.xyz, pl_data); + +#ifndef KINEMATICS_API // kinematics may segment long jog moves triggering auto start (RUN)... sys_state_t state = state_get(); if ((state == STATE_IDLE || state == STATE_TOOL_CHANGE) && plan_get_current_block() != NULL) { // Check if there is a block to execute. state_set(STATE_JOG); st_prep_buffer(); st_wake_up(); // NOTE: Manual start. No state machine required. } +#endif return Status_OK; } @@ -796,6 +813,28 @@ status_code_t mc_homing_cycle (axes_signals_t cycle) return Status_Unhandled; } +#ifdef KINEMATICS_API + if(kinematics.homing_cycle_validate) { + + uint_fast8_t idx = N_AXIS; + + if (!home_all) { // Perform homing cycle based on mask. + if(!kinematics.homing_cycle_validate(cycle)) { + system_set_exec_alarm(Alarm_HomingFail); + return Status_Unhandled; + } + } else do { + if(settings.homing.cycle[--idx].mask) { + if(!kinematics.homing_cycle_validate(settings.homing.cycle[idx])) { + system_set_exec_alarm(Alarm_HomingFail); + return Status_Unhandled; + } + } + } while(idx); + + } +#endif + state_set(STATE_HOMING); // Set homing system state. #if COMPATIBILITY_LEVEL == 0 protocol_enqueue_realtime_command(CMD_STATUS_REPORT); // Force a status report and diff --git a/planner.c b/planner.c index f2ad813..91be424 100644 --- a/planner.c +++ b/planner.c @@ -29,9 +29,7 @@ #include "hal.h" #include "nuts_bolts.h" #include "planner.h" -#ifdef KINEMATICS_API -#include "kinematics.h" -#endif + #ifndef MINIMUM_JUNCTION_SPEED #define MINIMUM_JUNCTION_SPEED 0.0f #endif @@ -394,10 +392,6 @@ bool plan_buffer_line (float *target, plan_line_data_t *pl_data) // Compute and store initial move distance data. -#ifdef KINEMATICS_API - kinematics.plan_target_to_steps(target_steps, target); -#endif - idx = N_AXIS; do { idx--; @@ -405,9 +399,7 @@ bool plan_buffer_line (float *target, plan_line_data_t *pl_data) // Also, compute individual axes distance for move and prep unit vector calculations. // NOTE: Computes true distance from converted step values. -#ifndef KINEMATICS_API target_steps[idx] = lroundf(target[idx] * settings.axis[idx].steps_per_mm); -#endif delta_steps = target_steps[idx] - position_steps[idx]; block->steps[idx] = labs(delta_steps); block->step_event_count = max(block->step_event_count, block->steps[idx]); @@ -540,6 +532,22 @@ bool plan_buffer_line (float *target, plan_line_data_t *pl_data) } +// Get the planner position vectors. +float *plan_get_position (void) +{ + static float position[N_AXIS]; + + uint_fast8_t idx = N_AXIS; + + do { + idx--; + position[idx] = pl.position[idx] / settings.axis[idx].steps_per_mm; + } while(idx); + + return position; +} + + // Reset the planner position vectors. Called by the system abort/initialization routine. void plan_sync_position () { diff --git a/planner.h b/planner.h index 2eaceb2..1be6875 100644 --- a/planner.h +++ b/planner.h @@ -3,7 +3,7 @@ Part of grblHAL - Copyright (c) 2019-2021 Terje Io + Copyright (c) 2019-2022 Terje Io Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC Copyright (c) 2009-2011 Simen Svale Skogsrud @@ -132,6 +132,9 @@ float plan_get_exec_block_exit_speed_sqr(); // Called by main program during planner calculations and step segment buffer during initialization. float plan_compute_profile_nominal_speed(plan_block_t *block); +// Get the planner position vectors. +float *plan_get_position (void); + // Reset the planner position vector (in steps) void plan_sync_position(); diff --git a/protocol.c b/protocol.c index 166103d..e454a7d 100644 --- a/protocol.c +++ b/protocol.c @@ -805,6 +805,8 @@ ISR_CODE bool ISR_FUNC(protocol_enqueue_realtime_command)(char c) if (state_get() & STATE_JOG) // Block all other states from invoking motion cancel. system_set_exec_state_flag(EXEC_MOTION_CANCEL); #endif + if(grbl.on_jog_cancel) + grbl.on_jog_cancel(state_get()); break; case CMD_GCODE_REPORT: diff --git a/settings.c b/settings.c index 137b3d6..8c760df 100644 --- a/settings.c +++ b/settings.c @@ -77,6 +77,7 @@ PROGMEM const settings_t defaults = { .flags.legacy_rt_commands = DEFAULT_LEGACY_RTCOMMANDS, .flags.report_inches = DEFAULT_REPORT_INCHES, .flags.sleep_enable = DEFAULT_SLEEP_ENABLE, + .flags.compatibility_level = COMPATIBILITY_LEVEL, #if DISABLE_G92_PERSISTENCE .flags.g92_is_volatile = 1, #else @@ -104,7 +105,13 @@ PROGMEM const settings_t defaults = { .steppers.step_invert.mask = DEFAULT_STEPPING_INVERT_MASK, .steppers.dir_invert.mask = DEFAULT_DIRECTION_INVERT_MASK, .steppers.ganged_dir_invert.mask = DEFAULT_GANGED_DIRECTION_INVERT_MASK, +#if COMPATIBILITY_LEVEL <= 2 .steppers.enable_invert.mask = INVERT_ST_ENABLE_MASK, +#elif INVERT_ST_ENABLE_MASK + .steppers.enable_invert.mask = 0, +#else + .steppers.enable_invert.mask = AXES_BITMASK, +#endif .steppers.deenergize.mask = ST_DEENERGIZE_MASK, #if N_AXIS > 3 .steppers.is_rotational.mask = (ST_ROTATIONAL_MASK & AXES_BITMASK) >> 3, @@ -361,6 +368,9 @@ static char *get_linear_piece (setting_id_t id); #if N_AXIS > 3 static status_code_t set_rotational_axes (setting_id_t id, uint_fast16_t int_value); #endif +#if COMPATIBILITY_LEVEL > 2 +static status_code_t set_enable_invert_mask (setting_id_t id, uint_fast16_t int_value); +#endif #if COMPATIBILITY_LEVEL > 1 static status_code_t set_limits_invert_mask (setting_id_t id, uint_fast16_t int_value); #endif @@ -384,7 +394,11 @@ PROGMEM static const setting_detail_t setting_detail[] = { { Setting_StepperIdleLockTime, Group_Stepper, "Step idle delay", "milliseconds", Format_Int16, "####0", NULL, "65535", Setting_IsLegacy, &settings.steppers.idle_lock_time, NULL, NULL }, { Setting_StepInvertMask, Group_Stepper, "Step pulse invert", NULL, Format_AxisMask, NULL, NULL, NULL, Setting_IsLegacy, &settings.steppers.step_invert.mask, NULL, NULL }, { Setting_DirInvertMask, Group_Stepper, "Step direction invert", NULL, Format_AxisMask, NULL, NULL, NULL, Setting_IsLegacy, &settings.steppers.dir_invert.mask, NULL, NULL }, +#if COMPATIBILITY_LEVEL <= 2 { Setting_InvertStepperEnable, Group_Stepper, "Invert stepper enable pin(s)", NULL, Format_AxisMask, NULL, NULL, NULL, Setting_IsLegacy, &settings.steppers.enable_invert.mask, NULL, NULL }, +#else + { Setting_InvertStepperEnable, Group_Stepper, "Invert stepper enable pins", NULL, Format_Bool, NULL, NULL, NULL, Setting_IsLegacyFn, set_enable_invert_mask, get_int, NULL }, +#endif #if COMPATIBILITY_LEVEL <= 1 { Setting_LimitPinsInvertMask, Group_Limits, "Invert limit pins", NULL, Format_AxisMask, NULL, NULL, NULL, Setting_IsLegacy, &settings.limits.invert.mask, NULL, NULL }, #else @@ -525,9 +539,13 @@ PROGMEM static const setting_descr_t setting_descr[] = { { Setting_StepperIdleLockTime, "Sets a short hold delay when stopping to let dynamics settle before disabling steppers. Value 255 keeps motors enabled." }, { Setting_StepInvertMask, "Inverts the step signals (active low)." }, { Setting_DirInvertMask, "Inverts the direction signals (active low)." }, +#if COMPATIBILITY_LEVEL <= 2 { Setting_InvertStepperEnable, "Inverts the stepper driver enable signals. Most drivers uses active low enable requiring inversion.\\n\\n" "NOTE: If the stepper drivers shares the same enable signal only X is used." }, +#else + { Setting_InvertStepperEnable, "Inverts the stepper driver enable signals. Drivers using active high enable require inversion.\\n\\n" }, +#endif { Setting_LimitPinsInvertMask, "Inverts the axis limit input signals." }, { Setting_InvertProbePin, "Inverts the probe input pin signal." }, { Setting_SpindlePWMBehaviour, "" }, @@ -737,13 +755,26 @@ setting_details_t *settings_get_details (void) return &setting_details; } +#if COMPATIBILITY_LEVEL > 2 + +static status_code_t set_enable_invert_mask (setting_id_t id, uint_fast16_t int_value) +{ + settings.steppers.enable_invert.mask = int_value ? 0 : AXES_BITMASK; + + return Status_OK; +} + +#endif + #if COMPATIBILITY_LEVEL > 1 + static status_code_t set_limits_invert_mask (setting_id_t id, uint_fast16_t int_value) { settings.limits.invert.mask = (int_value ? ~(INVERT_LIMIT_BIT_MASK) : INVERT_LIMIT_BIT_MASK) & AXES_BITMASK; return Status_OK; } + #endif static status_code_t set_probe_invert (setting_id_t id, uint_fast16_t int_value) @@ -1209,6 +1240,12 @@ static uint32_t get_int (setting_id_t id) switch(id) { +#if COMPATIBILITY_LEVEL > 2 + case Setting_InvertStepperEnable: + value = settings.steppers.enable_invert.mask ? 0 : 1; + break; +#endif + #if COMPATIBILITY_LEVEL > 1 case Setting_LimitPinsInvertMask: value = settings.limits.invert.mask == INVERT_LIMIT_BIT_MASK ? 0 : 1; @@ -1620,6 +1657,11 @@ bool read_global_settings () settings.flags.g92_is_volatile = On; #endif +#if COMPATIBILITY_LEVEL > 2 + if(settings.steppers.enable_invert.mask) + settings.steppers.enable_invert.mask = AXES_BITMASK; +#endif + return ok && settings.version == SETTINGS_VERSION; } @@ -1630,6 +1672,8 @@ void settings_write_global (void) if(override_backup.valid) restore_override_backup(); + settings.flags.compatibility_level = COMPATIBILITY_LEVEL; + if(hal.nvs.type != NVS_None) hal.nvs.memcpy_to_nvs(NVS_ADDR_GLOBAL, (uint8_t *)&settings, sizeof(settings_t), true); } diff --git a/settings.h b/settings.h index 2434506..74dec9a 100644 --- a/settings.h +++ b/settings.h @@ -334,7 +334,8 @@ typedef union { restore_after_feed_hold :1, unused1 :1, g92_is_volatile :1, - unassigned :6; + compatibility_level :4, + unassigned :2; }; } settingflags_t; diff --git a/system.c b/system.c index 688ee6f..b9a04f7 100644 --- a/system.c +++ b/system.c @@ -267,6 +267,10 @@ void system_command_help (void) #else hal.stream.write("$RST=# - reset offsets" ASCII_EOL); #endif + if(hal.spindle.reset_data) + hal.stream.write("$SR - reset spindle encoder data" ASCII_EOL); + if(hal.spindle.get_data) + hal.stream.write("$SD - output spindle encoder data" ASCII_EOL); hal.stream.write("$TLR - set tool offset reference" ASCII_EOL); hal.stream.write("$TPW - probe tool plate" ASCII_EOL); hal.stream.write("$EA - enumerate alarms" ASCII_EOL); @@ -761,7 +765,7 @@ static status_code_t output_all_build_info (sys_state_t state, char *args) char info[sizeof(stored_line_t)]; settings_read_build_info(info); - report_build_info(info, false); + report_build_info(info, true); return Status_OK; } @@ -893,7 +897,7 @@ void system_flag_wco_change (void) void system_convert_array_steps_to_mpos (float *position, int32_t *steps) { #ifdef KINEMATICS_API - kinematics.convert_array_steps_to_mpos(position, steps); + kinematics.transform_steps_to_cartesian(position, steps); #else uint_fast8_t idx = N_AXIS; do { diff --git a/tool_change.c b/tool_change.c index cd228cc..55d5b6f 100644 --- a/tool_change.c +++ b/tool_change.c @@ -451,8 +451,10 @@ status_code_t tc_probe_workpiece (void) gc_parser_flags_t flags = {0}; plan_line_data_t plan_data = {0}; +#if COMPATIBILITY_LEVEL <= 1 if(probe_fixture) grbl.on_probe_fixture(next_tool, system_xy_at_fixture(CoordinateSystem_G59_3, TOOLSETTER_RADIUS), true); +#endif // Get current position. system_convert_array_steps_to_mpos(target.values, sys.position); diff --git a/wall_plotter.c b/wall_plotter.c index 1c9380a..223eb6e 100644 --- a/wall_plotter.c +++ b/wall_plotter.c @@ -28,9 +28,13 @@ #ifdef WALL_PLOTTER +#include +#include + #include "settings.h" #include "planner.h" #include "kinematics.h" +#include "hal.h" #define A_MOTOR X_AXIS // Must be X_AXIS #define B_MOTOR Y_AXIS // Must be Y_AXIS @@ -52,11 +56,12 @@ typedef struct { float b; } coord_t; +static bool jog_cancel = false; static machine_t machine = {0}; // 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 void wp_convert_array_steps_to_mpos (float *position, int32_t *steps) +static float *wp_convert_array_steps_to_mpos (float *position, int32_t *steps) { coord_t len; @@ -67,71 +72,109 @@ static void wp_convert_array_steps_to_mpos (float *position, int32_t *steps) 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; + + 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) +{ + coord_t len; + + len.a = position[A_MOTOR]; + len.b = position[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]; + + return target; } // 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 int32_t wp_convert_to_a_motor_steps (float *target) +inline static float wp_convert_to_a_motor_steps (float *target) { - return (int32_t)lroundf(sqrtf(target[A_MOTOR] * target[A_MOTOR] + target[B_MOTOR] * target[B_MOTOR]) * settings.axis[A_MOTOR].steps_per_mm); + return sqrtf(target[A_MOTOR] * target[A_MOTOR] + target[B_MOTOR] * target[B_MOTOR]); } -inline static int32_t wp_convert_to_b_motor_steps (float *target) +inline static float wp_convert_to_b_motor_steps (float *target) { float xpos = machine.width_mm - target[A_MOTOR]; - return (int32_t)lroundf(sqrtf(xpos * xpos + target[B_MOTOR] * target[B_MOTOR]) * settings.axis[B_MOTOR].steps_per_mm); + + return sqrtf(xpos * xpos + target[B_MOTOR] * target[B_MOTOR]); } -// Transform absolute position from cartesian coordinate system (mm) to wall plotter coordinate system (step) -static void wp_plan_target_to_steps (int32_t *target_steps, float *target) +// Transform absolute position from cartesian coordinate system to wall plotter coordinate system +static float *transform_from_cartesian (float *target, float *position) { uint_fast8_t idx = N_AXIS - 1; do { - target_steps[idx] = lroundf(target[idx] * settings.axis[idx].steps_per_mm); + target[idx] = position[idx]; } while(--idx > Y_AXIS); - target_steps[A_MOTOR] = wp_convert_to_a_motor_steps(target); - target_steps[B_MOTOR] = wp_convert_to_b_motor_steps(target); + target[A_MOTOR] = wp_convert_to_a_motor_steps(position); + target[B_MOTOR] = wp_convert_to_b_motor_steps(position); + + return target; +} + + +static inline float get_distance (float *p0, float *p1) +{ + uint_fast8_t idx = Z_AXIS; + float distance = 0.0f; + + do { + idx--; + distance += (p0[idx] - p1[idx]) * (p0[idx] - p1[idx]); + } while(idx); + + return sqrtf(distance); } // Wall plotter is circular in motion, so long lines must be divided up -static bool wp_segment_line (float *target, plan_line_data_t *pl_data, bool init) +static float *wp_segment_line (float *target, float *position, plan_line_data_t *pl_data, bool init) { static uint_fast16_t iterations; static bool segmented; - static float delta[N_AXIS], segment_target[N_AXIS]; + static coord_data_t delta, segment_target, final_target, cpos; // static plan_line_data_t plan; uint_fast8_t idx = N_AXIS; if(init) { - float max_delta = 0.0f; + jog_cancel = false; - do { - idx--; - delta[idx] = target[idx] - gc_state.position[idx]; - max_delta = max(max_delta, fabsf(delta[idx])); - } while(idx); + memcpy(final_target.values, target, sizeof(final_target)); - if((segmented = !(pl_data->condition.rapid_motion || pl_data->condition.jog_motion) && - max_delta > MAX_SEG_LENGTH_MM && !(delta[X_AXIS] == 0.0f && delta[Y_AXIS] == 0.0f))) { + transform_to_cartesian(segment_target.values, position); - idx = N_AXIS; - iterations = (uint_fast16_t)ceilf(max_delta / MAX_SEG_LENGTH_MM); + delta.x = target[X_AXIS] - segment_target.x; + delta.y = target[Y_AXIS] - segment_target.y; + delta.z = target[Z_AXIS] - segment_target.z; + + float distance = sqrtf(delta.x * delta.x + delta.y * delta.y); - memcpy(segment_target, gc_state.position, sizeof(segment_target)); -// memcpy(&plan, pl_data, sizeof(plan_line_data_t)); + if((segmented = !pl_data->condition.rapid_motion && distance > MAX_SEG_LENGTH_MM && !(delta.x == 0.0f && delta.y == 0.0f))) { + + idx = N_AXIS; + iterations = (uint_fast16_t)ceilf(distance / MAX_SEG_LENGTH_MM); do { - delta[--idx] /= (float)iterations; - target[idx] = gc_state.position[idx]; + --idx; + delta.values[idx] = delta.values[idx] / (float)iterations; } while(idx); - } else + } else { iterations = 1; + memcpy(&segment_target, &final_target, sizeof(coord_data_t)); + } iterations++; // return at least one iteration @@ -139,16 +182,19 @@ static bool wp_segment_line (float *target, plan_line_data_t *pl_data, bool init iterations--; - if(segmented && iterations) do { - idx--; - segment_target[idx] += delta[idx]; - target[idx] = segment_target[idx]; -// memcpy(pl_data, &plan, sizeof(plan_line_data_t)); - } while(idx); + if(segmented && iterations > 1) { + do { + idx--; + segment_target.values[idx] += delta.values[idx]; + } while(idx); + + } else + memcpy(&segment_target, &final_target, sizeof(coord_data_t)); + transform_from_cartesian(cpos.values, segment_target.values); } - return iterations != 0; + return iterations == 0 || jog_cancel ? NULL : cpos.values; } @@ -233,6 +279,10 @@ static void wp_limits_set_machine_positions (axes_signals_t cycle) } while(idx); } +static void cancel_jog (sys_state_t state) +{ + jog_cancel = true; +} // Initialize API pointers for Wall Plotter kinematics void wall_plotter_init (void) @@ -253,9 +303,11 @@ void wall_plotter_init (void) kinematics.limits_set_target_pos = wp_limits_set_target_pos; kinematics.limits_get_axis_mask = wp_limits_get_axis_mask; kinematics.limits_set_machine_positions = wp_limits_set_machine_positions; - kinematics.plan_target_to_steps = wp_plan_target_to_steps; - kinematics.convert_array_steps_to_mpos = wp_convert_array_steps_to_mpos; + kinematics.transform_from_cartesian = transform_from_cartesian; + kinematics.transform_steps_to_cartesian = wp_convert_array_steps_to_mpos; kinematics.segment_line = wp_segment_line; + + grbl.on_jog_cancel = cancel_jog; } #endif