From 7ade244a78469828ea054fc880d0eabc03aaa126 Mon Sep 17 00:00:00 2001 From: Terje Io Date: Mon, 12 Aug 2024 15:46:58 +0200 Subject: [PATCH] Improved handling of extended M commands (plugin based) command words. Fixes issues for programs containing extended M-codes using single meaning words (which they as a rule should not do). Added core support for spindle encoder binding to spindles. Added sorting of spindle report: enabled spindles are sorted first in order of spindle number, disabled by type then spindle id. Changed realtime report to report spindle number instead of spindle id on changes in the |S: element. --- changelog.md | 35 ++++++++++++++++++ gcode.c | 49 ++++++++++++++---------- grbl.h | 2 +- modbus.h | 2 +- report.c | 71 +++++++++++++++++++++++++++++++++-- report.h | 2 + spindle_control.c | 94 ++++++++++++++++++++++++++++++----------------- spindle_control.h | 20 ++++++++-- stepper2.c | 9 +++++ stepper2.h | 1 + 10 files changed, 224 insertions(+), 61 deletions(-) diff --git a/changelog.md b/changelog.md index cae8d7c..75d1785 100644 --- a/changelog.md +++ b/changelog.md @@ -1,5 +1,40 @@ ## grblHAL changelog +Build 20240812 + +Core: + +* Improved handling of extended M commands \(plugin based\) command words. Fixes issues for programs containing extended M-codes using single meaning words \(which they as a rule should not do\). + +* Added core support for spindle encoder binding to spindles. + +* Added sorting of spindle report: enabled spindles are sorted first in order of spindle number, disabled by type then spindle id. + +* Changed realtime report to report spindle number instead of spindle id on changes in the `|S:` element. Part of fix for ioSender [issue #399](https://github.com/terjeio/ioSender/issues/399). + +Drivers: + +* imXRT1061, MSP432, STM32F4xx, STM32F7xx: Updated to take advandage of new spindle encoder binding functionality. + +Plugins: + +* Spindle: updated relevant drivers to use new spindle encoder binding functionality, simplified code. Fixes [issue #30](https://github.com/grblHAL/Plugins_spindle/issues/30). + +--- + +Build 20240805 + +Core: + +* Added function for getting speed \(RPM\) of stepper controlled by secondary stepper motor driver. + +Plugins: + +* Spindle: stepper spindle code now uses new function for getting speed \(RPM\) of motor. +HAL functions for getting spindle data \(actual RPM, angular position etc.\) directed to stepper spindle code, + +--- + Build 20240801 Core: diff --git a/gcode.c b/gcode.c index 6ac12ec..01d21b2 100644 --- a/gcode.c +++ b/gcode.c @@ -848,7 +848,7 @@ status_code_t gc_execute_block (char *block) // Initialize command and value words and parser flags variables. modal_groups_t command_words = {0}; // Bitfield for tracking G and M command words. Also used for modal group violations. gc_parser_flags_t gc_parser_flags = {0}; // Parser flags for handling special cases. - static parameter_words_t user_words = {0}; // User M-code words "taken" + parameter_words_t user_words = {0}; // User M-code words "taken" // Determine if the line is a jogging motion or a normal g-code block. if (block[0] == '$') { // NOTE: `$J=` already parsed when passed to this function. @@ -1680,6 +1680,24 @@ status_code_t gc_execute_block (char *block) // same way. If there is an explicit/implicit axis command, XYZ words are always used and are // are removed at the end of error-checking. + // [0. User defined M commands ]: + if(command_words.M10 && gc_block.user_mcode) { + + user_words.mask = gc_block.words.mask; + if((int_value = (uint_fast16_t)hal.user_mcode.validate(&gc_block, &gc_block.words))) + FAIL((status_code_t)int_value); + user_words.mask ^= gc_block.words.mask; // Flag "taken" words for execution + + if(user_words.i) + ijk_words.i = Off; + if(user_words.j) + ijk_words.j = Off; + if(user_words.k) + ijk_words.k = Off; + + axis_words.mask = 0; + } + // [1. Comments ]: MSG's may be supported by driver layer. Comment handling performed by protocol. // [2. Set feed rate mode ]: G93 F word missing with G1,G2/3 active, implicitly or explicitly. Feed rate @@ -1736,7 +1754,7 @@ status_code_t gc_execute_block (char *block) // [4. Set spindle speed and address spindle ]: S or D is negative (done.) if(gc_block.words.$) { - bool single_spindle_only = (gc_block.words.s && !user_words.s) || + bool single_spindle_only = gc_block.words.s || (command_words.G0 && (gc_block.modal.motion == MotionMode_SpindleSynchronized || gc_block.modal.motion == MotionMode_RigidTapping || gc_block.modal.motion == MotionMode_Threading)) || @@ -1779,11 +1797,11 @@ status_code_t gc_execute_block (char *block) gc_state.modal.spindle.rpm_mode = gc_block.modal.spindle.rpm_mode; } - spindle_event = gc_block.words.s && !user_words.s; + spindle_event = gc_block.words.s; - if (!gc_block.words.s) + if(!gc_block.words.s) gc_block.values.s = gc_state.modal.spindle.rpm_mode == SpindleSpeedMode_RPM ? gc_state.spindle.rpm : gc_state.spindle.hal->param->css.max_rpm; - else if(!user_words.s && gc_state.modal.spindle.rpm_mode == SpindleSpeedMode_CSS) { + else if(gc_state.modal.spindle.rpm_mode == SpindleSpeedMode_CSS) { // Unsure what to do about S values when in SpindleSpeedMode_CSS - ignore? For now use it to (re)calculate surface speed. // Reinsert commented out code above if this is removed!! gc_block.values.s *= (gc_block.modal.units_imperial ? MM_PER_INCH * 12.0f : 1000.0f); // convert surface speed to mm/min @@ -1796,9 +1814,9 @@ status_code_t gc_execute_block (char *block) if(set_tool) { // M61 if(!gc_block.words.q) FAIL(Status_GcodeValueWordMissing); - if (floorf(gc_block.values.q) - gc_block.values.q != 0.0f) + if(!isintf(gc_block.values.q)) FAIL(Status_GcodeCommandValueNotInteger); - if ((uint32_t)gc_block.values.q > (grbl.tool_table.n_tools ? grbl.tool_table.n_tools : MAX_TOOL_NUMBER)) + if((uint32_t)gc_block.values.q > (grbl.tool_table.n_tools ? grbl.tool_table.n_tools : MAX_TOOL_NUMBER)) FAIL(Status_GcodeIllegalToolTableEntry); gc_block.values.t = (uint32_t)gc_block.values.q; @@ -1815,7 +1833,7 @@ status_code_t gc_execute_block (char *block) } } #endif - } else if (!gc_block.words.t) + } else if(!gc_block.words.t) gc_block.values.t = gc_state.tool_pending; if(command_words.M10 && port_command) { @@ -1952,15 +1970,6 @@ status_code_t gc_execute_block (char *block) } } - // [9a. User defined M commands ]: - if (command_words.M10 && gc_block.user_mcode) { - user_words.mask = gc_block.words.mask; - if((int_value = (uint_fast16_t)hal.user_mcode.validate(&gc_block, &gc_block.words))) - FAIL((status_code_t)int_value); - user_words.mask ^= gc_block.words.mask; // Flag "taken" words for execution - axis_words.mask = ijk_words.mask = 0; - } - // [10. Dwell ]: P value missing. NOTE: See below. if (gc_block.non_modal_command == NonModal_Dwell) { if (!gc_block.words.p) @@ -3071,7 +3080,7 @@ status_code_t gc_execute_block (char *block) } } - if(!user_words.s && ((gc_state.spindle.rpm != gc_block.values.s) || gc_parser_flags.spindle_force_sync)) { + if(gc_state.spindle.rpm != gc_block.values.s || gc_parser_flags.spindle_force_sync) { if(gc_state.modal.spindle.state.on && !gc_parser_flags.laser_is_motion) { if(gc_block.spindle) { gc_block.spindle->param->rpm = gc_block.values.s; @@ -3215,7 +3224,7 @@ status_code_t gc_execute_block (char *block) bool spindle_ok = false; if(gc_block.spindle) { if(grbl.on_spindle_programmed) - grbl.on_spindle_programmed(gc_block.spindle, gc_block.modal.spindle.state, plan_data.spindle.rpm, gc_block.modal.spindle.rpm_mode); + grbl.on_spindle_programmed(gc_block.spindle, gc_block.modal.spindle.state, plan_data.spindle.rpm, gc_block.modal.spindle.rpm_mode); if((spindle_ok = spindle_sync(gc_block.spindle, gc_block.modal.spindle.state, plan_data.spindle.rpm))) gc_block.spindle->param->state = gc_block.modal.spindle.state; } else { @@ -3224,7 +3233,7 @@ status_code_t gc_execute_block (char *block) if(spindle_is_enabled(--idx)) { spindle_ptrs_t *spindle = spindle_get(idx); if(grbl.on_spindle_programmed) - grbl.on_spindle_programmed(spindle, gc_block.modal.spindle.state, plan_data.spindle.rpm, gc_block.modal.spindle.rpm_mode); + grbl.on_spindle_programmed(spindle, gc_block.modal.spindle.state, plan_data.spindle.rpm, gc_block.modal.spindle.rpm_mode); if(spindle_sync(spindle, gc_block.modal.spindle.state, plan_data.spindle.rpm)) spindle->param->state = gc_block.modal.spindle.state; else diff --git a/grbl.h b/grbl.h index 862dbb0..e4353db 100644 --- a/grbl.h +++ b/grbl.h @@ -42,7 +42,7 @@ #else #define GRBL_VERSION "1.1f" #endif -#define GRBL_BUILD 20240801 +#define GRBL_BUILD 20240812 #define GRBL_URL "https://github.com/grblHAL" diff --git a/modbus.h b/modbus.h index c48a538..9368391 100644 --- a/modbus.h +++ b/modbus.h @@ -25,7 +25,7 @@ #define _MODBUS_H_ #ifndef MODBUS_MAX_ADU_SIZE -#define MODBUS_MAX_ADU_SIZE 10 +#define MODBUS_MAX_ADU_SIZE 12 #endif #ifndef MODBUS_QUEUE_LENGTH #define MODBUS_QUEUE_LENGTH 8 diff --git a/report.c b/report.c index 7a513fc..46e0805 100644 --- a/report.c +++ b/report.c @@ -333,6 +333,16 @@ static void report_help_message (void) hal.stream.write("[HLP:$$ $# $G $I $N $x=val $Nx=line $J=line $SLP $C $X $H $B ~ ! ? ctrl-x]" ASCII_EOL); } +// Prints plugin info. +void report_plugin (const char *name, const char *version) +{ + hal.stream.write("[PLUGIN:"); + hal.stream.write(name); + hal.stream.write(" v"); + hal.stream.write(version); + hal.stream.write("]" ASCII_EOL); +} + static bool report_group_settings (const setting_group_detail_t *groups, const uint_fast8_t n_groups, char *args) { bool found = false; @@ -1088,7 +1098,6 @@ void report_build_info (char *line, bool extended) } } - // Prints the character string line grblHAL has received from the user, which has been pre-parsed, // and has been sent into protocol_execute_line() routine to be executed by grblHAL. void report_echo_line_received (char *line) @@ -1098,6 +1107,15 @@ void report_echo_line_received (char *line) hal.stream.write("]" ASCII_EOL); } +#if N_SYS_SPINDLE == 1 && N_SPINDLE > 1 + +static void report_spindle_num (spindle_info_t *spindle, void *data) +{ + if(spindle->id == *((spindle_id_t *)data)) + hal.stream.write_all(appendbuf(2, "|S:", uitoa((uint32_t)spindle->num))); +} + +#endif // Prints real-time data. This function grabs a real-time snapshot of the stepper subprogram // and the actual location of the CNC machine. Users may change the following function to their @@ -1246,7 +1264,7 @@ void report_realtime_status (void) #elif N_SPINDLE > 1 if(report.spindle_id) - hal.stream.write_all(appendbuf(2, "|S:", uitoa((uint32_t)spindle_0->id))); + spindle_enumerate_spindles(report_spindle_num, &spindle_0->id); #endif if(settings.status_report.pin_state) { @@ -2516,9 +2534,56 @@ static void report_spindle (spindle_info_t *spindle, void *data) } } +#if N_SPINDLE > 1 + +typedef struct { + uint32_t idx; + uint32_t n_spindles; + spindle_info_t *spindles; +} spindle_rdata_t; + +static void get_spindles (spindle_info_t *spindle, void *data) +{ + memcpy(&((spindle_rdata_t *)data)->spindles[((spindle_rdata_t *)data)->idx++], spindle, sizeof(spindle_info_t)); +} + +static int cmp_spindles (const void *a, const void *b) +{ + uint32_t key_a = ((spindle_info_t *)a)->num == -1 ? ((((spindle_info_t *)a)->hal->type + 1) << 8) | ((spindle_info_t *)a)->id : ((spindle_info_t *)a)->num, + key_b = ((spindle_info_t *)b)->num == -1 ? ((((spindle_info_t *)b)->hal->type + 1) << 8) | ((spindle_info_t *)b)->id : ((spindle_info_t *)b)->num; + + return key_a - key_b; +} + +#endif + status_code_t report_spindles (bool machine_readable) { - if(!spindle_enumerate_spindles(report_spindle, (void *)machine_readable) && !machine_readable) + bool has_spindles; + +#if N_SPINDLE > 1 + + spindle_rdata_t spindle_data = {0}; + + if((spindle_data.spindles = malloc(N_SPINDLE * sizeof(spindle_info_t)))) { + + has_spindles = spindle_enumerate_spindles(get_spindles, &spindle_data); + + spindle_data.n_spindles = spindle_data.idx; + + qsort(spindle_data.spindles, spindle_data.n_spindles, sizeof(spindle_info_t), cmp_spindles); + for(spindle_data.idx = 0; spindle_data.idx < spindle_data.n_spindles; spindle_data.idx++) + report_spindle(&spindle_data.spindles[spindle_data.idx], (void *)machine_readable); + + free(spindle_data.spindles); + + } else + +#endif + + has_spindles = spindle_enumerate_spindles(report_spindle, (void *)machine_readable); + + if(!has_spindles && !machine_readable) hal.stream.write("No spindles registered." ASCII_EOL); return Status_OK; diff --git a/report.h b/report.h index b5b1b3a..1a7d99f 100644 --- a/report.h +++ b/report.h @@ -52,6 +52,8 @@ void report_warning (void *message); // Prints Grbl help. status_code_t report_help (char *args); +void report_plugin (const char *name, const char *version); + // Prints Grbl settings void report_grbl_settings (bool all, void *data); diff --git a/spindle_control.c b/spindle_control.c index 769c4f1..d2d19ec 100644 --- a/spindle_control.c +++ b/spindle_control.c @@ -51,6 +51,7 @@ typedef struct { static uint8_t n_spindle = 0; static spindle_sys_t sys_spindle[N_SYS_SPINDLE] = {0}; static spindle_reg_t spindles[N_SPINDLE] = {0}, *pwm_spindle = NULL; +static const spindle_data_ptrs_t *encoder; /*! \internal \brief Activates and registers a spindle as enabled with a specific spindle number. \param spindle_id spindle id of spindle to activate as a \ref spindle_id_t. @@ -96,20 +97,6 @@ static bool spindle_activate (spindle_id_t spindle_id, spindle_num_t spindle_num memcpy(&spindle_hal, &spindle->hal, sizeof(spindle_ptrs_t)); - if(spindle->cfg->get_data == NULL) { - if(settings.offset_lock.encoder_spindle == spindle_id) { - spindle_hal.get_data = hal.spindle_data.get; - spindle_hal.reset_data = hal.spindle_data.reset; - if(!spindle->cfg->cap.at_speed) - spindle_hal.cap.at_speed = !!spindle_hal.get_data; - } else { - spindle_hal.get_data = NULL; - spindle_hal.reset_data = NULL; - if(!spindle->cfg->cap.at_speed) - spindle_hal.cap.at_speed = Off; - } - } - spindle_hal.cap.laser &= settings.mode == Mode_Laser; if(grbl.on_spindle_select) @@ -305,22 +292,68 @@ uint8_t spindle_get_count (void) static spindle_num_t spindle_get_num (spindle_id_t spindle_id) { - uint_fast8_t idx = N_SPINDLE_SELECTABLE; - spindle_num_t spindle_num = -1; + spindle_num_t spindle_num; - const setting_detail_t *setting; + if((spindle_num = spindle_get_count() == 1 ? 0 : -1) == -1) { - do { - idx--; - if((setting = setting_get_details(idx == 0 ? Setting_SpindleType : (setting_id_t)(Setting_SpindleEnable0 + idx), NULL))) { - if(setting_get_int_value(setting, 0) == spindle_id) - spindle_num = idx; - } - } while(idx && spindle_num == -1); + const setting_detail_t *setting; + uint_fast8_t idx = N_SPINDLE_SELECTABLE; + + do { + idx--; + if((setting = setting_get_details(idx == 0 ? Setting_SpindleType : (setting_id_t)(Setting_SpindleEnable0 + idx), NULL))) { + if(setting_get_int_value(setting, 0) == spindle_id) + spindle_num = idx; + } + } while(idx && spindle_num == -1); + } return spindle_num; } +void spindle_bind_encoder (const spindle_data_ptrs_t *encoder_data) +{ + uint_fast8_t idx; + spindle_ptrs_t *spindle; + spindle_num_t spindle_num; + + encoder = encoder_data; + + for(idx = 0; idx < n_spindle; idx++) { + + spindle = spindle_get((spindle_num = spindle_get_num(idx))); + + if(encoder_data && spindle_num == settings.offset_lock.encoder_spindle) { + spindles[idx].hal.get_data = encoder_data->get; + spindles[idx].hal.reset_data = encoder_data->reset; + spindles[idx].hal.cap.at_speed = spindles[idx].hal.cap.variable; + } else { + spindles[idx].hal.get_data = spindles[idx].cfg->get_data; + spindles[idx].hal.reset_data = spindles[idx].cfg->reset_data; + spindles[idx].hal.cap.at_speed = spindles[idx].cfg->cap.at_speed; + } + + if(spindle) { + spindle->get_data = spindles[idx].hal.get_data; + spindle->reset_data = spindles[idx].hal.reset_data; + spindle->cap.at_speed = spindles[idx].hal.cap.at_speed; + } + } +} + +bool spindle_set_at_speed_range (spindle_ptrs_t *spindle, spindle_data_t *spindle_data, float rpm) +{ + spindle_data->rpm_programmed = rpm; + spindle_data->state_programmed.at_speed = false; + + if((spindle_data->at_speed_enabled = spindle->at_speed_tolerance > 0.0f)) { + spindle_data->rpm_low_limit = rpm * (1.0f - (spindle->at_speed_tolerance / 100.0f)); + spindle_data->rpm_high_limit = rpm * (1.0f + (spindle->at_speed_tolerance / 100.0f)); + } + + return spindle_data->at_speed_enabled; +} + /*! \brief Enumerate registered spindles by calling a callback function for each of them. \param callback pointer to a \ref spindle_enumerate_callback_ptr type function. \param data pointer to optional data to pass to the callback function. @@ -555,7 +588,7 @@ bool spindle_sync (spindle_ptrs_t *spindle, spindle_state_t state, float rpm) if (!(ok = state_get() == STATE_CHECK_MODE)) { - bool at_speed = !state.on || !spindle->cap.at_speed || settings.spindle.at_speed_tolerance <= 0.0f; + bool at_speed = !state.on || !spindle->cap.at_speed || spindle->at_speed_tolerance <= 0.0f; // Empty planner buffer to ensure spindle is set when programmed. if((ok = protocol_buffer_synchronize()) && set_state(spindle, state, rpm) && !at_speed) { @@ -595,7 +628,7 @@ bool spindle_restore (spindle_ptrs_t *spindle, spindle_state_t state, float rpm) if(state.on) { if((ok = !spindle->cap.at_speed)) ok = delay_sec(settings.safety_door.spindle_on_delay, DelayMode_SysSuspend); - else if((ok == (settings.spindle.at_speed_tolerance <= 0.0f))) { + else if((ok == (spindle->at_speed_tolerance <= 0.0f))) { float delay = 0.0f; while(!(ok = spindle->get_state(spindle).at_speed)) { if(!(ok = delay_sec(0.1f, DelayMode_SysSuspend))) @@ -624,13 +657,7 @@ float spindle_set_rpm (spindle_ptrs_t *spindle, float rpm, override_t override_p if(override_pct != 100) rpm *= 0.01f * (float)override_pct; // Scale RPM by override value. - // Apply RPM limits - if (rpm <= 0.0f) // TODO: remove this test? - rpm = 0.0f; - else if (rpm > spindle->rpm_max) - rpm = spindle->rpm_max; - else if (rpm < spindle->rpm_min) - rpm = spindle->rpm_min; + rpm = rpm <= 0.0f ? 0.0f : constrain(rpm, spindle->rpm_min, spindle->rpm_max); spindle->param->rpm_overridden = rpm; spindle->param->override_pct = override_pct; @@ -756,6 +783,7 @@ bool spindle_precompute_pwm_values (spindle_ptrs_t *spindle, spindle_pwm_t *pwm_ pwm_data->settings = settings; spindle->rpm_min = pwm_data->rpm_min = settings->rpm_min; spindle->rpm_max = settings->rpm_max; + spindle->at_speed_tolerance = settings->at_speed_tolerance; spindle->cap.rpm_range_locked = On; if((spindle->cap.variable = !settings->flags.pwm_disable && spindle->rpm_max > spindle->rpm_min)) { diff --git a/spindle_control.h b/spindle_control.h index 889a9e8..471762e 100644 --- a/spindle_control.h +++ b/spindle_control.h @@ -98,6 +98,7 @@ typedef struct { uint32_t index_count; uint32_t pulse_count; uint32_t error_count; + bool at_speed_enabled; spindle_state_t state_programmed; } spindle_data_t; @@ -208,14 +209,14 @@ typedef struct { float rpm_max; float rpm_min; float pwm_freq; - float pwm_period; // currently unused + float pwm_period; // currently unused float pwm_off_value; float pwm_min_value; float pwm_max_value; - float at_speed_tolerance; + float at_speed_tolerance; //!< Tolerance in percent of programmed speed. pwm_piece_t pwm_piece[SPINDLE_NPWM_PIECES]; pid_values_t pid; - uint16_t ppr; // Spindle encoder pulses per revolution + uint16_t ppr; //!< Spindle encoder pulses per revolution (PPR). spindle_state_t invert; spindle_settings_flags_t flags; } spindle_settings_t; @@ -273,6 +274,7 @@ struct spindle_ptrs { uint_fast16_t pwm_off_value; //!< Value for switching PWM signal off. float rpm_min; //!< Minimum spindle RPM. float rpm_max; //!< Maximum spindle RPM. + float at_speed_tolerance; //!< Tolerance in percent of programmed speed. spindle_config_ptr config; //!< Optional handler for configuring the spindle. spindle_set_state_ptr set_state; //!< Handler for setting spindle state. spindle_get_state_ptr get_state; //!< Handler for getting spindle state. @@ -350,6 +352,15 @@ void spindle_all_off (void); // The following functions are not called by the core, may be called by driver code. // +#define spindle_validate_at_speed(d, r) { d.rpm = r; d.state_programmed.at_speed = !d.at_speed_enabled || (d.rpm >= d.rpm_low_limit && d.rpm <= d.rpm_high_limit); } +/* +__attribute__((always_inline)) static inline void spindle_validate_at_speed (spindle_data_t *spindle_data, float rpm) +{ + spindle_data->rpm = rpm; + spindle_data->state_programmed.at_speed = !spindle_data->at_speed_enabled || (spindle_data->rpm >= spindle_data->rpm_low_limit && spindle_data->rpm <= spindle_data->rpm_high_limit); +} +*/ + bool spindle_precompute_pwm_values (spindle_ptrs_t *spindle, spindle_pwm_t *pwm_data, spindle_settings_t *settings, uint32_t clock_hz); spindle_id_t spindle_register (const spindle_ptrs_t *spindle, const char *name); @@ -364,6 +375,9 @@ spindle_cap_t spindle_get_caps (bool active); void spindle_update_caps (spindle_ptrs_t *spindle, spindle_pwm_t *pwm_caps); +void spindle_bind_encoder (const spindle_data_ptrs_t *encoder_data); + +bool spindle_set_at_speed_range (spindle_ptrs_t *spindle, spindle_data_t *spindle_data, float rpm); spindle_ptrs_t *spindle_get_hal (spindle_id_t spindle_id, spindle_hal_t hal); diff --git a/stepper2.c b/stepper2.c index 894b60b..ea2c082 100644 --- a/stepper2.c +++ b/stepper2.c @@ -272,6 +272,15 @@ st2_motor_t *st2_motor_init (uint_fast8_t axis_idx, bool is_spindle) return motor; } +/*! \brief Get current speed (RPM). +\param motor pointer to a \a st2_motor structure. +\returns current speed in RPM. +*/ +float st2_get_speed (st2_motor_t *motor) +{ + return motor->state == State_Idle ? 0.0f : 60.0f / ((float)motor->delay * settings.axis[motor->idx].steps_per_mm / 1000000.0f); +} + /*! \brief Set speed. Change speed of a running motor. Typically used for motors bound as a spindle. diff --git a/stepper2.h b/stepper2.h index 8f3dc01..d5984ca 100644 --- a/stepper2.h +++ b/stepper2.h @@ -33,6 +33,7 @@ typedef struct st2_motor st2_motor_t; st2_motor_t *st2_motor_init (uint_fast8_t axis_idx, bool is_spindle); bool st2_motor_bind_spindle (uint_fast8_t axis_idx); +float st2_get_speed (st2_motor_t *motor); float st2_motor_set_speed (st2_motor_t *motor, float speed); bool st2_motor_move (st2_motor_t *motor, const float move, const float speed, position_t type); bool st2_motor_run (st2_motor_t *motor);