g2/g2core/cycle_probing.cpp

/*
 * cycle_probing.c - probing cycle extension to canonical_machine.c
 * This file is part of the g2core project
 *
 * Copyright (c) 2010 - 2019 Alden S Hart, Jr., Sarah Tappon, Tom Cauchois, Robert Giseburt
 * With contributions from Other Machine Company.
 *
 * 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 "config.h"
#include "json_parser.h"
#include "text_parser.h"
#include "canonical_machine.h"
#include "kinematics.h"
#include "encoder.h"
#include "spindle.h"
#include "report.h"
#include "gpio.h"
#include "planner.h"
#include "util.h"
#include "xio.h"


/**** Local stuff ****/

#define MINIMUM_PROBE_TRAVEL 0.254      // mm of travel below which the probe will err out

struct pbProbingSingleton {             // persistent probing runtime variables

    // probe target
    float target[AXES];
    bool  flags[AXES];

    // controls for probing cycle
    int8_t probe_input;                 // digital input to read
    bool trip_sense;                    // true if contact CLOSURE trips probe  (true for G38.2 and G38.3)
    bool alarm_flag;                    // true if failure triggers alarm       (true for G38.2 and G38.4)
    bool waiting_for_motion_complete;   // true if waiting for a motion to complete
    stat_t (*func)();                   // binding for callback function state machine

    // saved gcode model state
    cmUnitsMode saved_units_mode;       // G20,G21 setting
    cmDistanceMode saved_distance_mode; // G90,G91 global setting
    bool saved_soft_limits;             // turn off soft limits during probing
    float saved_jerk[AXES];             // saved and restored for each axis
};
static struct pbProbingSingleton pb;

/**** NOTE: global prototypes and other .h info is located in canonical_machine.h ****/

static stat_t _probing_start();
static stat_t _probing_backoff();
static stat_t _probing_finish();
static stat_t _probing_exception_exit(stat_t status);
static stat_t _probe_move(const float target[], const bool flags[]);
static void _send_probe_report(void);

void _prepare_for_probe();
void _store_probe_position();

/**** JSON INTERFACE *******************************************************************
 * cm_set_probe() - a command to tell it to store the current point as a probe point
 */

stat_t cm_set_probe(nvObj_t *nv)
{
    if (!fp_ZERO(nv->value_int)) {
        nv->valuetype = TYPE_BOOLEAN;
        nv->value_int = true;

        _prepare_for_probe();
        cm->probe_state[0] = PROBE_SUCCEEDED;
        _store_probe_position();
    }
    return (STAT_OK);
}


/**** HELPERS ***************************************************************************
 * _prepare_for_probe() - rotate the stored probes in preperation for storing a new probe
 * _store_probe_position() - store the position as a finalized probe
 */

void _prepare_for_probe() {
    // if the previous probe succeeded, roll probes to the next position
    if (cm->probe_state[0] == PROBE_SUCCEEDED) {
        for (uint8_t n = PROBES_STORED - 1; n > 0; n--) {
            cm->probe_state[n] = cm->probe_state[n - 1];
            for (uint8_t axis = 0; axis < AXES; axis++) {
                cm->probe_results[n][axis] = cm->probe_results[n - 1][axis];
            }
        }
    }
}

void _store_probe_position() {
    for (uint8_t axis = 0; axis < AXES; axis++) {
        cm->probe_results[0][axis] = cm_get_absolute_position(ACTIVE_MODEL, axis);
    }
}

// helper
static void _motion_end_callback(float* vect, bool* flag)
{
    pb.waiting_for_motion_complete = false;
}

/*
 * _probing_handler - a gpioDigitalInputHandler to capture pin change events
 *   Will be registered only during homing mode - see gpio.h for more info
 */
gpioDigitalInputHandler _probing_handler {
    [&](const bool state, const inputEdgeFlag edge, const uint8_t triggering_pin_number) {
        if (cm->cycle_type != CYCLE_PROBE) { return GPIO_NOT_HANDLED; }
        if (triggering_pin_number != pb.probe_input) { return GPIO_NOT_HANDLED; }

        en_take_encoder_snapshot();
        cm_request_feedhold(FEEDHOLD_TYPE_SKIP, FEEDHOLD_EXIT_STOP);

        return GPIO_HANDLED; // DO NOT allow others to see this notice (particularly limits)
    },
    100,    // priority
    nullptr // next - nullptr to start with
};

/***********************************************************************************
 **** G38.x Probing Cycle **********************************************************
 ***********************************************************************************/

