From 596510965f512bf896e813559dff7e163b85a469 Mon Sep 17 00:00:00 2001 From: Stefan de Bruijn Date: Thu, 20 May 2021 21:45:11 +0200 Subject: [PATCH] Fixed most ->get calls in motors, limits, steppers. --- Grbl_Esp32/src/Jog.cpp | 1 + Grbl_Esp32/src/Limits.cpp | 64 +++++++++++++------ Grbl_Esp32/src/MachineConfig.cpp | 33 +++++----- Grbl_Esp32/src/MachineConfig.h | 42 ++++++++---- Grbl_Esp32/src/MotionControl.cpp | 8 ++- Grbl_Esp32/src/Motors/Dynamixel2.cpp | 2 +- Grbl_Esp32/src/Motors/Dynamixel2.h | 4 +- Grbl_Esp32/src/Motors/RcServo.cpp | 25 ++------ Grbl_Esp32/src/Motors/RcServo.h | 8 +-- Grbl_Esp32/src/Motors/StandardStepper.cpp | 3 +- Grbl_Esp32/src/Motors/StepStick.h | 1 + Grbl_Esp32/src/Motors/TrinamicDriver.cpp | 24 +++---- Grbl_Esp32/src/Motors/TrinamicDriver.h | 2 + Grbl_Esp32/src/Motors/TrinamicUartDriver.h | 10 +-- .../src/Motors/TrinamicUartDriverClass.cpp | 26 ++++---- Grbl_Esp32/src/Stepper.cpp | 9 +-- 16 files changed, 148 insertions(+), 114 deletions(-) diff --git a/Grbl_Esp32/src/Jog.cpp b/Grbl_Esp32/src/Jog.cpp index 03474c7a..fe428e74 100644 --- a/Grbl_Esp32/src/Jog.cpp +++ b/Grbl_Esp32/src/Jog.cpp @@ -34,6 +34,7 @@ Error jog_execute(plan_line_data_t* pl_data, parser_block_t* gc_block, bool* can #ifdef USE_LINE_NUMBERS pl_data->line_number = gc_block->values.n; #endif + if (soft_limits->get()) { if (limitsCheckTravel(gc_block->values.xyz)) { return Error::TravelExceeded; diff --git a/Grbl_Esp32/src/Limits.cpp b/Grbl_Esp32/src/Limits.cpp index a5786c36..ff8c24fb 100644 --- a/Grbl_Esp32/src/Limits.cpp +++ b/Grbl_Esp32/src/Limits.cpp @@ -118,7 +118,7 @@ void limits_go_home(uint8_t cycle_mask) { } // Set search mode with approach at seek rate to quickly engage the specified cycle_mask limit switches. bool approach = true; - float homing_rate = homing_seek_rate->get(); + float homing_rate = homing_seek_rate->get(); // TODO FIXME: in YAML this is per-axis. We should move this into the loop. uint8_t n_active_axis; AxisMask limit_state, axislock; do { @@ -127,6 +127,8 @@ void limits_go_home(uint8_t cycle_mask) { axislock = 0; n_active_axis = 0; for (uint8_t idx = 0; idx < n_axis; idx++) { + auto axisConfig = MachineConfig::instance()->_axes->_axis[idx]; + // Set target location for active axes and setup computation for homing rate. if (bit_istrue(cycle_mask, bit(idx))) { n_active_axis++; @@ -228,6 +230,7 @@ void limits_go_home(uint8_t cycle_mask) { homing_rate = homing_seek_rate->get(); } } while (n_cycle-- > 0); + // The active cycle axes should now be homed and machine limits have been located. By // default, Grbl defines machine space as all negative, as do most CNCs. Since limit switches // can be on either side of an axes, check and set axes machine zero appropriately. Also, @@ -239,10 +242,15 @@ void limits_go_home(uint8_t cycle_mask) { auto mask = homing_dir_mask->get(); auto pulloff = homing_pulloff->get(); for (uint8_t idx = 0; idx < n_axis; idx++) { - auto steps = axis_settings[idx]->steps_per_mm->get(); + Axis* axisConf = MachineConfig::instance()->_axes->_axis[idx]; + auto homing = axisConf->_homing; + auto steps = axisConf->_stepsPerMm; if (cycle_mask & bit(idx)) { - float travel = axis_settings[idx]->max_travel->get(); - float mpos = axis_settings[idx]->home_mpos->get(); + float travel = axisConf->_maxTravel; + float mpos = 0; + if (axisConf->_homing != nullptr) { + mpos = axisConf->_homing->_mpos; + } if (bit_istrue(homing_dir_mask->get(), bit(idx))) { sys_position[idx] = (mpos + pulloff) * steps; @@ -264,8 +272,10 @@ void limits_init() { auto n_axis = MachineConfig::instance()->_axes->_numberAxis; for (int axis = 0; axis < n_axis; axis++) { for (int gang_index = 0; gang_index < 2; gang_index++) { - Pin pin; - if ((pin = LimitPins[axis][gang_index]->get()) != Pin::UNDEFINED) { + auto gangConfig = MachineConfig::instance()->_axes->_axis[axis]->_gangs[gang_index]; + if (gangConfig->_endstops != nullptr && !gangConfig->_endstops->_dual.undefined()) { + Pin& pin = gangConfig->_endstops->_dual; + #ifndef DISABLE_LIMIT_PIN_PULL_UP if (pin.capabilities().has(Pins::PinCapabilities::PullUp)) { mode = mode | Pin::Attr::PullUp; @@ -274,7 +284,7 @@ void limits_init() { pin.setAttr(mode); limit_mask |= bit(axis); - if (hard_limits->get()) { + if (gangConfig->_endstops->_hardLimits) { pin.attachInterrupt(isr_limit_switches, CHANGE, nullptr); } else { pin.detachInterrupt(); @@ -305,8 +315,9 @@ void limits_disable() { auto n_axis = MachineConfig::instance()->_axes->_numberAxis; for (int axis = 0; axis < n_axis; axis++) { for (int gang_index = 0; gang_index < 2; gang_index++) { - Pin pin = LimitPins[axis][gang_index]->get(); - if (pin != Pin::UNDEFINED) { + auto gangConfig = MachineConfig::instance()->_axes->_axis[axis]->_gangs[gang_index]; + if (gangConfig->_endstops != nullptr && !gangConfig->_endstops->_dual.undefined()) { + Pin& pin = gangConfig->_endstops->_dual; pin.detachInterrupt(); } } @@ -321,13 +332,10 @@ AxisMask limits_get_state() { auto n_axis = MachineConfig::instance()->_axes->_numberAxis; for (int axis = 0; axis < n_axis; axis++) { for (int gang_index = 0; gang_index < 2; gang_index++) { - Pin pin = LimitPins[axis][gang_index]->get(); - if (pin != Pin::UNDEFINED) { - if (limit_invert->get()) { - pinMask |= (!pin.read() << axis); - } else { - pinMask |= (pin.read() << axis); - } + auto gangConfig = MachineConfig::instance()->_axes->_axis[axis]->_gangs[gang_index]; + if (gangConfig->_endstops != nullptr && !gangConfig->_endstops->_dual.undefined()) { + Pin& pin = gangConfig->_endstops->_dual; + pinMask |= (pin.read() << axis); } } } @@ -385,15 +393,21 @@ void limitCheckTask(void* pvParameters) { } float limitsMaxPosition(uint8_t axis) { - float mpos = axis_settings[axis]->home_mpos->get(); + auto axisConfig = MachineConfig::instance()->_axes->_axis[axis]; + auto homing = axisConfig->_homing; + float mpos = (homing != nullptr) ? homing->_mpos : 0; + auto maxtravel = axisConfig->_maxTravel; - return bitnum_istrue(homing_dir_mask->get(), axis) ? mpos + axis_settings[axis]->max_travel->get() : mpos; + return (homing == nullptr || homing->_positiveDirection) ? mpos + maxtravel : mpos; } float limitsMinPosition(uint8_t axis) { - float mpos = axis_settings[axis]->home_mpos->get(); + auto axisConfig = MachineConfig::instance()->_axes->_axis[axis]; + auto homing = axisConfig->_homing; + float mpos = (homing != nullptr) ? homing->_mpos : 0; + auto maxtravel = axisConfig->_maxTravel; - return bitnum_istrue(homing_dir_mask->get(), axis) ? mpos : mpos - axis_settings[axis]->max_travel->get(); + return (homing == nullptr || homing->_positiveDirection) ? mpos : mpos - maxtravel; } // Checks and reports if target array exceeds machine travel limits. @@ -413,7 +427,15 @@ bool __attribute__((weak)) limitsCheckTravel(float* target) { } bool limitsSwitchDefined(uint8_t axis, uint8_t gang_index) { - return (LimitPins[axis][gang_index]->get() != Pin::UNDEFINED); + auto gangConfig = MachineConfig::instance()->_axes->_axis[axis]->_gangs[gang_index]; + + if (gangConfig->_endstops != nullptr) + { + return !gangConfig->_endstops->_dual.undefined(); + } + else { + return false; + } } bool __attribute__((weak)) user_defined_homing(uint8_t cycle_mask) { diff --git a/Grbl_Esp32/src/MachineConfig.cpp b/Grbl_Esp32/src/MachineConfig.cpp index b4f922fe..394cdaef 100644 --- a/Grbl_Esp32/src/MachineConfig.cpp +++ b/Grbl_Esp32/src/MachineConfig.cpp @@ -18,18 +18,18 @@ // TODO FIXME: Split this file up into several files, perhaps put it in some folder and namespace Machine? void Endstops::validate() const { - if (!_dual.undefined()) { - Assert(_positive.undefined(), "If dual endstops are defined, you cannot also define positive and negative endstops"); - Assert(_negative.undefined(), "If dual endstops are defined, you cannot also define positive and negative endstops"); - } - if (!_positive.undefined() || !_negative.undefined()) { - Assert(_positive.undefined(), "If positive or negative endstops are defined, you cannot also define dual endstops"); - } +// if (!_dual.undefined()) { +// Assert(_positive.undefined(), "If dual endstops are defined, you cannot also define positive and negative endstops"); +// Assert(_negative.undefined(), "If dual endstops are defined, you cannot also define positive and negative endstops"); +// } +// if (!_positive.undefined() || !_negative.undefined()) { +// Assert(_positive.undefined(), "If positive or negative endstops are defined, you cannot also define dual endstops"); +// } } void Endstops::handle(Configuration::HandlerBase& handler) { - handler.handle("positive", _positive); - handler.handle("negative", _negative); +// handler.handle("positive", _positive); +// handler.handle("negative", _negative); handler.handle("dual", _dual); handler.handle("hard_limits", _hardLimits); } @@ -98,6 +98,7 @@ Axes::Axes() : _axis() { void Axes::init() { grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Init Motors"); + // TODO FIXME! If use_stepstick, this should be in stepstick? #ifdef USE_STEPSTICK grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Using StepStick Mode"); @@ -107,9 +108,10 @@ void Axes::init() { } #endif - if (SteppersDisablePin->get() != Pin::UNDEFINED) { - SteppersDisablePin->get().setAttr(Pin::Attr::Output); // global motor enable pin - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Global stepper disable pin:%s", SteppersDisablePin->get().name()); + if (!_sharedStepperDisable.undefined()) + { + _sharedStepperDisable.setAttr(Pin::Attr::Output); + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Shared stepper disable pin:%s", _sharedStepperDisable.name()); } // certain motors need features to be turned on. Check them here @@ -147,10 +149,7 @@ void Axes::set_disable(bool disable) { } // invert only inverts the global stepper disable pin. - if (step_enable_invert->get()) { - disable = !disable; // Apply pin invert. - } - SteppersDisablePin->get().write(disable); + _sharedStepperDisable.write(disable); } void Axes::read_settings() { @@ -290,6 +289,7 @@ void Axes::validate() const {} void Axes::handle(Configuration::HandlerBase& handler) { handler.handle("number_axis", _numberAxis); + handler.handle("shared_stepper_disable", _sharedStepperDisable); const char* allAxis = "xyzabc"; @@ -360,6 +360,7 @@ void MachineConfig::handle(Configuration::HandlerBase& handler) { handler.handle("coolant", _coolant); handler.handle("probe", _probe); handler.handle("laser_mode", _laserMode); + handler.handle("pulse_microseconds", _pulseMicroSeconds); } void MachineConfig::afterParse() { diff --git a/Grbl_Esp32/src/MachineConfig.h b/Grbl_Esp32/src/MachineConfig.h index 424c6fca..f2eec039 100644 --- a/Grbl_Esp32/src/MachineConfig.h +++ b/Grbl_Esp32/src/MachineConfig.h @@ -14,14 +14,14 @@ namespace Motors { } class Endstops : public Configuration::Configurable { - Pin _positive; - Pin _negative; - Pin _dual; - bool _hardLimits = true; - public: Endstops() = default; + // Pin _positive; + // Pin _negative; + Pin _dual; + bool _hardLimits = true; + // Configuration system helpers: void validate() const override; void handle(Configuration::HandlerBase& handler) override; @@ -102,6 +102,8 @@ class Axes : public Configuration::Configurable { public: Axes(); + Pin _sharedStepperDisable; + int _numberAxis = 3; Axis* _axis[MAX_NUMBER_AXIS]; @@ -110,6 +112,15 @@ public: size_t findAxisIndex(const Motors::Motor* const motor) const; size_t findAxisGanged(const Motors::Motor* const motor) const; + inline bool hasSoftLimits() const { + for (int i = 0; i < _numberAxis; ++i) { + if (_axis[i]->_softLimits) { + return true; + } + } + return false; + } + // These are used for setup and to talk to the motors as a group. void init(); void read_settings(); // more like 'after read settings, before init'. Oh well... @@ -315,14 +326,19 @@ public: class MachineConfig : public Configuration::Configurable { public: - MachineConfig() = default; - Axes* _axes = nullptr; - SPIBus* _spi = nullptr; - I2SOBus* _i2so = nullptr; - CoolantControl* _coolant = nullptr; - Probe* _probe = nullptr; - bool _laserMode = false; - Communications* _comms = nullptr; + MachineConfig() = default; + + Axes* _axes = nullptr; + SPIBus* _spi = nullptr; + I2SOBus* _i2so = nullptr; + CoolantControl* _coolant = nullptr; + Probe* _probe = nullptr; + Communications* _comms = nullptr; + + bool _laserMode = false; + int _pulseMicroSeconds = 3; + float _arcTolerance = 0.002; + float _junctionDeviation = 0.01; static MachineConfig*& instance() { static MachineConfig* instance = nullptr; diff --git a/Grbl_Esp32/src/MotionControl.cpp b/Grbl_Esp32/src/MotionControl.cpp index 09147fe7..910b8a9d 100644 --- a/Grbl_Esp32/src/MotionControl.cpp +++ b/Grbl_Esp32/src/MotionControl.cpp @@ -49,7 +49,8 @@ bool mc_line(float* target, plan_line_data_t* pl_data) { // If enabled, check for soft limit violations. Placed here all line motions are picked up // from everywhere in Grbl. - if (soft_limits->get()) { + bool hasSoftLimits = MachineConfig::instance()->_axes->hasSoftLimits(); + if (hasSoftLimits) { // NOTE: Block jog state. Jogging is a special case and soft limits are handled independently. if (sys.state != State::Jog) { limits_soft_check(target); @@ -154,11 +155,14 @@ void mc_arc(float* target, angular_travel += 2 * M_PI; } } + + auto mconfig = MachineConfig::instance(); + // NOTE: Segment end points are on the arc, which can lead to the arc diameter being smaller by up to // (2x) arc_tolerance. For 99% of users, this is just fine. If a different arc segment fit // is desired, i.e. least-squares, midpoint on arc, just change the mm_per_arc_segment calculation. // For the intended uses of Grbl, this value shouldn't exceed 2000 for the strictest of cases. - uint16_t segments = floor(fabs(0.5 * angular_travel * radius) / sqrt(arc_tolerance->get() * (2 * radius - arc_tolerance->get()))); + uint16_t segments = floor(fabs(0.5 * angular_travel * radius) / sqrt(mconfig->_arcTolerance * (2 * radius - mconfig->_arcTolerance))); if (segments) { // Multiply inverse feed_rate to compensate for the fact that this movement is approximated // by a number of discrete segments. The inverse feed_rate should be correct for the sum of diff --git a/Grbl_Esp32/src/Motors/Dynamixel2.cpp b/Grbl_Esp32/src/Motors/Dynamixel2.cpp index c14fee82..7828ce0f 100644 --- a/Grbl_Esp32/src/Motors/Dynamixel2.cpp +++ b/Grbl_Esp32/src/Motors/Dynamixel2.cpp @@ -109,7 +109,7 @@ namespace Motors { _dxl_count_min = DXL_COUNT_MIN; _dxl_count_max = DXL_COUNT_MAX; - if (bitnum_istrue(dir_invert_mask->get(), _axis_index)) { // normal direction + if (_invert_direction) { // normal direction swap(_dxl_count_min, _dxl_count_min); } } diff --git a/Grbl_Esp32/src/Motors/Dynamixel2.h b/Grbl_Esp32/src/Motors/Dynamixel2.h index 948f8043..bc2262f0 100644 --- a/Grbl_Esp32/src/Motors/Dynamixel2.h +++ b/Grbl_Esp32/src/Motors/Dynamixel2.h @@ -108,6 +108,7 @@ namespace Motors { Pin _rts_pin; uart_port_t _uart_num; int _axis_index; + bool _invert_direction = false; bool _disabled; bool _has_errors; @@ -137,7 +138,8 @@ namespace Motors { handler.handle("tx", _tx_pin); handler.handle("rx", _rx_pin); handler.handle("rts", _rts_pin); - + handler.handle("invert_direction", _invert_direction); + int id = _id; handler.handle("id", id); _id = id; diff --git a/Grbl_Esp32/src/Motors/RcServo.cpp b/Grbl_Esp32/src/Motors/RcServo.cpp index 73974250..e88b6021 100644 --- a/Grbl_Esp32/src/Motors/RcServo.cpp +++ b/Grbl_Esp32/src/Motors/RcServo.cpp @@ -39,19 +39,6 @@ namespace Motors { auto pwmNative = _pwm_pin.getNative(Pin::Capabilities::PWM); -#ifdef LATER - char* setting_cal_min = (char*)malloc(20); - sprintf(setting_cal_min, "%c/RcServo/Cal/Min", report_get_axis_letter(_axis_index)); // - rc_servo_cal_min = new FloatSetting(EXTENDED, WG, NULL, setting_cal_min, 1.0, 0.5, 2.0); - - char* setting_cal_max = (char*)malloc(20); - sprintf(setting_cal_max, "%c/RcServo/Cal/Max", report_get_axis_letter(_axis_index)); // - rc_servo_cal_max = new FloatSetting(EXTENDED, WG, NULL, setting_cal_max, 1.0, 0.5, 2.0); - - rc_servo_cal_min->load(); - rc_servo_cal_max->load(); -#endif - read_settings(); _channel_num = sys_get_next_PWM_chan_num(); ledcSetup(_channel_num, SERVO_PULSE_FREQ, SERVO_PULSE_RES_BITS); @@ -127,17 +114,15 @@ namespace Motors { } void RcServo::read_settings() { -#ifdef LATER - if (bitnum_istrue(dir_invert_mask->get(), _axis_index)) { + if (_invert_direction) { // swap the pwm values - _pwm_pulse_min = SERVO_MAX_PULSE * (1.0 + (1.0 - rc_servo_cal_min->get())); - _pwm_pulse_max = SERVO_MIN_PULSE * (1.0 + (1.0 - rc_servo_cal_max->get())); + _pwm_pulse_min = SERVO_MAX_PULSE * (1.0 + (1.0 - _cal_min)); + _pwm_pulse_max = SERVO_MIN_PULSE * (1.0 + (1.0 - _cal_max)); } else { - _pwm_pulse_min = SERVO_MIN_PULSE * rc_servo_cal_min->get(); - _pwm_pulse_max = SERVO_MAX_PULSE * rc_servo_cal_max->get(); + _pwm_pulse_min = SERVO_MIN_PULSE * _cal_min; + _pwm_pulse_max = SERVO_MAX_PULSE * _cal_max; } -#endif } // Configuration registration diff --git a/Grbl_Esp32/src/Motors/RcServo.h b/Grbl_Esp32/src/Motors/RcServo.h index aaa7021f..f2eaa20e 100644 --- a/Grbl_Esp32/src/Motors/RcServo.h +++ b/Grbl_Esp32/src/Motors/RcServo.h @@ -34,8 +34,8 @@ namespace Motors { Pin _pwm_pin; uint8_t _channel_num; uint32_t _current_pwm_duty; - - float _homing_position; + float _homing_position; + bool _invert_direction = false; float _pwm_pulse_min; float _pwm_pulse_max; @@ -60,9 +60,7 @@ namespace Motors { void _write_pwm(uint32_t duty); // Configuration handlers: - void validate() const override { - Assert(!_pwm_pin.undefined(), "PWM pin should be configured."); - } + void validate() const override { Assert(!_pwm_pin.undefined(), "PWM pin should be configured."); } void handle(Configuration::HandlerBase& handler) override { handler.handle("pwm", _pwm_pin); diff --git a/Grbl_Esp32/src/Motors/StandardStepper.cpp b/Grbl_Esp32/src/Motors/StandardStepper.cpp index d29efb09..6b4fc618 100644 --- a/Grbl_Esp32/src/Motors/StandardStepper.cpp +++ b/Grbl_Esp32/src/Motors/StandardStepper.cpp @@ -22,6 +22,7 @@ */ #include "StandardStepper.h" +#include "../MachineConfig.h" namespace Motors { rmt_item32_t StandardStepper::rmtItem[2]; @@ -69,7 +70,7 @@ namespace Motors { auto stepPulseDelay = _direction_delay_ms; rmtItem[0].duration0 = stepPulseDelay < 1 ? 1 : stepPulseDelay * 4; - rmtItem[0].duration1 = 4 * pulse_microseconds->get(); + rmtItem[0].duration1 = 4 * MachineConfig::instance()->_pulseMicroSeconds; rmtItem[1].duration0 = 0; rmtItem[1].duration1 = 0; diff --git a/Grbl_Esp32/src/Motors/StepStick.h b/Grbl_Esp32/src/Motors/StepStick.h index d699a536..cdb7abeb 100644 --- a/Grbl_Esp32/src/Motors/StepStick.h +++ b/Grbl_Esp32/src/Motors/StepStick.h @@ -8,6 +8,7 @@ namespace Motors { Pin _MS1; Pin _MS2; Pin _MS3; + public: StepStick() = default; diff --git a/Grbl_Esp32/src/Motors/TrinamicDriver.cpp b/Grbl_Esp32/src/Motors/TrinamicDriver.cpp index 3b609d9f..03d89352 100644 --- a/Grbl_Esp32/src/Motors/TrinamicDriver.cpp +++ b/Grbl_Esp32/src/Motors/TrinamicDriver.cpp @@ -111,7 +111,7 @@ namespace Motors { xTaskCreatePinnedToCore(readSgTask, // task "readSgTask", // name for task 4096, // size of task stack - NULL, // parameters + this, // parameters 1, // priority NULL, SUPPORT_TASK_CORE // must run the task on same core @@ -266,7 +266,7 @@ namespace Motors { tmcstepper->THIGH(calc_tstep(feedrate, 60.0)); tmcstepper->sfilt(1); tmcstepper->diag1_stall(true); // stallguard i/o is on diag1 - tmcstepper->sgt(constrain(axis_settings[axis_index()]->stallguard->get(), -64, 63)); + tmcstepper->sgt(constrain(_stallguard, -64, 63)); break; } default: @@ -295,7 +295,7 @@ namespace Motors { tmcstepper->stallguard(), tmcstepper->sg_result(), feedrate, - constrain(axis_settings[axis_index()]->stallguard->get(), -64, 63)); + constrain(_stallguard, -64, 63)); TMC2130_n ::DRV_STATUS_t status { 0 }; // a useful struct to access the bits. status.sr = tmcstepper->DRV_STATUS(); @@ -357,22 +357,22 @@ namespace Motors { // Prints StallGuard data that is useful for tuning. void TrinamicDriver::readSgTask(void* pvParameters) { + auto trinamicDriver = static_cast(pvParameters); + TickType_t xLastWakeTime; const TickType_t xreadSg = 200; // in ticks (typically ms) auto n_axis = MachineConfig::instance()->_axes->_numberAxis; xLastWakeTime = xTaskGetTickCount(); // Initialise the xLastWakeTime variable with the current time. while (true) { // don't ever return from this or the task dies - if (stallguard_debug_mask->get() != 0) { - if (sys.state == State::Cycle || sys.state == State::Homing || sys.state == State::Jog) { - for (TrinamicDriver* p = List; p; p = p->link) { - if (bitnum_istrue(stallguard_debug_mask->get(), p->axis_index())) { - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "SG:%d", stallguard_debug_mask->get()); - p->debug_message(); - } + if (sys.state == State::Cycle || sys.state == State::Homing || sys.state == State::Jog) { + for (TrinamicDriver* p = List; p; p = p->link) { + if (p->_stallguardDebugMode) { + //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "SG:%d", stallguard_debug_mask->get()); + p->debug_message(); } - } // sys.state - } // if mask + } + } // sys.state vTaskDelayUntil(&xLastWakeTime, xreadSg); diff --git a/Grbl_Esp32/src/Motors/TrinamicDriver.h b/Grbl_Esp32/src/Motors/TrinamicDriver.h index 60b6175d..d49a1790 100644 --- a/Grbl_Esp32/src/Motors/TrinamicDriver.h +++ b/Grbl_Esp32/src/Motors/TrinamicDriver.h @@ -86,6 +86,7 @@ namespace Motors { float _hold_current = 0.25; int _microsteps = 256; int _stallguard = 0; + bool _stallguardDebugMode = false; TrinamicMode _mode = TrinamicMode::None; bool test(); @@ -137,6 +138,7 @@ namespace Motors { handler.handle("hold_current", _hold_current); handler.handle("microsteps", _microsteps); handler.handle("stallguard", _stallguard); + handler.handle("stallguardDebugMode", _stallguardDebugMode); StandardStepper::handle(handler); } diff --git a/Grbl_Esp32/src/Motors/TrinamicUartDriver.h b/Grbl_Esp32/src/Motors/TrinamicUartDriver.h index e46a015a..ffdc4cd7 100644 --- a/Grbl_Esp32/src/Motors/TrinamicUartDriver.h +++ b/Grbl_Esp32/src/Motors/TrinamicUartDriver.h @@ -90,10 +90,11 @@ namespace Motors { bool _has_errors; bool _disabled; - float _run_current = 0.25; - float _hold_current = 0.25; - int _microsteps = 256; - int _stallguard = 0; + float _run_current = 0.25; + float _hold_current = 0.25; + int _microsteps = 256; + int _stallguard = 0; + bool _stallguardDebugMode = false; TrinamicUartMode _mode = TrinamicUartMode::None; bool test(); @@ -142,6 +143,7 @@ namespace Motors { handler.handle("hold_current", _hold_current); handler.handle("microsteps", _microsteps); handler.handle("stallguard", _stallguard); + handler.handle("stallguardDebugMode", _stallguardDebugMode); StandardStepper::handle(handler); } diff --git a/Grbl_Esp32/src/Motors/TrinamicUartDriverClass.cpp b/Grbl_Esp32/src/Motors/TrinamicUartDriverClass.cpp index 5f4977cf..402c6e5a 100644 --- a/Grbl_Esp32/src/Motors/TrinamicUartDriverClass.cpp +++ b/Grbl_Esp32/src/Motors/TrinamicUartDriverClass.cpp @@ -235,12 +235,16 @@ namespace Motors { tmcstepper->pwm_autoscale(false); break; case TrinamicUartMode ::StallGuard: //TODO: check all configurations for stallguard + { + auto axisConfig = MachineConfig::instance()->_axes->_axis[this->axis_index()]; + auto homingFeedRate = (axisConfig->_homing != nullptr) ? axisConfig->_homing->_feedRate : 200; //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Stallguard"); tmcstepper->en_spreadCycle(false); tmcstepper->pwm_autoscale(false); - tmcstepper->TCOOLTHRS(calc_tstep(homing_feed_rate->get(), 150.0)); - tmcstepper->SGTHRS(constrain(axis_settings[axis_index()]->stallguard->get(), 0, 255)); + tmcstepper->TCOOLTHRS(calc_tstep(homingFeedRate, 150.0)); + tmcstepper->SGTHRS(constrain(_stallguard, 0, 255)); break; + } default: grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Unknown Trinamic mode:d", _mode); } @@ -266,7 +270,7 @@ namespace Motors { reportAxisNameMsg(axis_index(), dual_axis_index()), tmcstepper->SG_RESULT(), // tmcstepper->sg_result(), feedrate, - constrain(axis_settings[axis_index()]->stallguard->get(), -64, 63)); + constrain(_stallguard, -64, 63)); TMC2208_n ::DRV_STATUS_t status { 0 }; // a useful struct to access the bits. status.sr = tmcstepper->DRV_STATUS(); @@ -334,16 +338,14 @@ namespace Motors { xLastWakeTime = xTaskGetTickCount(); // Initialise the xLastWakeTime variable with the current time. while (true) { // don't ever return from this or the task dies - if (stallguard_debug_mask->get() != 0) { - if (sys.state == State::Cycle || sys.state == State::Homing || sys.state == State::Jog) { - for (TrinamicUartDriver* p = List; p; p = p->link) { - if (bitnum_istrue(stallguard_debug_mask->get(), p->axis_index())) { - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "SG:%d", stallguard_debug_mask->get()); - p->debug_message(); - } + if (sys.state == State::Cycle || sys.state == State::Homing || sys.state == State::Jog) { + for (TrinamicUartDriver* p = List; p; p = p->link) { + if (p->_stallguardDebugMode) { + //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "SG:%d", _stallguardDebugMode); + p->debug_message(); } - } // sys.state - } // if mask + } + } // sys.state vTaskDelayUntil(&xLastWakeTime, xreadSg); diff --git a/Grbl_Esp32/src/Stepper.cpp b/Grbl_Esp32/src/Stepper.cpp index d8e8fb54..6d3c5892 100644 --- a/Grbl_Esp32/src/Stepper.cpp +++ b/Grbl_Esp32/src/Stepper.cpp @@ -993,12 +993,9 @@ void IRAM_ATTR Stepper_Timer_Stop() { } bool get_stepper_disable() { // returns true if steppers are disabled - if (SteppersDisablePin->get() != Pin::UNDEFINED) { - bool disabled = SteppersDisablePin->get().read(); - - if (step_enable_invert->get()) { - disabled = !disabled; // Apply pin invert. - } + auto axesConfig = MachineConfig::instance()->_axes; + if (!axesConfig->_sharedStepperDisable.undefined()) { + bool disabled = axesConfig->_sharedStepperDisable.read(); return disabled; } else { return false; // thery are never disabled if there is no pin defined