mirror/Grbl_Esp32
1
0
Fork 0
mirror of https://github.com/bdring/Grbl_Esp32.git synced 2025-08-27 08:14:31 +02:00
Code Issues Releases Wiki Activity

Added Homing.cpp

Browse Source
This commit is contained in:
Mitch Bradley
2021-07-29 06:50:46 -10:00
parent 2d78c4afbd
commit ab29243efb
1 changed files with 336 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

336
Grbl_Esp32/src/Machine/Homing.cpp Normal file
View File

@@ -0,0 +1,336 @@
#include "Homing.h"
#include "../MotionControl.h" // mc_reset
#include "../NutsBolts.h" // set_bitnum, etc
#include "../System.h" // sys.*
#include "../Stepper.h" // st_wake
#include "../Report.h" // CLIENT_
#include "../Protocol.h" // protocol_execute_realtime
#include "../Machine/Axes.h"
#include "../Machine/Endstops.h"
#include "../Machine/MachineConfig.h" // config
namespace Machine {
// 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.
const uint32_t GANG0 = 0xffff;
const uint32_t GANG1 = 0xffff0000;
const uint32_t BOTH_GANGS = (GANG0 | GANG1);
// The return value is the setting time
uint32_t Homing::plan_move(MotorMask motors, 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();
AxisMask axesMask = 0;
// Find the axis that will take the longest
for (int axis = 0; axis < n_axis; axis++) {
if (bits_are_false(motors, axis | (axis << 16))) {
continue;
}
// Record active axes for the next phase
set_bitnum(axesMask, axis);
// 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);
float travel;
if (seek) {
travel = axisConfig->_maxTravel;
} else {
Machine::Endstops* endstop = axisConfig->_endstops;
if (endstop) {
travel = endstop->_pulloff;
} else {
travel = 0.0f;
if (bitnum_is_true(motors, axis)) {
travel = axisConfig->_gangs[0]->_endstops->_pulloff;
}
if (bitnum_is_true(motors, axis + 16)) {
travel = std::max(travel, axisConfig->_gangs[1]->_endstops->_pulloff);
}
}
// We know that endstop is not nullptr because we checked for a
// valid endstop configuration before starting to home.
travel = endstop->_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 = REPORT_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;
}
void Homing::run(MotorMask remainingMotors, bool approach, bool seek) {
// See if any motors are left. This could be 0 if none of the motors specified
// by the original value of axes is capable of standard homing.
if (remainingMotors == 0) {
return;
}
uint32_t settling_ms = plan_move(remainingMotors, approach, seek);
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?
MotorMask limitedMotors = Machine::Axes::posLimitMask | Machine::Axes::negLimitMask;
config->_axes->stop_motors(limitedMotors);
clear_bits(remainingMotors, limitedMotors);
}
Stepper::prep_buffer(); // Check and prep segment buffer.
// 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.
throw ExecAlarm::HomingFailReset;
}
if (rtSafetyDoor) {
// Homing failure: Safety door was opened.
throw ExecAlarm::HomingFailDoor;
}
if (rtCycleStop) {
rtCycleStop = false;
if (approach) {
// Homing failure: Limit switch not found during approach.
throw ExecAlarm::HomingFailApproach;
}
// Pulloff
if ((Machine::Axes::posLimitMask | Machine::Axes::negLimitMask) & remainingMotors) {
// Homing failure: Limit switch still engaged after pull-off motion
throw ExecAlarm::HomingFailPulloff;
}
// Normal termination for pulloff cycle
remainingMotors = 0;
}
} while (remainingMotors);
Stepper::reset(); // Immediately force kill steppers and reset step segment buffer.
delay_ms(settling_ms); // Delay to allow transient dynamics to dissipate.
}
// If there is a squared axis with only one limit switch, we must perform
// an approximate squaring cycle that could leave the axis slightly racked.
bool Homing::squaredOneSwitch(MotorMask motors) {
AxisMask squaredAxes = motors & (motors >> 16);
if (squaredAxes == 0) {
// 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;
}
auto axisConfig = axes->_axis[axis];
Machine::Endstops* endstop = axisConfig->_endstops;
if (endstop) {
// Shared endstop on squared axis
return true;
}
endstop = axisConfig->_gangs[0]->_endstops;
if (!endstop) {
// Missing endstop on gang 0
return true;
}
endstop = axisConfig->_gangs[1]->_endstops;
if (!endstop) {
// Missing endstop on gang 1
return true;
}
}
// If we get here, all of the squared axes in this cycle have separate
// limit switches.
return false;
}
// 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.
// axes cannot be 0. The 0 case - run all cycles - is
// handled by the caller mc_homing_cycle()
void Homing::set_mpos(AxisMask axisMask) {
// 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.
auto axes = config->_axes;
auto n_axis = axes->_numberAxis;
// 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(axisMask, 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(axisMask, false); // tell motors homing is done
}
void Homing::run_one_cycle(AxisMask axisMask) {
axisMask &= Machine::Axes::homingMask;
MotorMask motors = config->_axes->set_homing_mode(axisMask, true);
try {
run(motors, true, true); // Approach fast
run(motors, false, false); // Pulloff
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 {
for (int i = 0; i < NHomingLocateCycle; i++) {
run(motors, true, false); // Approach slowly
run(motors, false, false); // Pulloff
}
}
} catch (ExecAlarm alarm) {
sys_rt_exec_alarm = alarm;
config->_axes->set_homing_mode(axisMask, 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;
}
set_mpos(axisMask);
}
void Homing::run_cycles(AxisMask axisMask) {
// -------------------------------------------------------------------------------------
// Perform homing routine. NOTE: Special motion case. Only system reset works.
if (axisMask != AllCycles) {
run_one_cycle(axisMask);
} else {
// Run all homing cycles
bool someAxisHomed = false;
for (int cycle = 1; cycle <= MAX_N_AXIS; cycle++) {
// Set axisMask to the axes that home on this cycle
axisMask = 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(axisMask, axis);
}
}
if (axisMask) { // if there are some axes in this cycle
someAxisHomed = true;
run_one_cycle(axisMask);
}
}
if (!someAxisHomed) {
report_status_message(Error::HomingNoCycles, CLIENT_ALL);
sys.state = State::Alarm;
}
}
}
}