/***********************************************************************************
 * cm_probing_cycle_start()    - G38.x probing cycle using contact (digital input)
 *
 *  cm_probe_cycle_start() is the entry point for a probe cycle. It checks for
 *  some errors, sets up the cycle, then prevents any new commands from queuing
 *  to the planner so that the planner can move to a stop and report motion stopped.
 *
 *  --- Some further details ---
 *
 *  Start with the G38.x documentation, which is not repeated here.
 *  https://github.com/synthetos/g2/wiki/Gcode-Probes
 *
 *  When the probe input fires the input interrupt takes a snapshot of the internal
 *  encoders, then requests a "high speed" feedhold. We then run forward kinematics
 *  on the encoder snapshot to get the reported position. We also execute a move
 *  from the final position (after the feedhold) back to the point we report.
 *
 *  Additionally, we record the last PROBES_STORED (at least 3) probe points that
 *  succeeded. The current or most recent probe (be it success, failure, or
 *  in-progress) occupies one of those positions, which is the one reported by the
 *  "prb" JSON.
 *
 *  Internally the active/most recent probe is stored in cm->probe_results[0] and
 *  cm->probe_state[0]. Before a new probe is started, if cm->probe_state[0] ==
 *  PROBE_SUCCEEDED, then 0 rolls to 1, and 1 to 2, up to PROBES_STORED-1.
 *  The oldest probe is "lost."
 *
 *  Alarms and exceptions: It is *not* necessarily an error condition for the
 *  probe not to trigger, depending on the G38.x command received. It is an error
 *  for the limit or homing switches to fire, or for some other configuration error.
 *  These are trapped and cause Alarms.
 *
 *  Note: Spindle and coolant are not affected during probing. Some probes require
 *  the spindle to be turned on.
 *
 *  Note: When coding a cycle (like this one) you get to perform one queued
 *  move per entry into the continuation, then you must exit. We put two buffer
 *  items into the queue: We queue a move, then we queue a "command" that simply
 *  sets a flag in the probing object (pb.waiting_for_motion_end) to tell us that
 *  the move has finished. The runtime has a special exception for probing and
 *  homing where if a move is interrupted it clears it out of the queue.
 *
 *  You must also wait until the last move has actually completed before declaring
 *  the cycle to be done. Otherwise there is a nasty race condition in the
 *  _controller_HSM() that may accept the next command before the position of the
 *  final move has been recorded in the Gcode model. That's part of what what the
 *  wait_for_motion_end callback is about.
 */

uint8_t cm_straight_probe(float target[], bool flags[], bool trip_sense, bool alarm_flag)
{
    // error if zero feed rate
    if (fp_ZERO(cm->gm.feed_rate)) {
        return(cm_alarm(STAT_FEEDRATE_NOT_SPECIFIED, "Feedrate is zero"));
    }

    // error if no axes specified
    if (!(flags[AXIS_X] | flags[AXIS_Y] | flags[AXIS_Z] |
          flags[AXIS_A] | flags[AXIS_B] | flags[AXIS_C])) {
        return(cm_alarm(STAT_AXIS_IS_MISSING, "Axis is missing"));
    }

    // initialize the probe input; error if no probe input specified
    if ((pb.probe_input = cm->probe_input) == -1) {
        return(cm_alarm(STAT_NO_PROBE_INPUT_CONFIGURED, "Probe input not configured"));
    }

    // setup
    pb.alarm_flag = alarm_flag;             // set true to enable probe fail alarms (all exceptions alarm regardless)
    pb.trip_sense = trip_sense;             // set to sense of "tripped" contact
    pb.func = _probing_start;               // bind probing start function

    cm_set_model_target(target, flags);     // convert target to canonical form taking all offsets into account
    copy_vector(pb.target, cm->gm.target);   // cm_set_model_target() sets target in gm, move it to pb
    copy_vector(pb.flags, flags);           // set axes involved in the move

     _prepare_for_probe();

    // clear the old probe results
    clear_vector(cm->probe_results[0]);      // NOTE: relying on cm->probe_results will not detect a probe to 0,0,0.

    // queue a function to let us know when we can start probing
    cm->probe_state[0] = PROBE_WAITING;      // wait until planner queue empties before starting movement
    pb.waiting_for_motion_complete = true;
    mp_queue_command(_motion_end_callback, nullptr, nullptr);  // note: these args are ignored
    return (STAT_OK);
}

/***********************************************************************************
 *  cm_probing_cycle_callback() - handle probing progress
 *
 *  This is called regularly from the controller. If we report NOOP, the controller
 *  will continue with other tasks. Otherwise the controller will not execute any
 *  later tasks, including read any more "data".
 */

