mirror/Grbl_Esp32
1
0
Fork 0
mirror of https://github.com/bdring/Grbl_Esp32.git synced 2025-08-15 11:04:16 +02:00
Code Issues Releases Wiki Activity

Homing - support single switch squared axes.

Browse Source
This commit is contained in:
Mitch Bradley
2021-07-28 12:52:52 -10:00
parent 4260eb8446
commit 2d78c4afbd
12 changed files with 67 additions and 385 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
Grbl_Esp32/data/config.yaml
View File

@@ -16,8 +16,6 @@ axes:
shared_stepper_disable: gpio.13:high
x:
steps_per_mm: 1500
max_rate: 50000
steps_per_mm: 800
max_rate: 5000
acceleration: 100

337
Grbl_Esp32/src/Limits.cpp
View File

@@ -28,331 +28,17 @@
#include "Limits.h"
#include "Machine/MachineConfig.h"
#include "Planner.h"
#include "MotionControl.h" // HOMING_CYCLE_LINE_NUMBER
#include "NutsBolts.h" // set_bitnum, etc
#include "MotionControl.h" // mc_reset
#include "System.h" // sys.*
#include "Stepper.h" // st_wake
#include "Report.h" // CLIENT_
#include "Protocol.h" // protocol_execute_realtime
#include "I2SOut.h" // I2S_OUT_DELAY_MS
#include "Platform.h"
#include "Machine/Axes.h"
#include "Platform.h" // WEAK_LINK
#include <freertos/task.h>
#include <freertos/queue.h>
#include <string.h> // memset, memcpy
#include <algorithm> // min, max
#include <atomic> // fence
xQueueHandle limit_sw_queue; // used by limit switch debouncing
// Calculate the motion for the next homing move.
// Input: axesMask - the axes that should participate in this homing cycle
// Input: approach - the direction of motion - true for approach, false for pulloff
// Input: seek - the phase - true for the initial high-speed seek, false for the slow second phase
// Output: axislock - the axes that actually participate, accounting
// Output: target - the endpoint vector of the motion
// Output: rate - the feed rate
// Return: debounce - the maximum delay time of all the axes
// For multi-axis homing, we use the per-axis rates and travel limits to compute
// a target vector and a feedrate as follows:
// The goal is for each axis to travel at its specified rate, and for the
// maximum travel to be enough for each participating axis to reach its limit.
// For the rate goal, the axis components of the target vector must be proportional
// to the per-axis rates, and the overall feed rate must be the magnitude of the
// vector of per-axis rates.
// For the travel goal, the axis components of the target vector must be scaled
// according to the one that would take the longest.
// The time to complete a maxTravel move for a given feedRate is maxTravel/feedRate.
// We compute that time for all axes in the homing cycle, then find the longest one.
// Then we scale the travel distances for the other axes so they would complete
// at the same time.
static uint32_t limits_plan_move(AxisMask axesMask, bool approach, bool seek) {
float maxSeekTime = 0.0;
float limitingRate = 0.0;
uint32_t debounce = 0;
float rate = 0.0;
auto axes = config->_axes;
auto n_axis = axes->_numberAxis;
float* target = system_get_mpos();
// Find the axis that will take the longest
for (int axis = 0; axis < n_axis; axis++) {
if (bitnum_is_false(axesMask, axis)) {
continue;
}
// Set target location for active axes and setup computation for homing rate.
sys_position[axis] = 0;
auto axisConfig = axes->_axis[axis];
auto homing = axisConfig->_homing;
debounce = std::max(debounce, homing->_debounce_ms);
float axis_rate = seek ? homing->_seekRate : homing->_feedRate;
// Accumulate the squares of the homing rates for later use
// in computing the aggregate feed rate.
rate += (axis_rate * axis_rate);
auto travel = seek ? axisConfig->_maxTravel : homing->_pulloff;
// First we compute the maximum-time-to-completion vector; later we will
// convert it back to positions after we determine the limiting axis.
// Set target direction based on cycle mask and homing cycle approach state.
auto seekTime = travel / axis_rate;
target[axis] = (homing->_positiveDirection ^ approach) ? -seekTime : seekTime;
if (seekTime > maxSeekTime) {
maxSeekTime = seekTime;
limitingRate = axis_rate;
}
}
// Scale the target array, currently in units of time, back to positions
// When approaching a small fudge factor to ensure that the limit is reached -
// but no fudge factor when pulling off.
for (int axis = 0; axis < n_axis; axis++) {
if (bitnum_is_true(axesMask, axis)) {
auto homing = config->_axes->_axis[axis]->_homing;
auto scaler = approach ? (seek ? homing->_seek_scaler : homing->_feed_scaler) : 1.0;
target[axis] *= limitingRate * scaler;
}
}
plan_line_data_t plan_data;
plan_data.spindle_speed = 0;
plan_data.motion = {};
plan_data.motion.systemMotion = 1;
plan_data.motion.noFeedOverride = 1;
plan_data.spindle = SpindleState::Disable;
plan_data.coolant.Mist = 0;
plan_data.coolant.Flood = 0;
plan_data.line_number = HOMING_CYCLE_LINE_NUMBER;
plan_data.is_jog = false;
plan_data.feed_rate = float(sqrt(rate)); // Magnitude of homing rate vector
plan_buffer_line(target, &plan_data); // Bypass mc_line(). Directly plan homing motion.
sys.step_control = {};
sys.step_control.executeSysMotion = true; // Set to execute homing motion and clear existing flags.
Stepper::prep_buffer(); // Prep and fill segment buffer from newly planned block.
Stepper::wake_up(); // Initiate motion
return debounce;
}
// Returns true if an error occurred
static ExecAlarm limits_handle_errors(bool approach, uint8_t cycle_mask) {
// This checks some of the events that would normally be handled
// by protocol_execute_realtime(). The homing loop is time-critical
// so we handle those events directly here, calling protocol_execute_realtime()
// only if one of those events is active.
if (rtReset) {
// Homing failure: Reset issued during cycle.
return ExecAlarm::HomingFailReset;
}
if (rtSafetyDoor) {
// Homing failure: Safety door was opened.
return ExecAlarm::HomingFailDoor;
}
if (rtCycleStop) {
if (approach) {
// Homing failure: Limit switch not found during approach.
return ExecAlarm::HomingFailApproach;
}
// Pulloff
if (limits_check(cycle_mask)) {
// Homing failure: Limit switch still engaged after pull-off motion
return ExecAlarm::HomingFailPulloff;
}
}
// If we get here, either none of the rtX events were triggered, or
// it was rtCycleStop during a pulloff with the limit switches
// disengaged, i.e. a normal pulloff completion.
return ExecAlarm::None;
}
// Homes the specified cycle axes, sets the machine position, and performs a pull-off motion after
// completing. Homing is a special motion case, which involves rapid uncontrolled stops to locate
// the trigger point of the limit switches. The rapid stops are handled by a system level axis lock
// mask, which prevents the stepper algorithm from executing step pulses. Homing motions typically
// circumvent the processes for executing motions in normal operation.
// NOTE: Only the abort realtime command can interrupt this process.
// cycle_mask cannot be 0. The 0 case - run all cycles - is
// handled by the caller mc_homing_cycle()
static void limits_run_one_homing_cycle(AxisMask cycle_mask) {
if (sys.abort) {
return; // Block if system reset has been issued.
}
auto axes = config->_axes;
auto n_axis = axes->_numberAxis;
cycle_mask &= Machine::Axes::homingMask;
// Initialize plan data struct for homing motion. Spindle and coolant are disabled.
// Put motors on axes listed in cycle_mask in homing mode and
// replace cycle_mask with the list of motors that are ready for homing.
// Motors with non standard homing can home during motors_set_homing_mode(...)
cycle_mask = config->_axes->set_homing_mode(cycle_mask, true); // tell motors homing is about to start
// See if any motors are left. This could be 0 if none of the motors specified
// by the original value of cycle_mask is capable of standard homing.
if (cycle_mask == 0) {
return;
}
// Initialize variables used for homing computations.
uint8_t n_cycle = (2 * NHomingLocateCycle + 1);
// approach is the direction of motion; it cycles between true and false
bool approach = true;
// seek starts out true for initial rapid motion toward the homing switches,
// then becomes false after the first approach cycle, for slower motion on
// subsequent fine-positioning steps
bool seek = true;
do {
uint32_t debounce_ms = limits_plan_move(cycle_mask, approach, seek);
// Perform homing cycle. Planner buffer should be empty, as required to initiate the homing cycle.
// Start with all motors allowed to move
config->_axes->release_all_motors();
// For approach cycles:
// remainingMotors starts out with the bits set for all the motors in this homing cycle.
// As limit switches are hit, their bits are cleared and the associated motor is stopped,
// continuing until no bits are set (normal exit)
// For pulloff cycles:
// Motion continues until rtCycleStop is set, indicating that the target was reached,
// without looking at the limit switches (which are initially active)
// There are also some error conditions that can abort the operation:
// rtReset or rtSafetyDoor - the user hits either of those buttons
// rtCycleStop in approach
// - the max travel distance was reached without hitting all the limit switches
// rtCycleStop in pulloff but a switch is still active
// - pulloff failed to clear all the switches
// XXX we need to include gang1 in the remaining mask
// The following might fail if only one gang has limit switches. Anaylze me.
uint32_t remainingMotors = (cycle_mask | (cycle_mask << 16)) & Machine::Axes::motorMask;
do {
if (approach) {
// Check limit state. Lock out cycle axes when they change.
// XXX do we check only the switch in the direction of motion?
uint32_t limitedAxes = Machine::Axes::posLimitMask | Machine::Axes::negLimitMask;
config->_axes->stop_motors(limitedAxes);
clear_bits(remainingMotors, limitedAxes);
}
Stepper::prep_buffer(); // Check and prep segment buffer.
ExecAlarm alarm = limits_handle_errors(approach, cycle_mask);
if (alarm != ExecAlarm::None) {
// Homing failure
sys_rt_exec_alarm = alarm;
config->_axes->set_homing_mode(cycle_mask, false); // tell motors homing is done...failed
log_debug("Homing fail");
mc_reset(); // Stop motors, if they are running.
// protocol_execute_realtime() will handle any pending rtXXX conditions
protocol_execute_realtime();
return;
}
if (rtCycleStop) {
// Normal pulloff completion with limit switches disengaged
rtCycleStop = false;
break;
}
// Keep trying until all axes have finished
} while (remainingMotors);
if (!approach) {
config->_stepping->synchronize();
}
Stepper::reset(); // Immediately force kill steppers and reset step segment buffer.
delay_ms(debounce_ms); // Delay to allow transient dynamics to dissipate.
// After the initial approach, we switch to the slow rate for subsequent steps
// The pattern is fast approach, slow pulloff, slow approach, slow pulloff, ...
seek = false;
// Reverse direction.
approach = !approach;
} 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,
// set up pull-off maneuver from axes limit switches that have been homed. This provides
// some initial clearance off the switches and should also help prevent them from falsely
// triggering when hard limits are enabled or when more than one axes shares a limit pin.
// Set machine positions for homed limit switches. Don't update non-homed axes.
for (int axis = 0; axis < n_axis; axis++) {
Machine::Axis* axisConf = config->_axes->_axis[axis];
auto homing = axisConf->_homing;
if (bitnum_is_true(cycle_mask, axis)) {
auto mpos = homing->_mpos;
auto pulloff = homing->_pulloff;
auto steps = axisConf->_stepsPerMm;
if (homing->_positiveDirection) {
sys_position[axis] = int32_t((mpos + pulloff) * steps);
} else {
sys_position[axis] = int32_t((mpos - pulloff) * steps);
}
}
}
sys.step_control = {}; // Return step control to normal operation.
config->_axes->set_homing_mode(cycle_mask, false); // tell motors homing is done
}
void limits_run_homing_cycles(AxisMask axis_mask) {
// -------------------------------------------------------------------------------------
// Perform homing routine. NOTE: Special motion case. Only system reset works.
if (axis_mask != HOMING_CYCLE_ALL) {
limits_run_one_homing_cycle(axis_mask);
} else {
// Run all homing cycles
bool someAxisHomed = false;
for (int cycle = 1; cycle <= MAX_N_AXIS; cycle++) {
// Set axis_mask to the axes that home on this cycle
axis_mask = 0;
auto n_axis = config->_axes->_numberAxis;
for (int axis = 0; axis < n_axis; axis++) {
auto axisConfig = config->_axes->_axis[axis];
auto homing = axisConfig->_homing;
if (homing && homing->_cycle == cycle) {
set_bitnum(axis_mask, axis);
}
}
if (axis_mask) { // if there are some axes in this cycle
someAxisHomed = true;
limits_run_one_homing_cycle(axis_mask);
}
}
if (!someAxisHomed) {
report_status_message(Error::HomingNoCycles, CLIENT_ALL);
sys.state = State::Alarm;
}
}
}
void limits_init() {
if (Machine::Axes::limitMask) {
if (limit_sw_queue == NULL && config->_softwareDebounceMs != 0) {
@@ -370,19 +56,11 @@ void limits_init() {
}
}
// Check the limit switches for the axes listed in check_mask.
// Return a mask of the switches that are engaged.
AxisMask limits_check(AxisMask check_mask) {
// Expand the bitmask to include both gangs
set_bits(check_mask, check_mask << 16);
return (Machine::Axes::posLimitMask | Machine::Axes::negLimitMask) & check_mask;
}
// Returns limit state as a bit-wise uint8 variable. Each bit indicates an axis limit, where
// triggered is 1 and not triggered is 0. Invert mask is applied. Axes are defined by their
// number in bit position, i.e. Z_AXIS is bit(2), and Y_AXIS is bit(1).
AxisMask limits_get_state() {
return limits_check(Machine::Axes::limitMask);
// number in bit position, i.e. Z_AXIS is bitnum_to_mask(2), and Y_AXIS is bitnum_to_mask(1).
MotorMask limits_get_state() {
return Machine::Axes::posLimitMask | Machine::Axes::negLimitMask;
}
// Performs a soft limit check. Called from mcline() only. Assumes the machine has been homed,
@@ -418,8 +96,7 @@ void limitCheckTask(void* pvParameters) {
int evt;
xQueueReceive(limit_sw_queue, &evt, portMAX_DELAY); // block until receive queue
vTaskDelay(config->_softwareDebounceMs / portTICK_PERIOD_MS); // delay a while
AxisMask switch_state;
switch_state = limits_get_state();
auto switch_state = limits_get_state();
if (switch_state) {
log_debug("Limit Switch State " << String(switch_state, HEX));
mc_reset(); // Initiate system kill.
@@ -465,6 +142,6 @@ bool WEAK_LINK limitsCheckTravel(float* target) {
return false;
}
bool WEAK_LINK user_defined_homing(AxisMask cycle_mask) {
bool WEAK_LINK user_defined_homing(AxisMask axisMask) {
return false;
}

7
Grbl_Esp32/src/Limits.h
View File

@@ -31,16 +31,13 @@
#include <cstdint>
const int HOMING_CYCLE_ALL = 0; // Must be zero.
const int HOMING_CYCLE_LINE_NUMBER = 0;
// Initialize the limits module
void limits_init();
// Returns limit state
AxisMask limits_get_state();
MotorMask limits_get_state();
void limits_run_homing_cycles(AxisMask axis_mask);
void homing_run_cycles(AxisMask axis_mask);
// Check for soft limit violations
void limits_soft_check(float* target);

31
Grbl_Esp32/src/Machine/Axes.cpp
View File

@@ -8,11 +8,11 @@
#include "MachineConfig.h" // config->
namespace Machine {
uint32_t Axes::posLimitMask = 0;
uint32_t Axes::negLimitMask = 0;
uint32_t Axes::homingMask = 0;
uint32_t Axes::limitMask = 0;
uint32_t Axes::motorMask = 0;
MotorMask Axes::posLimitMask = 0;
MotorMask Axes::negLimitMask = 0;
MotorMask Axes::homingMask = 0;
MotorMask Axes::limitMask = 0;
MotorMask Axes::motorMask = 0;
Axes::Axes() : _axis() {
for (int i = 0; i < MAX_N_AXIS; ++i) {
@@ -78,25 +78,20 @@ namespace Machine {
}
}
// use this to tell all the motors what the current homing mode is
// They can use this to setup things like Stall
uint32_t Axes::set_homing_mode(uint8_t homing_mask, bool isHoming) {
// 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 homing_mask, bool isHoming) {
release_all_motors(); // On homing transitions, cancel all motor lockouts
uint8_t can_home = 0;
MotorMask can_home = 0;
for (uint8_t axis = X_AXIS; axis < _numberAxis; axis++) {
if (bitnum_is_true(homing_mask, axis)) {
auto a = _axis[axis];
if (a != nullptr) {
auto motor = a->_gangs[0]->_motor;
if (motor->set_homing_mode(isHoming)) {
set_bitnum(can_home, axis);
for (uint8_t gang = 0; gang < Axis::MAX_NUMBER_GANGED; gang++) {
if (a->_gangs[gang]->_motor->set_homing_mode(isHoming)) {
set_bitnum(can_home, gang * 16 + axis);
}
for (uint8_t gang_index = 1; gang_index < Axis::MAX_NUMBER_GANGED; gang_index++) {
auto a2 = _axis[axis]->_gangs[gang_index]->_motor;
a2->set_homing_mode(isHoming);
}
}
}
@@ -106,7 +101,7 @@ namespace Machine {
}
void Axes::release_all_motors() { _motorLockoutMask = 0xffffffff; }
void Axes::stop_motors(uint32_t mask) { clear_bits(_motorLockoutMask, mask); }
void Axes::stop_motors(MotorMask mask) { clear_bits(_motorLockoutMask, mask); }
void IRAM_ATTR Axes::step(uint8_t step_mask, uint8_t dir_mask) {
auto n_axis = _numberAxis;

16
Grbl_Esp32/src/Machine/Axes.h
View File

@@ -33,17 +33,17 @@ namespace Machine {
// During homing, this is used to stop stepping on motors that have
// reached their limit switches, by clearing bits in the mask.
uint32_t _motorLockoutMask = 0;
MotorMask _motorLockoutMask = 0;
public:
Axes();
// Bitmasks to collect information about axes that have limits and homing
static uint32_t posLimitMask;
static uint32_t negLimitMask;
static uint32_t homingMask;
static uint32_t limitMask;
static uint32_t motorMask;
static MotorMask posLimitMask;
static MotorMask negLimitMask;
static MotorMask homingMask;
static MotorMask limitMask;
static MotorMask motorMask;
inline char axisName(int index) { return index < MAX_N_AXIS ? _names[index] : '?'; }
@@ -89,9 +89,9 @@ namespace Machine {
// These are used during homing cycles.
// The return value is a bitmask of axes that can home
uint32_t set_homing_mode(uint8_t homing_mask, bool isHoming);
MotorMask set_homing_mode(AxisMask homing_mask, bool isHoming);
void release_all_motors();
void stop_motors(uint32_t motor_mask);
void stop_motors(MotorMask motor_mask);
void set_disable(int axis, bool disable);
void set_disable(bool disable);

1
Grbl_Esp32/src/Machine/Endstops.cpp
View File

@@ -46,5 +46,6 @@ namespace Machine {
handler.item("limit_pos", _posPin);
handler.item("limit_all", _allPin);
handler.item("hard_limits", _hardLimits);
handler.item("pulloff", _pulloff);
}
}

1
Grbl_Esp32/src/Machine/Endstops.h
View File

@@ -36,6 +36,7 @@ namespace Machine {
Pin _posPin;
Pin _allPin;
bool _hardLimits = true;
float _pulloff = 1.0f; // mm
void init();

12
Grbl_Esp32/src/Machine/Homing.h
View File

@@ -19,12 +19,24 @@
*/
#include "../Configuration/Configurable.h"
#include "../System.h" // AxisMask, MotorMask
namespace Machine {
class Homing : public Configuration::Configurable {
static uint32_t plan_move(MotorMask motors, bool approach, bool seek);
static void run(MotorMask remainingMotors, bool approach, bool seek);
static bool squaredOneSwitch(MotorMask motors);
static void set_mpos(AxisMask axisMask);
static void run_one_cycle(AxisMask axisMask);
static const int REPORT_LINE_NUMBER = 0;
public:
Homing() = default;
static const int AllCycles = 0; // Must be zero.
static void run_cycles(AxisMask axisMask);
// The homing cycles are 1,2,3 etc. 0 means not homed as part of home-all,
// but you can still home it manually with e.g. $HA
int _cycle = -1; // what auto-homing cycle does this axis home on?

3
Grbl_Esp32/src/MotionControl.cpp
View File

@@ -25,6 +25,7 @@
#include "MotionControl.h"
#include "Machine/MachineConfig.h"
#include "Machine/Homing.h" // run_cycles
#include "Limits.h" // limits_soft_check
#include "Protocol.h" // protocol_execute_realtime
#include "Report.h" // CLIENT_*
@@ -293,7 +294,7 @@ void mc_homing_cycle(AxisMask axis_mask) {
}
// Might set an alarm; if so protocol_execute_realtime will handle it
limits_run_homing_cycles(axis_mask);
Machine::Homing::run_cycles(axis_mask);
protocol_execute_realtime(); // Check for reset and set system abort.
if (sys.abort) {

4
Grbl_Esp32/src/NutsBolts.h
View File

@@ -81,8 +81,8 @@ const float INCH_PER_MM = (0.0393701f);
#define set_bits(target, mask) (target) |= (mask)
#define clear_bits(target, mask) (target) &= ~(mask)
#define bits_are_true(target, mask) ((target & mask) != 0)
#define bits_are_false(target, mask) ((target & mask) == 0)
#define bits_are_true(target, mask) ((target & (mask)) != 0)
#define bits_are_false(target, mask) ((target & (mask)) == 0)
#define set_bitnum(target, num) (target) |= bitnum_to_mask(num)
#define clear_bitnum(target, num) (target) &= ~bitnum_to_mask(num)
#define bitnum_is_true(target, num) ((target & bitnum_to_mask(num)) != 0)

4
Grbl_Esp32/src/ProcessSettings.cpp
View File

@@ -281,14 +281,14 @@ Error home(int cycle) {
if (!sys.abort) { // Execute startup scripts after successful homing.
sys.state = State::Idle; // Set to IDLE when complete.
Stepper::go_idle(); // Set steppers to the settings idle state before returning.
if (cycle == HOMING_CYCLE_ALL) {
if (cycle == Machine::Homing::AllCycles) {
system_execute_startup();
}
}
return Error::Ok;
}
Error home_all(const char* value, WebUI::AuthenticationLevel auth_level, WebUI::ESPResponseStream* out) {
return home(HOMING_CYCLE_ALL);
return home(Machine::Homing::AllCycles);
}
Error home_x(const char* value, WebUI::AuthenticationLevel auth_level, WebUI::ESPResponseStream* out) {
return home(bitnum_to_mask(X_AXIS));

4
Grbl_Esp32/src/System.h
View File

@@ -70,7 +70,8 @@ union Suspend {
SuspendBits bit;
};
typedef uint8_t AxisMask; // Bits indexed by axis number
typedef uint32_t MotorMask; // Bits indexed by gang*16 + axis
typedef uint16_t AxisMask; // Bits indexed by axis number
typedef uint8_t Percent; // Integer percent
typedef uint8_t Counter; // Report interval
@@ -99,7 +100,6 @@ struct system_t {
bool soft_limit; // Tracks soft limit errors for the state machine. (boolean)
StepControl step_control; // Governs the step segment generator depending on system state.
bool probe_succeeded; // Tracks if last probing cycle was successful.
AxisMask homing_axis_lock; // Locks axes when limits engage. Used as an axis motion mask in the stepper ISR.
Percent f_override; // Feed rate override value in percent
Percent r_override; // Rapids override value in percent
Percent spindle_speed_ovr; // Spindle speed value in percent