From 06ab61b81159923738ff26deb15b9526ff2e2c8c Mon Sep 17 00:00:00 2001 From: bdring Date: Tue, 3 Aug 2021 13:40:21 -0500 Subject: [PATCH] WIP - Fixed POG Style single switch squaring --- Grbl_Esp32/src/Machine/Axes.cpp | 212 +++++++++++++++--------------- Grbl_Esp32/src/Machine/Axes.h | 7 +- Grbl_Esp32/src/Machine/Homing.cpp | 9 +- 3 files changed, 116 insertions(+), 112 deletions(-) diff --git a/Grbl_Esp32/src/Machine/Axes.cpp b/Grbl_Esp32/src/Machine/Axes.cpp index 0ae8b55f..11075cdc 100644 --- a/Grbl_Esp32/src/Machine/Axes.cpp +++ b/Grbl_Esp32/src/Machine/Axes.cpp @@ -28,7 +28,7 @@ namespace Machine { _sharedStepperDisable.report("Shared stepper disable"); } - release_all_motors(); + unlock_all_motors(); // certain motors need features to be turned on. Check them here for (uint8_t axis = X_AXIS; axis < _numberAxis; axis++) { @@ -83,7 +83,7 @@ namespace Machine { // Put the motors in the given axes into homing mode, returning a // mask of which motors (considering gangs) can do homing. MotorMask Axes::set_homing_mode(AxisMask axisMask, bool isHoming) { - release_all_motors(); // On homing transitions, cancel all motor lockouts + unlock_all_motors(); // On homing transitions, cancel all motor lockouts MotorMask motorsCanHome = 0; for (uint8_t axis = X_AXIS; axis < _numberAxis; axis++) { @@ -102,125 +102,127 @@ namespace Machine { return motorsCanHome; } - void Axes::release_all_motors() { _motorLockoutMask = 0xffffffff; } - void Axes::stop_motors(MotorMask mask) { clear_bits(_motorLockoutMask, mask); } + void Axes::unlock_all_motors() { _motorLockoutMask = 0; } + void Axes::lock_motors(MotorMask mask) { set_bits(_motorLockoutMask, mask); } + void Axes::unlock_motors(MotorMask mask) {clear_bits(_motorLockoutMask, mask); } + ; - void IRAM_ATTR Axes::step(uint8_t step_mask, uint8_t dir_mask) { - auto n_axis = _numberAxis; - //log_info("motors_set_direction_pins:0x%02X", onMask); + void IRAM_ATTR Axes::step(uint8_t step_mask, uint8_t dir_mask) { + auto n_axis = _numberAxis; + //log_info("motors_set_direction_pins:0x%02X", onMask); - // Set the direction pins, but optimize for the common - // situation where the direction bits haven't changed. - static uint8_t previous_dir = 255; // should never be this value - if (dir_mask != previous_dir) { - previous_dir = dir_mask; + // Set the direction pins, but optimize for the common + // situation where the direction bits haven't changed. + static uint8_t previous_dir = 255; // should never be this value + if (dir_mask != previous_dir) { + previous_dir = dir_mask; - for (int axis = X_AXIS; axis < n_axis; axis++) { - bool thisDir = bitnum_is_true(dir_mask, axis); + for (int axis = X_AXIS; axis < n_axis; axis++) { + bool thisDir = bitnum_is_true(dir_mask, axis); + for (uint8_t gang_index = 0; gang_index < Axis::MAX_NUMBER_GANGED; gang_index++) { + auto a = _axis[axis]->_gangs[gang_index]->_motor; + + if (a != nullptr) { + a->set_direction(thisDir); + } + } + } + config->_stepping->waitDirection(); + } + + config->_stepping->startPulseTimer(); + + // Turn on step pulses for motors that are supposed to step now + for (uint8_t axis = X_AXIS; axis < n_axis; axis++) { + if (bitnum_is_true(step_mask, axis)) { + auto a = _axis[axis]; + + if (bitnum_is_false(_motorLockoutMask, axis)) { + a->_gangs[0]->_motor->step(); + } + if (bitnum_is_false(_motorLockoutMask, axis + 16)) { + a->_gangs[1]->_motor->step(); + } + } + } + } + + // Turn all stepper pins off + void IRAM_ATTR Axes::unstep() { + config->_stepping->waitPulse(); + auto n_axis = _numberAxis; + for (uint8_t axis = X_AXIS; axis < n_axis; axis++) { for (uint8_t gang_index = 0; gang_index < Axis::MAX_NUMBER_GANGED; gang_index++) { auto a = _axis[axis]->_gangs[gang_index]->_motor; - - if (a != nullptr) { - a->set_direction(thisDir); - } + a->unstep(); + a->unstep(); } } - config->_stepping->waitDirection(); + + config->_stepping->finishPulse(); } - config->_stepping->startPulseTimer(); - - // Turn on step pulses for motors that are supposed to step now - for (uint8_t axis = X_AXIS; axis < n_axis; axis++) { - if (bitnum_is_true(step_mask, axis)) { - auto a = _axis[axis]; - - if (bitnum_is_true(_motorLockoutMask, axis)) { - a->_gangs[0]->_motor->step(); - } - if (bitnum_is_true(_motorLockoutMask, axis + 16)) { - a->_gangs[1]->_motor->step(); - } - } - } - } - - // Turn all stepper pins off - void IRAM_ATTR Axes::unstep() { - config->_stepping->waitPulse(); - auto n_axis = _numberAxis; - for (uint8_t axis = X_AXIS; axis < n_axis; axis++) { - for (uint8_t gang_index = 0; gang_index < Axis::MAX_NUMBER_GANGED; gang_index++) { - auto a = _axis[axis]->_gangs[gang_index]->_motor; - a->unstep(); - a->unstep(); - } - } - - config->_stepping->finishPulse(); - } - - // Some small helpers to find the axis index and axis ganged index for a given motor. This - // is helpful for some motors that need this info, as well as debug information. - size_t Axes::findAxisIndex(const Motors::Motor* const motor) const { - for (int i = 0; i < _numberAxis; ++i) { - for (int j = 0; j < Axis::MAX_NUMBER_GANGED; ++j) { - if (_axis[i] != nullptr && _axis[i]->hasMotor(motor)) { - return i; - } - } - } - - Assert(false, "Cannot find axis for motor. Something wonky is going on here..."); - return SIZE_MAX; - } - - size_t Axes::findAxisGanged(const Motors::Motor* const motor) const { - for (int i = 0; i < _numberAxis; ++i) { - if (_axis[i] != nullptr && _axis[i]->hasMotor(motor)) { + // Some small helpers to find the axis index and axis ganged index for a given motor. This + // is helpful for some motors that need this info, as well as debug information. + size_t Axes::findAxisIndex(const Motors::Motor* const motor) const { + for (int i = 0; i < _numberAxis; ++i) { for (int j = 0; j < Axis::MAX_NUMBER_GANGED; ++j) { - if (_axis[i]->_gangs[j]->_motor == motor) { - return j; + if (_axis[i] != nullptr && _axis[i]->hasMotor(motor)) { + return i; } } } + + Assert(false, "Cannot find axis for motor. Something wonky is going on here..."); + return SIZE_MAX; + } + + size_t Axes::findAxisGanged(const Motors::Motor* const motor) const { + for (int i = 0; i < _numberAxis; ++i) { + if (_axis[i] != nullptr && _axis[i]->hasMotor(motor)) { + for (int j = 0; j < Axis::MAX_NUMBER_GANGED; ++j) { + if (_axis[i]->_gangs[j]->_motor == motor) { + return j; + } + } + } + } + + Assert(false, "Cannot find axis for motor. Something wonky is going on here..."); + return SIZE_MAX; + } + + // Configuration helpers: + + void Axes::group(Configuration::HandlerBase & handler) { + handler.item("number_axis", _numberAxis); + handler.item("shared_stepper_disable", _sharedStepperDisable); + + // Handle axis names xyzabc. handler.section is inferred + // from a template. + char tmp[3]; + tmp[2] = '\0'; + + for (size_t i = 0; i < MAX_N_AXIS; ++i) { + tmp[0] = tolower(_names[i]); + tmp[1] = '\0'; + + handler.section(tmp, _axis[i], i); + } + } + + void Axes::afterParse() { + for (size_t i = 0; i < MAX_N_AXIS; ++i) { + if (_axis[i] == nullptr) { + _axis[i] = new Axis(i); + } + } } - Assert(false, "Cannot find axis for motor. Something wonky is going on here..."); - return SIZE_MAX; - } - - // Configuration helpers: - - void Axes::group(Configuration::HandlerBase& handler) { - handler.item("number_axis", _numberAxis); - handler.item("shared_stepper_disable", _sharedStepperDisable); - - // Handle axis names xyzabc. handler.section is inferred - // from a template. - char tmp[3]; - tmp[2] = '\0'; - - for (size_t i = 0; i < MAX_N_AXIS; ++i) { - tmp[0] = tolower(_names[i]); - tmp[1] = '\0'; - - handler.section(tmp, _axis[i], i); - } - } - - void Axes::afterParse() { - for (size_t i = 0; i < MAX_N_AXIS; ++i) { - if (_axis[i] == nullptr) { - _axis[i] = new Axis(i); + Axes::~Axes() { + for (int i = 0; i < MAX_N_AXIS; ++i) { + delete _axis[i]; } } } - - Axes::~Axes() { - for (int i = 0; i < MAX_N_AXIS; ++i) { - delete _axis[i]; - } - } -} diff --git a/Grbl_Esp32/src/Machine/Axes.h b/Grbl_Esp32/src/Machine/Axes.h index 14aec9ea..951f2bed 100644 --- a/Grbl_Esp32/src/Machine/Axes.h +++ b/Grbl_Esp32/src/Machine/Axes.h @@ -33,7 +33,7 @@ namespace Machine { // During homing, this is used to stop stepping on motors that have // reached their limit switches, by clearing bits in the mask. - MotorMask _motorLockoutMask = 0xffffffff; + MotorMask _motorLockoutMask = 0; public: Axes(); @@ -90,8 +90,9 @@ namespace Machine { // These are used during homing cycles. // The return value is a bitmask of axes that can home MotorMask set_homing_mode(AxisMask homing_mask, bool isHoming); - void release_all_motors(); - void stop_motors(MotorMask motor_mask); + void unlock_all_motors(); + void lock_motors(MotorMask motor_mask); + void unlock_motors(MotorMask motor_mask); void set_disable(int axis, bool disable); void set_disable(bool disable); diff --git a/Grbl_Esp32/src/Machine/Homing.cpp b/Grbl_Esp32/src/Machine/Homing.cpp index 6048c7d4..e20560f8 100644 --- a/Grbl_Esp32/src/Machine/Homing.cpp +++ b/Grbl_Esp32/src/Machine/Homing.cpp @@ -144,7 +144,8 @@ namespace Machine { uint32_t settling_ms = plan_move(remainingMotors, approach, seek); - config->_axes->release_all_motors(); + config->_axes->lock_motors(0xffffffff); + config->_axes->unlock_motors(remainingMotors); do { if (approach) { @@ -152,7 +153,7 @@ namespace Machine { // XXX do we check only the switch in the direction of motion? MotorMask limitedMotors = Machine::Axes::posLimitMask | Machine::Axes::negLimitMask; - config->_axes->stop_motors(limitedMotors); + config->_axes->lock_motors(limitedMotors); clear_bits(remainingMotors, limitedMotors); } @@ -198,9 +199,9 @@ namespace Machine { // No axis has multiple motors return false; } + auto axes = config->_axes; auto n_axis = axes->_numberAxis; - for (int axis = 0; axis < n_axis; axis++) { if (bitnum_is_false(squaredAxes, axis)) { continue; @@ -211,6 +212,7 @@ namespace Machine { // Shared endstop on squared axis return true; } + // check to see if at least one side is missing a switch endstop = axisConfig->_gangs[0]->_endstops; if (!endstop) { // Missing endstop on gang 0 @@ -277,7 +279,6 @@ namespace Machine { if (squaredOneSwitch(motors)) { run(motors & GANG0, true, false); // Approach slowly run(motors & GANG0, false, false); // Pulloff - run(motors & GANG1, true, false); // Approach slowly run(motors & GANG1, false, false); // Pulloff } else {