uint8_t cm_probing_cycle_callback(void)
{
    if ((cm->cycle_type != CYCLE_PROBE) && (cm->probe_state[0] != PROBE_WAITING)) {
        return (STAT_NOOP);                 // exit if not in a probing cycle
    }
    if (pb.waiting_for_motion_complete) {   // sync to planner move ends (using callback)
        // check for alarm or shutdown and recover
        // expect the alarm or shutdown to flush the queue, so don't worry about that
        if (cm->machine_state == MACHINE_ALARM || cm->machine_state == MACHINE_SHUTDOWN) {
            cm_abort_probing(cm);
            return (STAT_OK);
        }
       return (STAT_EAGAIN);
    }
    return (pb.func());                     // execute the current probing move
}

/***********************************************************************************
 * cm_abort_probing() - something big happened, the queue is flushing, reset to non-probing state
 *
 *  Note: No need to worry about resetting states we saved (if we are actually probing), since
 *  when this is called everything is being reset anyway.
 *
 *  The task here is to stop sending homing moves to the planner, and ensure we can re-enter
 *  homing fresh without issue.
 *
 */

void cm_abort_probing(cmMachine_t *_cm) {
    // The queue has been emptied, the callback is lost, and all of the states we saved are reset
    pb.waiting_for_motion_complete = false;

    // The cycle_type may have already been changed, but if it hasn't do so now
    if (_cm->cycle_type == CYCLE_PROBE) {
        _cm->cycle_type = CYCLE_NONE;
    }

    // Also clean up the latest probe record
    if (cm->probe_state[0] == PROBE_WAITING) {
        // we can stop waiting
        cm->probe_state[0] = PROBE_FAILED;
    }

    // This is idempotent - if it's not there, no worries
    din_handlers[INPUT_ACTION_INTERNAL].deregisterHandler(&_probing_handler);

    pb.func = nullptr;
}

/***********************************************************************************
 * _probe_move()          - function to execute probing moves
 * _motion_end_callback() - callback completes when motion has stopped
 *
 *  target[] must be provided in machine canonical coordinates (absolute, mm)
 *  cm_set_absolute_override() also zeros work offsets, which are restored on exit.
 */

static stat_t _probe_move(const float target[], const bool flags[])
{
    cm_set_absolute_override(MODEL, ABSOLUTE_OVERRIDE_ON_DISPLAY_WITH_OFFSETS);
    pb.waiting_for_motion_complete = true;          // set this BEFORE the motion starts
    cm_straight_feed(target, flags, PROFILE_FAST);  // NB: feed rate was set earlier, so it's OK
    mp_queue_command(_motion_end_callback, nullptr, nullptr); // the last two arguments are ignored anyway
    return (STAT_EAGAIN);
}


/***********************************************************************************
 * _probing_start() - start the probe or skip it if contact is already active
 */

static uint8_t _probing_start()
{
    // so optimistic... ;)
    // These initializations are required before starting the probing cycle but must
    // be done after the planner has exhausted all current moves as they affect the
    // runtime (specifically the digital input modes). Side effects would include
    // limit switches initiating probe actions instead of just killing movement

    cm->probe_state[0] = PROBE_FAILED;
    cm->machine_state = MACHINE_CYCLE;
    cm->cycle_type = CYCLE_PROBE;

    // save relevant non-axis parameters from Gcode model
    pb.saved_distance_mode = (cmDistanceMode)cm_get_distance_mode(ACTIVE_MODEL);
    pb.saved_units_mode = (cmUnitsMode)cm_get_units_mode(ACTIVE_MODEL);
    pb.saved_soft_limits = cm_get_soft_limits();
    cm_set_soft_limits(false);

    // set working values
    cm_set_distance_mode(ABSOLUTE_DISTANCE_MODE);
    cm_set_units_mode(MILLIMETERS);

    // Save the current jerk settings & change to the high-speed jerk settings
    for (uint8_t axis = 0; axis < AXES; axis++) {
        pb.saved_jerk[axis] = cm_get_axis_jerk(axis);  // save the max jerk value
        cm_set_axis_max_jerk(axis, cm->a[axis].jerk_high);  // use the high-speed jerk for probe
    }

    // Error if the probe target is too close to the current position
    if (get_axis_vector_length(cm->gmx.position, pb.target) < MINIMUM_PROBE_TRAVEL) {
        return(_probing_exception_exit(STAT_PROBE_TRAVEL_TOO_SMALL));
    }

    din_handlers[INPUT_ACTION_INTERNAL].registerHandler(&_probing_handler);

    // Get initial probe state, and don't probe if we're already tripped.
    // If the initial input is the same as the trip_sense it's an error.
    if (pb.trip_sense == gpio_read_input(pb.probe_input)) {     // == is exclusive nor for booleans
        return(_probing_exception_exit(STAT_PROBE_IS_ALREADY_TRIPPED));
    }

    // Everything checks out. Run the probe move
    _probe_move(pb.target, pb.flags);
    pb.func = _probing_backoff;
    return (STAT_EAGAIN);
}

