Mirrors_Framework/g2
1
0
Fork 0
mirror of https://github.com/synthetos/g2.git synced 2026-02-06 11:11:57 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity
Files
c290bdbbcff4eff5aa130447274d79cf8a14f3fb
g2/g2core/cycle_homing.cpp

571 lines
24 KiB
C++
Raw Blame History

/*
* cycle_homing.cpp - homing cycle extension to canonical_machine
* This file is part of the g2core project
*
* Copyright (c) 2010 - 2019 Alden S. Hart, Jr.
* Copyright (c) 2013 - 2019 Robert Giseburt
*
* This file ("the software") is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License, version 2 as published by the
* Free Software Foundation. You should have received a copy of the GNU General Public
* License, version 2 along with the software. If not, see <http://www.gnu.org/licenses/>.
*
* As a special exception, you may use this file as part of a software library without
* restriction. Specifically, if other files instantiate templates or use macros or
* inline functions from this file, or you compile this file and link it with other
* files to produce an executable, this file does not by itself cause the resulting
* executable to be covered by the GNU General Public License. This exception does not
* however invalidate any other reasons why the executable file might be covered by the
* GNU General Public License.
*
* THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY
* WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
* OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT
* SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF
* OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#include "g2core.h"
#include "util.h"
#include "config.h"
#include "json_parser.h"
#include "text_parser.h"
#include "canonical_machine.h"
#include "planner.h"
#include "encoder.h"
#include "kinematics.h"
#include "gpio.h"
#include "report.h"
#include "util.h"
/**** Homing singleton structure ****/
struct hmHomingSingleton { // persistent homing runtime variables
// controls for homing cycle
bool waiting_for_motion_end; // true when waiting for motion to complete.
int8_t axis; // axis currently being homed
int8_t homing_input; // homing input for current axis
bool set_coordinates; // G28.4 flag. true = set coords to zero at the end of homing cycle
stat_t (*func)(int8_t axis); // binding for callback function state machine
bool axis_flags[AXES]; // local storage for axis flags
// per-axis parameters
float direction; // set to 1 for positive (max), -1 for negative (to min);
float search_travel; // signed distance to travel in search
float search_velocity; // search speed as positive number
float latch_backoff; // max distance to back off switch during latch phase
float latch_velocity; // latch speed as positive number
float zero_backoff; // distance to back off switch before setting zero
float max_clear_backoff; // maximum distance of switch clearing backoffs before erring out
float setpoint; // ultimate setpoint, usually zero, but not always
// state saved from gcode model
cmUnitsMode saved_units_mode; // G20,G21 global setting
cmCoordSystem saved_coord_system; // G54 - G59 setting
cmDistanceMode saved_distance_mode; // G90, G91 global setting
cmFeedRateMode saved_feed_rate_mode; // G93, G94 global setting
float saved_feed_rate; // F setting
float saved_jerk; // saved and restored for each axis homed
};
static struct hmHomingSingleton hm;
/**** NOTE: global prototypes and other .h info is located in canonical_machine.h ****/
static stat_t _set_homing_func(stat_t (*func)(int8_t axis));
static stat_t _homing_axis_start(int8_t axis);
static stat_t _homing_axis_clear_init(int8_t axis);
static stat_t _homing_axis_search(int8_t axis);
static stat_t _homing_axis_clear(int8_t axis);
static stat_t _homing_axis_latch(int8_t axis);
static stat_t _homing_axis_setpoint_backoff(int8_t axis);
static stat_t _homing_axis_set_position(int8_t axis);
static stat_t _homing_axis_move(int8_t axis, float target, float velocity);
static stat_t _homing_error_exit(int8_t axis, stat_t status);
static stat_t _homing_finalize_exit(int8_t axis);
static int8_t _get_next_axis(int8_t axis);
static void _homing_axis_move_callback(float* vect, bool* flag);
/**** HELPERS ***************************************************************************
* _set_homing_func() - a convenience for setting the next dispatch vector and exiting
*/
static stat_t _set_homing_func(stat_t (*func)(int8_t axis)) {
hm.func = func;
return (STAT_EAGAIN);
}
/*
* _homing_handler - a gpioDigitalInputHandler to capture pin change events
* Will be registered only during homing mode - see gpio.h for more info
*/
gpioDigitalInputHandler _homing_handler {
[&](const bool state, const inputEdgeFlag edge, const uint8_t triggering_pin_number) {
if (cm->cycle_type != CYCLE_HOMING) { return GPIO_NOT_HANDLED; }
if (triggering_pin_number != hm.homing_input) { return GPIO_NOT_HANDLED; }
if (edge != INPUT_EDGE_LEADING) { return GPIO_NOT_HANDLED; }
en_take_encoder_snapshot();
cm_request_feedhold(FEEDHOLD_TYPE_SKIP, FEEDHOLD_EXIT_RESET_POSITION);
return GPIO_HANDLED; // DO NOT allow others to see this notice (particularly limits)
},
100, // priority
nullptr // next - nullptr to start with
};
/***********************************************************************************
**** G28.2 Homing Cycle ***********************************************************
***********************************************************************************/
/*****************************************************************************
* cm_homing_cycle_start() - G28.2 homing cycle using limit switches
* cm_homing_cycle_start_no_set() - G28.4 Homing cycle - do not set values
*
* --- How does this work? ---
*
* Homing is invoked using a G28.2 command with one or more axes specified in the
* command: e.g. g28.2 x0 y0 z0 (FYI: the number after each axis is irrelevant)
*
* To enable an axis for homing the Homing Input (hi) must be set to a valid input
* and that input configured for the proper switch type (NO, NC). It is preferable
* to have a unique input for each homing axis but it is possible to share an input
* across two or more axes. In this case the homing routine cannot automatically
* back off a homing switch that is fired at the start of the homing cycle.
*
* Homing is always run in the following order - for each enabled axis:
* Z,X,Y,A,B,C
*
* After initialization the following sequence is run for each axis to be homed:
*
* 0. Limits are automatically disabled. Shutdown and safety interlocks are not.
* 1. If a homing input is active on invocation, clear off the input (switch)
* 2. Drive towards homing switch in the set direction until switch is activated
* 3. Drive away from the homing switch at search velocity for latch distance
* 4. Drive towards homing switch at latch velocity until switch is activated
* 5. Back off switch by the zero backoff distance and set zero for that axis
*
* Homing works as a state machine that is driven by registering a callback function
* at hm.func() for the next state to be run. Once the axis is initialized each
* callback basically does two things (1) start the move for the current function,
* and (2) register the next state with hm.func(). When a move is started it will
* either be interrupted if the homing switch changes state. This will cause the
* move to stop with a feedhold. The other thing that can happen is the move will
* run to its full length if no switch change is detected (hit or open).
*
* Once all moves for an axis are complete the next axis in the sequence is homed
*
* When a homing cycle is initiated the homing state is set to HOMING_NOT_HOMED
* When homing completes successfully this is set to HOMING_HOMED, otherwise it
* remains HOMING_NOT_HOMED.
*/
/* --- Some further details ---
*
* Note: When coding a cycle (like this one) you get to perform one queued
* move per entry into the continuation, then you must exit.
*
* Another Note: When coding a cycle (like this one) you must wait until
* the last move has actually been queued (or has finished) before declaring
* the cycle to be done. Otherwise there is a nasty race condition in
* _controller_HSM() that may accept the next command before the position of
* the final move has been recorded in the Gcode model. That's what the call
* to cm_get_runtime_busy() is about.
*/
stat_t cm_homing_cycle_start(const float axes[], const bool flags[]) {
// save relevant non-axis parameters from Gcode model
hm.saved_units_mode = (cmUnitsMode)cm_get_units_mode(ACTIVE_MODEL);
hm.saved_coord_system = (cmCoordSystem)cm_get_coord_system(ACTIVE_MODEL);
hm.saved_distance_mode = (cmDistanceMode)cm_get_distance_mode(ACTIVE_MODEL);
hm.saved_feed_rate_mode = (cmFeedRateMode)cm_get_feed_rate_mode(ACTIVE_MODEL);
hm.saved_feed_rate = cm_get_feed_rate(ACTIVE_MODEL);
copy_vector(hm.axis_flags, flags);
// set working values
cm_set_units_mode(MILLIMETERS);
cm_set_distance_mode(INCREMENTAL_DISTANCE_MODE);
cm_set_coord_system(ABSOLUTE_COORDS); // homing is done in machine coordinates
cm_set_feed_rate_mode(UNITS_PER_MINUTE_MODE);
hm.set_coordinates = true;
// clear rotation matrix
canonical_machine_reset_rotation(cm);
hm.axis = -1; // set to retrieve initial axis
hm.func = _homing_axis_start; // bind initial processing function
cm->machine_state = MACHINE_CYCLE;
cm->cycle_type = CYCLE_HOMING;
cm->homing_state = HOMING_NOT_HOMED;
return (STAT_OK);
}
stat_t cm_homing_cycle_start_no_set(const float axes[], const bool flags[]) {
cm_homing_cycle_start(axes, flags);
hm.set_coordinates = false; // set flag to not update position variables at the end of the cycle
return (STAT_OK);
}
/***********************************************************************************
* cm_homing_cycle_callback() - main loop callback for running the homing cycle
*/
stat_t cm_homing_cycle_callback(void) {
if (cm->cycle_type != CYCLE_HOMING) { // exit if not in a homing cycle
return (STAT_NOOP);
}
if (hm.waiting_for_motion_end) { // sync to planner move ends (using callback)
return (STAT_EAGAIN);
}
return (hm.func(hm.axis)); // execute the current homing move
}
/***********************************************************************************
* Homing axis moves and helpers - these execute in sequence for each axis
***********************************************************************************/
/***********************************************************************************
* _homing_axis_start() - get next axis, initialize variables, call the clear
*/
static stat_t _homing_axis_start(int8_t axis) {
// get the first or next axis
if ((axis = _get_next_axis(axis)) < 0) { // axes are done or error
if (axis == -1) { // -1 is done
cm->homing_state = HOMING_HOMED;
return (_set_homing_func(_homing_finalize_exit));
} else if (axis == -2) { // -2 is error
return (_homing_error_exit(-2, STAT_HOMING_ERROR_BAD_OR_NO_AXIS));
}
}
// clear the homed flag for axis so we'll be able to move w/o triggering soft limits
cm->homed[axis] = false;
// trap axis mis-configurations
if (fp_ZERO(cm->a[axis].homing_input)) {
return (_homing_error_exit(axis, STAT_HOMING_ERROR_HOMING_INPUT_MISCONFIGURED));
}
if (fp_ZERO(cm->a[axis].search_velocity)) {
return (_homing_error_exit(axis, STAT_HOMING_ERROR_ZERO_SEARCH_VELOCITY));
}
if (fp_ZERO(cm->a[axis].latch_velocity)) {
return (_homing_error_exit(axis, STAT_HOMING_ERROR_ZERO_LATCH_VELOCITY));
}
// calculate and test travel distance
float travel_distance = std::abs(cm->a[axis].travel_max - cm->a[axis].travel_min) + cm->a[axis].latch_backoff;
if (fp_ZERO(travel_distance)) {
return (_homing_error_exit(axis, STAT_HOMING_ERROR_TRAVEL_MIN_MAX_IDENTICAL));
}
// Nothing to do about direction now that direction is explicit
// However, here's a good place to stash the homing_switch:
hm.homing_input = cm->a[axis].homing_input;
din_handlers[INPUT_ACTION_INTERNAL].registerHandler(&_homing_handler);
hm.axis = axis; // persist the axis
hm.search_velocity = std::abs(cm->a[axis].search_velocity); // search velocity is always positive
hm.latch_velocity = std::abs(cm->a[axis].latch_velocity); // latch velocity is always positive
bool homing_to_max = cm->a[axis].homing_dir;
// setup parameters for positive or negative travel (homing to the max or min switch)
if (homing_to_max) {
hm.search_travel = travel_distance; // search travels in positive direction
hm.latch_backoff = std::abs(cm->a[axis].latch_backoff); // latch travels in positive direction
hm.zero_backoff = -max(0.0f, cm->a[axis].zero_backoff);// zero backoff is negative direction (or zero)
// will set the maximum position
// (plus any negative backoff)
hm.setpoint = cm->a[axis].travel_max + (std::max(0.0f, -cm->a[axis].zero_backoff));
} else {
hm.search_travel = -travel_distance; // search travels in negative direction
hm.latch_backoff = -std::abs(cm->a[axis].latch_backoff); // latch travels in negative direction
hm.zero_backoff = std::max(0.0f, cm->a[axis].zero_backoff); // zero backoff is positive direction (or zero)
// will set the minimum position
// (minus any negative backoff)
hm.setpoint = cm->a[axis].travel_min + (std::max(0.0f, -cm->a[axis].zero_backoff));
}
// if homing is disabled for the axis then skip to the next axis
hm.saved_jerk = cm_get_axis_jerk(axis); // save the max jerk value
return (_set_homing_func(_homing_axis_clear_init)); // perform an initial clear
}
/***********************************************************************************
* _homing_axis_clear_init() - initiate a clear to move off a switch that is thrown at the start
*
* Handle an initial switch closure by backing off the closed switch
* NOTE: clear_init() relies on independent switches per axis (not shared)
*/
static stat_t _homing_axis_clear_init(int8_t axis) // first clear move
{
if (gpio_read_input(hm.homing_input) == INPUT_ACTIVE) { // the switch is closed at startup
// determine if the input switch for this axis is shared w/other axes
for (uint8_t check_axis = AXIS_X; check_axis < AXES; check_axis++) {
if (axis != check_axis && cm->a[check_axis].homing_input == hm.homing_input) {
return (_homing_error_exit(
axis, STAT_HOMING_ERROR_MUST_CLEAR_SWITCHES_BEFORE_HOMING)); // axis cannot be homed
}
}
_homing_axis_move(axis, -hm.latch_backoff, hm.search_velocity); // otherwise back off the switch
}
return (_set_homing_func(_homing_axis_search)); // start the search
}
/***********************************************************************************
* _homing_axis_search() - fast search for switch, closes switch
*/
static stat_t _homing_axis_search(int8_t axis) // drive to switch
{
cm_set_axis_max_jerk(axis, cm->a[axis].jerk_high); // use the high-speed jerk for search onward
_homing_axis_move(axis, hm.search_travel, hm.search_velocity);
return (_set_homing_func(_homing_axis_clear));
}
/***********************************************************************************
* _homing_axis_clear() - clear off the switch
*/
static stat_t _homing_axis_clear(int8_t axis) // drive away from switch at search speed
{
_homing_axis_move(axis, -hm.latch_backoff, hm.search_velocity);
return (_set_homing_func(_homing_axis_latch));
}
/***********************************************************************************
* _homing_axis_latch() - slow drive until until switch closes again
*/
static stat_t _homing_axis_latch(int8_t axis) // drive to switch at low speed
{
_homing_axis_move(axis, hm.latch_backoff, hm.latch_velocity);
return (_set_homing_func(_homing_axis_setpoint_backoff));
}
/***********************************************************************************
* _homing_axis_setpoint_backoff() - backoff to zero or max setpoint position
*/
static stat_t _homing_axis_setpoint_backoff(int8_t axis) //
{
_homing_axis_move(axis, hm.zero_backoff, hm.search_velocity);
return (_set_homing_func(_homing_axis_set_position));
}
/***********************************************************************************
* _homing_axis_set_position() - set axis zero / max and finish up
*/
static stat_t _homing_axis_set_position(int8_t axis)
{
if (hm.set_coordinates) {
cm_set_position_by_axis(axis, hm.setpoint);
cm->homed[axis] = true;
} else { // handle G28.4 cycle - set position to the point of switch closure
float contact_position[AXES];
kn_forward_kinematics(en_get_encoder_snapshot_vector(), contact_position);
_homing_axis_move(axis, contact_position[AXIS_Z], hm.search_velocity);
}
cm_set_axis_max_jerk(axis, hm.saved_jerk); // restore the max jerk value
din_handlers[INPUT_ACTION_INTERNAL].deregisterHandler(&_homing_handler); // end homing mode
return (_set_homing_func(_homing_axis_start));
}
/***********************************************************************************
* _homing_axis_move() - helper that actually executes the above moves
*/
static stat_t _homing_axis_move(int8_t axis, float target, float velocity) {
float vect[] = INIT_AXES_ZEROES;
bool flags[] = INIT_AXES_ZEROES;
hm.waiting_for_motion_end = true;
vect[axis] = target;
flags[axis] = true;
cm_set_feed_rate(velocity);
stat_t status = cm_straight_feed(vect, flags, PROFILE_FAST);
if (status != STAT_OK) {
rpt_exception(status, "Homing move failed. Check min/max settings");
return (_homing_error_exit(axis, STAT_HOMING_CYCLE_FAILED));
}
// the last two arguments are ignored anyway
mp_queue_command(_homing_axis_move_callback, nullptr, nullptr);
return (STAT_EAGAIN);
}
static void _homing_axis_move_callback(float* vect, bool* flag) { hm.waiting_for_motion_end = false; }
/***********************************************************************************
* _homing_error_exit()
*
* Generate an error message. Since the error exit returns via the homing callback
* - and not the main command parser - it requires its own display processing
*/
static stat_t _homing_error_exit(int8_t axis, stat_t status) {
nv_reset_nv_list();
if (axis == -2) {
nv_add_conditional_message((const char*)"Homing error - Bad or no axis(es) specified");
} else {
char msg[NV_MESSAGE_LEN];
sprintf(msg, "%c axis %s", cm_get_axis_char(axis), get_status_message(status));
nv_add_conditional_message(msg);
}
nv_print_list(STAT_HOMING_CYCLE_FAILED, TEXT_MULTILINE_FORMATTED, JSON_RESPONSE_FORMAT);
_homing_finalize_exit(axis);
return (STAT_HOMING_CYCLE_FAILED); // homing state remains HOMING_NOT_HOMED
}
/***********************************************************************************
* _homing_finalize_exit() - helper to finalize homing
*/
static stat_t _homing_finalize_exit(int8_t axis) // third part of return to home
{
cm_set_feed_rate(hm.saved_feed_rate);
cm_set_coord_system(hm.saved_coord_system); // restore to work coordinate system
cm_set_units_mode(hm.saved_units_mode);
cm_set_distance_mode(hm.saved_distance_mode);
cm_set_feed_rate_mode(hm.saved_feed_rate_mode);
cm_set_motion_mode(MODEL, MOTION_MODE_CANCEL_MOTION_MODE);
cm_canned_cycle_end();
return (STAT_OK);
}
/***********************************************************************************
* _get_next_axis() - return next axis in sequence based on axis in arg
*
* Accepts "axis" arg as the current axis; or -1 to retrieve the first axis
* Returns next axis based on "axis" argument and if that axis is flagged for homing in the gf struct
* Returns -1 when all axes have been processed
* Returns -2 if no axes are specified (Gcode calling error)
* Homes Z first, then the rest in sequence
*
* Isolating this function facilitates implementing more complex and
* user-specified axis homing orders
*/
static int8_t _get_next_axis(int8_t axis) {
#if (HOMING_AXES <= 4)
if (axis == -1) { // inelegant brute force solution
if (hm.axis_flags[AXIS_Z]) {
return (AXIS_Z);
}
if (hm.axis_flags[AXIS_X]) {
return (AXIS_X);
}
if (hm.axis_flags[AXIS_Y]) {
return (AXIS_Y);
}
if (hm.axis_flags[AXIS_A]) {
return (AXIS_A);
}
return (-2); // error
} else if (axis == AXIS_Z) {
if (hm.axis_flags[AXIS_X]) {
return (AXIS_X);
}
if (hm.axis_flags[AXIS_Y]) {
return (AXIS_Y);
}
if (hm.axis_flags[AXIS_A]) {
return (AXIS_A);
}
} else if (axis == AXIS_X) {
if (hm.axis_flags[AXIS_Y]) {
return (AXIS_Y);
}
if (hm.axis_flags[AXIS_A]) {
return (AXIS_A);
}
} else if (axis == AXIS_Y) {
if (hm.axis_flags[AXIS_A]) {
return (AXIS_A);
}
}
return (-1); // done
#else
if (axis == -1) {
if (hm.axis_flags[AXIS_Z]) {
return (AXIS_Z);
}
if (hm.axis_flags[AXIS_X]) {
return (AXIS_X);
}
if (hm.axis_flags[AXIS_Y]) {
return (AXIS_Y);
}
if (hm.axis_flags[AXIS_A]) {
return (AXIS_A);
}
if (hm.axis_flags[AXIS_B]) {
return (AXIS_B);
}
if (hm.axis_flags[AXIS_C]) {
return (AXIS_C);
}
return (-2); // error
} else if (axis == AXIS_Z) {
if (hm.axis_flags[AXIS_X]) {
return (AXIS_X);
}
if (hm.axis_flags[AXIS_Y]) {
return (AXIS_Y);
}
if (hm.axis_flags[AXIS_A]) {
return (AXIS_A);
}
if (hm.axis_flags[AXIS_B]) {
return (AXIS_B);
}
if (hm.axis_flags[AXIS_C]) {
return (AXIS_C);
}
} else if (axis == AXIS_X) {
if (hm.axis_flags[AXIS_Y]) {
return (AXIS_Y);
}
if (hm.axis_flags[AXIS_A]) {
return (AXIS_A);
}
if (hm.axis_flags[AXIS_B]) {
return (AXIS_B);
}
if (hm.axis_flags[AXIS_C]) {
return (AXIS_C);
}
} else if (axis == AXIS_Y) {
if (hm.axis_flags[AXIS_A]) {
return (AXIS_A);
}
if (hm.axis_flags[AXIS_B]) {
return (AXIS_B);
}
if (hm.axis_flags[AXIS_C]) {
return (AXIS_C);
}
} else if (axis == AXIS_A) {
if (hm.axis_flags[AXIS_B]) {
return (AXIS_B);
}
if (hm.axis_flags[AXIS_C]) {
return (AXIS_C);
}
} else if (axis == AXIS_B) {
if (hm.axis_flags[AXIS_C]) {
return (AXIS_C);
}
}
return (-1); // done
#endif // (HOMING_AXES <= 4)
}
Reference in New Issue View Git Blame Copy Permalink