/***********************************************************************************
 * _probing_backoff() - runs after the probe move, whether it contacted or not
 *
 * Back off to the measured touch position captured by encoder snapshot
 */

static stat_t _probing_backoff()
{
    // Test if we've contacted. If so, do the backoff. Convert the contact position
    // captured from the encoder in step space to steps to mm. The encoder snapshot
    // was taken by input interrupt at the time of closure.

    if (pb.trip_sense == gpio_read_input(pb.probe_input)) {  // exclusive or for booleans
        cm->probe_state[0] = PROBE_SUCCEEDED;
        float contact_position[AXES];
        kn_forward_kinematics(en_get_encoder_snapshot_vector(), contact_position);
        _probe_move(contact_position, pb.flags);   // NB: feed rate is the same as the probe move
    } else {
        cm->probe_state[0] = PROBE_FAILED;
    }
    pb.func = _probing_finish;
    return (STAT_EAGAIN);
}

/***********************************************************************************
 * _probe_restore_settings() - helper for both exits
 * _probing_exception_exit() - exit for probes that hit an exception
 * _probing_finish()         - exit for successful and non-contacted (failed) probes
 */

static void _probe_restore_settings()
{
    din_handlers[INPUT_ACTION_INTERNAL].deregisterHandler(&_probing_handler);

    for (uint8_t axis = 0; axis < AXES; axis++) {       // restore axis jerks
        cm->a[axis].jerk_max = pb.saved_jerk[axis];
    }
    cm_set_absolute_override(MODEL, ABSOLUTE_OVERRIDE_OFF); // release abs override and restore work offsets
    cm_set_distance_mode(pb.saved_distance_mode);
    cm_set_units_mode(pb.saved_units_mode);
    cm_set_soft_limits(pb.saved_soft_limits);

    cm_set_motion_mode(MODEL, MOTION_MODE_CANCEL_MOTION_MODE);// cancel feed modes used during probing
    cm_canned_cycle_end();
    sr_request_status_report(SR_REQUEST_IMMEDIATE);         // do this last
}

static stat_t _probing_exception_exit(stat_t status)
{
    _probe_restore_settings();          // cleanup first
    return (cm_alarm(status, "probe error"));
}

static stat_t _probing_finish()
{
    _probe_restore_settings();          // cleanup first

    _store_probe_position();

    // handle failed probes - successful probes already set the flag
    if (cm->probe_state[0] == PROBE_FAILED) {
        if (pb.alarm_flag) {
            cm_alarm(STAT_PROBE_CYCLE_FAILED, "probing failed");
        }
    }
    _send_probe_report();
    return (STAT_OK);
}

/*
 * _probe_report() - report probe results - must update results vector first
 */

static void _send_probe_report() {

    if (cm->probe_report_enable) {
        // If probe was successful the 'e' word == 1, otherwise e == 0 to signal an error
        char  buf[256];
        char* bufp = buf;
        bufp += sprintf(bufp, "{\"prb\":{\"e\":%i,", (int)cm->probe_state[0]);
        bufp += sprintf(bufp, "\"x\":%0.5f,", cm->probe_results[0][AXIS_X]);
        bufp += sprintf(bufp, "\"y\":%0.5f,", cm->probe_results[0][AXIS_Y]);
        bufp += sprintf(bufp, "\"z\":%0.5f,", cm->probe_results[0][AXIS_Z]);
        bufp += sprintf(bufp, "\"a\":%0.5f,", cm->probe_results[0][AXIS_A]);
        bufp += sprintf(bufp, "\"b\":%0.5f,", cm->probe_results[0][AXIS_B]);
        bufp += sprintf(bufp, "\"c\":%0.5f", cm->probe_results[0][AXIS_C]);
        bufp += sprintf(bufp, "}}\n");
        xio_writeline(buf);
    }
}

/*
 * cm_get_prbr() - get probe report enable setting
 * cm_set_prbr() - set probe report enable setting
 */

stat_t cm_get_prbr(nvObj_t *nv)
{
    nv->value_int = cm->probe_report_enable;
    nv->valuetype = TYPE_INTEGER;               // ++++ should probably be type boolean
    return (STAT_OK);
}

stat_t cm_set_prbr(nvObj_t *nv)
{
    cm->probe_report_enable = nv->value_int;
    return (STAT_OK);
}