[clinton/Smoothieware.git] / src / modules / tools / zprobe / ThreePointStrategy.cpp

/*
    Author: Jim Morris (wolfmanjm@gmail.com)
    License: GPL3 or better see <http://www.gnu.org/licenses/>

    Summary
    -------
    Probes three user specified points on the bed and determines the plane of the bed relative to the probe.
    as the head moves in X and Y it will adjust Z to keep the head tram with the bed.

    Configuration
    -------------
    The strategy must be enabled in the cofnig as well as zprobe.

    leveling-strategy.three-point-leveling.enable         true

    Three probe points must be defined, these are best if they are the three points of an equilateral triangle, as far apart as possible.
    They can be defined in the config file as:-

    leveling-strategy.three-point-leveling.point1         100.0,0.0   # the first probe point (x,y)
    leveling-strategy.three-point-leveling.point2         200.0,200.0 # the second probe point (x,y)
    leveling-strategy.three-point-leveling.point3         0.0,200.0   # the third probe point (x,y)

    or they may be defined (and saved with M500) using M557 P0 X30 Y40.5  where P is 0,1,2

    probe offsets from the nozzle or tool head can be defined with

    leveling-strategy.three-point-leveling.probe_offsets  0,0,0  # probe offsetrs x,y,z

    they may also be set with M565 X0 Y0 Z0

    To force homing in X and Y before G32 does the probe the following can be set in config, this is the default

    leveling-strategy.three-point-leveling.home_first    true   # disable by setting to false

    The probe tolerance can be set using the config line

    leveling-strategy.three-point-leveling.tolerance   0.03    # the probe tolerance in mm, default is 0.03mm


    Usage
    -----
    G29 probes the three probe points and reports the Z at each point, if a plane is active it will be used to level the probe.
    G32 probes the three probe points and defines the bed plane, this will remain in effect until reset or M561
    G31 reports the status

    M557 defines the probe points
    M561 clears the plane and the bed leveling is disabled until G32 is run again
    M565 defines the probe offsets from the nozzle or tool head

    M500 saves the probe points and the probe offsets
    M503 displays the current settings
*/

#include "ThreePointStrategy.h"
#include "Kernel.h"
#include "Config.h"
#include "Robot.h"
#include "StreamOutputPool.h"
#include "Gcode.h"
#include "checksumm.h"
#include "ConfigValue.h"
#include "PublicDataRequest.h"
#include "PublicData.h"
#include "Conveyor.h"
#include "ZProbe.h"
#include "Plane3D.h"
#include "nuts_bolts.h"

#include <string>
#include <algorithm>
#include <cstdlib>
#include <cmath>

#define probe_point_1_checksum       CHECKSUM("point1")
#define probe_point_2_checksum       CHECKSUM("point2")
#define probe_point_3_checksum       CHECKSUM("point3")
#define probe_offsets_checksum       CHECKSUM("probe_offsets")
#define home_checksum                CHECKSUM("home_first")
#define tolerance_checksum           CHECKSUM("tolerance")
#define save_plane_checksum          CHECKSUM("save_plane")

ThreePointStrategy::ThreePointStrategy(ZProbe *zprobe) : LevelingStrategy(zprobe)
{
    for (int i = 0; i < 3; ++i) {
        probe_points[i] = std::make_tuple(NAN, NAN);
    }
    plane = nullptr;
}

ThreePointStrategy::~ThreePointStrategy()
{
    delete plane;
}

bool ThreePointStrategy::handleConfig()
{
    // format is xxx,yyy for the probe points
    std::string p1 = THEKERNEL->config->value(leveling_strategy_checksum, three_point_leveling_strategy_checksum, probe_point_1_checksum)->by_default("")->as_string();
    std::string p2 = THEKERNEL->config->value(leveling_strategy_checksum, three_point_leveling_strategy_checksum, probe_point_2_checksum)->by_default("")->as_string();
    std::string p3 = THEKERNEL->config->value(leveling_strategy_checksum, three_point_leveling_strategy_checksum, probe_point_3_checksum)->by_default("")->as_string();
    if(!p1.empty()) probe_points[0] = parseXY(p1.c_str());
    if(!p2.empty()) probe_points[1] = parseXY(p2.c_str());
    if(!p3.empty()) probe_points[2] = parseXY(p3.c_str());

    // Probe offsets xxx,yyy,zzz
    std::string po = THEKERNEL->config->value(leveling_strategy_checksum, three_point_leveling_strategy_checksum, probe_offsets_checksum)->by_default("0,0,0")->as_string();
    this->probe_offsets= parseXYZ(po.c_str());

    this->home= THEKERNEL->config->value(leveling_strategy_checksum, three_point_leveling_strategy_checksum, home_checksum)->by_default(true)->as_bool();
    this->tolerance= THEKERNEL->config->value(leveling_strategy_checksum, three_point_leveling_strategy_checksum, tolerance_checksum)->by_default(0.03F)->as_number();
    this->save= THEKERNEL->config->value(leveling_strategy_checksum, three_point_leveling_strategy_checksum, save_plane_checksum)->by_default(false)->as_bool();
    return true;
}

bool ThreePointStrategy::handleGcode(Gcode *gcode)
{
    if(gcode->has_g) {
        // G code processing
        if(gcode->g == 29) { // test probe points for level
            if(!test_probe_points(gcode)) {
                gcode->stream->printf("Probe failed to complete, probe not triggered or other error\n");
            }
            return true;

        } else if( gcode->g == 31 ) { // report status
            if(this->plane == nullptr) {
                 gcode->stream->printf("Bed leveling plane is not set\n");
            }else{
                 gcode->stream->printf("Bed leveling plane normal= %f, %f, %f\n", plane->getNormal()[0], plane->getNormal()[1], plane->getNormal()[2]);
            }
            gcode->stream->printf("Probe is %s\n", zprobe->getProbeStatus() ? "Triggered" : "Not triggered");
            return true;

        } else if( gcode->g == 32 ) { // three point probe
            // first wait for an empty queue i.e. no moves left
            THEKERNEL->conveyor->wait_for_idle();

             // clear any existing plane and compensation
            delete this->plane;
            this->plane= nullptr;
            setAdjustFunction(false);

            if(!doProbing(gcode->stream)) {
                gcode->stream->printf("Probe failed to complete, probe not triggered or other error\n");
            } else {
                gcode->stream->printf("Probe completed, bed plane defined\n");
            }
            return true;
        }

    } else if(gcode->has_m) {
        if(gcode->m == 557) { // M557 - set probe points eg M557 P0 X30 Y40.5  where P is 0,1,2
            int idx = 0;
            float x = NAN, y = NAN;
            if(gcode->has_letter('P')) idx = gcode->get_value('P');
            if(gcode->has_letter('X')) x = gcode->get_value('X');
            if(gcode->has_letter('Y')) y = gcode->get_value('Y');
            if(idx >= 0 && idx <= 2) {
                probe_points[idx] = std::make_tuple(x, y);
            }else{
                 gcode->stream->printf("only 3 probe points allowed P0-P2\n");
            }
            return true;

        } else if(gcode->m == 561) { // M561: Set Identity Transform with no parameters, set the saved plane if A B C D are given
            delete this->plane;
            if(gcode->get_num_args() == 0) {
                this->plane= nullptr;
                // delete the compensationTransform in robot
                setAdjustFunction(false);
                gcode->stream->printf("saved plane cleared\n");
            }else{
                // smoothie specific way to restore a saved plane
                uint32_t a,b,c,d;
                a=b=c=d= 0;
                if(gcode->has_letter('A')) a = gcode->get_uint('A');
                if(gcode->has_letter('B')) b = gcode->get_uint('B');
                if(gcode->has_letter('C')) c = gcode->get_uint('C');
                if(gcode->has_letter('D')) d = gcode->get_uint('D');
                this->plane= new Plane3D(a, b, c, d);
                setAdjustFunction(true);
            }
            return true;

        } else if(gcode->m == 565) { // M565: Set Z probe offsets
            float x= 0, y= 0, z= 0;
            if(gcode->has_letter('X')) x = gcode->get_value('X');
            if(gcode->has_letter('Y')) y = gcode->get_value('Y');
            if(gcode->has_letter('Z')) z = gcode->get_value('Z');
            probe_offsets = std::make_tuple(x, y, z);
            return true;

        } else if(gcode->m == 500 || gcode->m == 503) { // M500 save, M503 display
            float x, y, z;
            gcode->stream->printf(";Probe points:\n");
            for (int i = 0; i < 3; ++i) {
                std::tie(x, y) = probe_points[i];
                gcode->stream->printf("M557 P%d X%1.5f Y%1.5f\n", i, x, y);
            }
            gcode->stream->printf(";Probe offsets:\n");
            std::tie(x, y, z) = probe_offsets;
            gcode->stream->printf("M565 X%1.5f Y%1.5f Z%1.5f\n", x, y, z);

            // encode plane and save if set and M500 and enabled
            if(this->save && this->plane != nullptr) {
                if(gcode->m == 500) {
                    uint32_t a, b, c, d;
                    this->plane->encode(a, b, c, d);
                    gcode->stream->printf(";Saved bed plane:\nM561 A%lu B%lu C%lu D%lu \n", a, b, c, d);
                }else{
                    gcode->stream->printf(";The bed plane will be saved on M500\n");
                }
            }
            return true;

        }
        #if 0
         else if(gcode->m == 9999) {
            // DEBUG run a test M9999 A B C X Y set Z to A B C and test for point at X Y
            Vector3 v[3];
            float x, y, z, a= 0, b= 0, c= 0;
            if(gcode->has_letter('A')) a = gcode->get_value('A');
            if(gcode->has_letter('B')) b = gcode->get_value('B');
            if(gcode->has_letter('C')) c = gcode->get_value('C');
            std::tie(x, y) = probe_points[0]; v[0].set(x, y, a);
            std::tie(x, y) = probe_points[1]; v[1].set(x, y, b);
            std::tie(x, y) = probe_points[2]; v[2].set(x, y, c);
            delete this->plane;
            this->plane = new Plane3D(v[0], v[1], v[2]);
            gcode->stream->printf("plane normal= %f, %f, %f\n", plane->getNormal()[0], plane->getNormal()[1], plane->getNormal()[2]);
            x= 0; y=0;
            if(gcode->has_letter('X')) x = gcode->get_value('X');
            if(gcode->has_letter('Y')) y = gcode->get_value('Y');
            z= getZOffset(x, y);
            gcode->stream->printf("z= %f\n", z);
            // tell robot to adjust z on each move
            setAdjustFunction(true);
            return true;
        }
        #endif
    }

    return false;
}

void ThreePointStrategy::homeXY()
{
    Gcode gc("G28 X0 Y0", &(StreamOutput::NullStream));
    THEKERNEL->call_event(ON_GCODE_RECEIVED, &gc);
}

bool ThreePointStrategy::doProbing(StreamOutput *stream)
{
    float x, y;
    // check the probe points have been defined
    for (int i = 0; i < 3; ++i) {
        std::tie(x, y) = probe_points[i];
        if(isnan(x) || isnan(y)) {
            stream->printf("Probe point P%d has not been defined, use M557 P%d Xnnn Ynnn to define it\n", i, i);
            return false;
        }
    }

    // optionally home XY axis first, but allow for manual homing
    if(this->home)
        homeXY();

    // move to the first probe point
    std::tie(x, y) = probe_points[0];
    // offset by the probe XY offset
    x -= std::get<X_AXIS>(this->probe_offsets);
    y -= std::get<Y_AXIS>(this->probe_offsets);
    zprobe->coordinated_move(x, y, NAN, zprobe->getFastFeedrate());

    // for now we use probe to find bed and not the Z min endstop
    // the first probe point becomes Z == 0 effectively so if we home Z or manually set z after this, it needs to be at the first probe point

    // TODO this needs to be configurable to use min z or probe

    // find bed via probe
    float mm;
    if(!zprobe->run_probe(mm, zprobe->getSlowFeedrate())) return false;

    // TODO if using probe then we probably need to set Z to 0 at first probe point, but take into account probe offset from head
    THEROBOT->reset_axis_position(std::get<Z_AXIS>(this->probe_offsets), Z_AXIS);

    // move up to specified probe start position
    zprobe->coordinated_move(NAN, NAN, zprobe->getProbeHeight(), zprobe->getSlowFeedrate()); // move to probe start position

    // probe the three points
    Vector3 v[3];
    for (int i = 0; i < 3; ++i) {
        std::tie(x, y) = probe_points[i];
        // offset moves by the probe XY offset
        float z = zprobe->probeDistance(x-std::get<X_AXIS>(this->probe_offsets), y-std::get<Y_AXIS>(this->probe_offsets));
        if(isnan(z)) return false; // probe failed
        z= zprobe->getProbeHeight() - z; // relative distance between the probe points, lower is negative z
        stream->printf("DEBUG: P%d:%1.4f\n", i, z);
        v[i] = Vector3(x, y, z);
    }

    // if first point is not within tolerance report it, it should ideally be 0
    if(fabsf(v[0][2]) > this->tolerance) {
        stream->printf("WARNING: probe is not within tolerance: %f > %f\n", fabsf(v[0][2]), this->tolerance);
    }

    // define the plane
    delete this->plane;
    // check tolerance level here default 0.03mm
    auto mmx = std::minmax({v[0][2], v[1][2], v[2][2]});
    if((mmx.second - mmx.first) <= this->tolerance) {
        this->plane= nullptr; // plane is flat no need to do anything
        stream->printf("DEBUG: flat plane\n");
        // clear the compensationTransform in robot
        setAdjustFunction(false);

    }else{
        this->plane = new Plane3D(v[0], v[1], v[2]);
        stream->printf("DEBUG: plane normal= %f, %f, %f\n", plane->getNormal()[0], plane->getNormal()[1], plane->getNormal()[2]);
        setAdjustFunction(true);
    }

    return true;
}

// Probes the 3 points and reports heights
bool ThreePointStrategy::test_probe_points(Gcode *gcode)
{
    // check the probe points have been defined
    float max_delta= 0;
    float last_z= NAN;
    for (int i = 0; i < 3; ++i) {
        float x, y;
        std::tie(x, y) = probe_points[i];
        if(isnan(x) || isnan(y)) {
            gcode->stream->printf("Probe point P%d has not been defined, use M557 P%d Xnnn Ynnn to define it\n", i, i);
            return false;
        }

        float z = zprobe->probeDistance(x-std::get<X_AXIS>(this->probe_offsets), y-std::get<Y_AXIS>(this->probe_offsets));
        if(isnan(z)) return false; // probe failed
        gcode->stream->printf("X:%1.4f Y:%1.4f Z:%1.4f\n", x, y, z);

        if(isnan(last_z)) {
            last_z= z;
        }else{
            max_delta= std::max(max_delta, fabsf(z-last_z));
        }
    }

    gcode->stream->printf("max delta: %f\n", max_delta);

    return true;
}

void ThreePointStrategy::setAdjustFunction(bool on)
{
    if(on) {
        // set the compensationTransform in robot
        THEROBOT->compensationTransform= [this](float *target, bool inverse) { if(inverse) target[2] -= this->plane->getz(target[0], target[1]); else target[2] += this->plane->getz(target[0], target[1]); };
    }else{
        // clear it
        THEROBOT->compensationTransform= nullptr;
    }
}

// find the Z offset for the point on the plane at x, y
float ThreePointStrategy::getZOffset(float x, float y)
{
    if(this->plane == nullptr) return NAN;
    return this->plane->getz(x, y);
}

// parse a "X,Y" string return x,y
std::tuple<float, float> ThreePointStrategy::parseXY(const char *str)
{
    float x = NAN, y = NAN;
    char *p;
    x = strtof(str, &p);
    if(p + 1 < str + strlen(str)) {
        y = strtof(p + 1, nullptr);
    }
    return std::make_tuple(x, y);
}

// parse a "X,Y,Z" string return x,y,z tuple
std::tuple<float, float, float> ThreePointStrategy::parseXYZ(const char *str)
{
    float x = 0, y = 0, z= 0;
    char *p;
    x = strtof(str, &p);
    if(p + 1 < str + strlen(str)) {
        y = strtof(p + 1, &p);
        if(p + 1 < str + strlen(str)) {
            z = strtof(p + 1, nullptr);
        }
    }
    return std::make_tuple(x, y, z);
}
Commit	Line	Data
d60189a9 JM	1	/*
	2	Author: Jim Morris (wolfmanjm@gmail.com)
	3	License: GPL3 or better see <http://www.gnu.org/licenses/>
	4
	5	Summary
	6	-------
	7	Probes three user specified points on the bed and determines the plane of the bed relative to the probe.
	8	as the head moves in X and Y it will adjust Z to keep the head tram with the bed.
	9
	10	Configuration
	11	-------------
	12	The strategy must be enabled in the cofnig as well as zprobe.
	13
	14	leveling-strategy.three-point-leveling.enable true
	15
	16	Three probe points must be defined, these are best if they are the three points of an equilateral triangle, as far apart as possible.
	17	They can be defined in the config file as:-
	18
	19	leveling-strategy.three-point-leveling.point1 100.0,0.0 # the first probe point (x,y)
	20	leveling-strategy.three-point-leveling.point2 200.0,200.0 # the second probe point (x,y)
	21	leveling-strategy.three-point-leveling.point3 0.0,200.0 # the third probe point (x,y)
	22
	23	or they may be defined (and saved with M500) using M557 P0 X30 Y40.5 where P is 0,1,2
	24
	25	probe offsets from the nozzle or tool head can be defined with
	26
	27	leveling-strategy.three-point-leveling.probe_offsets 0,0,0 # probe offsetrs x,y,z
	28
	29	they may also be set with M565 X0 Y0 Z0
	30
	31	To force homing in X and Y before G32 does the probe the following can be set in config, this is the default
	32
	33	leveling-strategy.three-point-leveling.home_first true # disable by setting to false
	34
	35	The probe tolerance can be set using the config line
	36
	37	leveling-strategy.three-point-leveling.tolerance 0.03 # the probe tolerance in mm, default is 0.03mm
	38
	39
	40	Usage
	41	-----
c90d92cb	42	G29 probes the three probe points and reports the Z at each point, if a plane is active it will be used to level the probe.
d60189a9 JM	43	G32 probes the three probe points and defines the bed plane, this will remain in effect until reset or M561
	44	G31 reports the status
	45
	46	M557 defines the probe points
	47	M561 clears the plane and the bed leveling is disabled until G32 is run again
	48	M565 defines the probe offsets from the nozzle or tool head
	49
	50	M500 saves the probe points and the probe offsets
	51	M503 displays the current settings
	52	*/
	53
97832d6d JM	54	#include "ThreePointStrategy.h"
	55	#include "Kernel.h"
	56	#include "Config.h"
	57	#include "Robot.h"
	58	#include "StreamOutputPool.h"
	59	#include "Gcode.h"
	60	#include "checksumm.h"
	61	#include "ConfigValue.h"
	62	#include "PublicDataRequest.h"
	63	#include "PublicData.h"
	64	#include "Conveyor.h"
	65	#include "ZProbe.h"
a5542cae	66	#include "Plane3D.h"
cef9acea	67	#include "nuts_bolts.h"
97832d6d JM	68
	69	#include <string>
	70	#include <algorithm>
0e44e7d7	71	#include <cstdlib>
a5542cae	72	#include <cmath>
97832d6d JM	73
	74	#define probe_point_1_checksum CHECKSUM("point1")
	75	#define probe_point_2_checksum CHECKSUM("point2")
	76	#define probe_point_3_checksum CHECKSUM("point3")
cef9acea	77	#define probe_offsets_checksum CHECKSUM("probe_offsets")
5fdf2c47 JM	78	#define home_checksum CHECKSUM("home_first")
5fdf2c47 JM	79	#define tolerance_checksum CHECKSUM("tolerance")
230079d4	80	#define save_plane_checksum CHECKSUM("save_plane")
97832d6d	81
a5542cae JM	82	ThreePointStrategy::ThreePointStrategy(ZProbe *zprobe) : LevelingStrategy(zprobe)
	83	{
	84	for (int i = 0; i < 3; ++i) {
	85	probe_points[i] = std::make_tuple(NAN, NAN);
	86	}
	87	plane = nullptr;
	88	}
	89
	90	ThreePointStrategy::~ThreePointStrategy()
	91	{
	92	delete plane;
	93	}
97832d6d JM	94
	95	bool ThreePointStrategy::handleConfig()
	96	{
0e44e7d7	97	// format is xxx,yyy for the probe points
a5542cae JM	98	std::string p1 = THEKERNEL->config->value(leveling_strategy_checksum, three_point_leveling_strategy_checksum, probe_point_1_checksum)->by_default("")->as_string();
	99	std::string p2 = THEKERNEL->config->value(leveling_strategy_checksum, three_point_leveling_strategy_checksum, probe_point_2_checksum)->by_default("")->as_string();
	100	std::string p3 = THEKERNEL->config->value(leveling_strategy_checksum, three_point_leveling_strategy_checksum, probe_point_3_checksum)->by_default("")->as_string();
	101	if(!p1.empty()) probe_points[0] = parseXY(p1.c_str());
	102	if(!p2.empty()) probe_points[1] = parseXY(p2.c_str());
	103	if(!p3.empty()) probe_points[2] = parseXY(p3.c_str());
5fdf2c47	104
cef9acea JM	105	// Probe offsets xxx,yyy,zzz
	106	std::string po = THEKERNEL->config->value(leveling_strategy_checksum, three_point_leveling_strategy_checksum, probe_offsets_checksum)->by_default("0,0,0")->as_string();
	107	this->probe_offsets= parseXYZ(po.c_str());
	108
5fdf2c47 JM	109	this->home= THEKERNEL->config->value(leveling_strategy_checksum, three_point_leveling_strategy_checksum, home_checksum)->by_default(true)->as_bool();
5fdf2c47 JM	110	this->tolerance= THEKERNEL->config->value(leveling_strategy_checksum, three_point_leveling_strategy_checksum, tolerance_checksum)->by_default(0.03F)->as_number();
230079d4	111	this->save= THEKERNEL->config->value(leveling_strategy_checksum, three_point_leveling_strategy_checksum, save_plane_checksum)->by_default(false)->as_bool();
97832d6d JM	112	return true;
	113	}
	114
	115	bool ThreePointStrategy::handleGcode(Gcode *gcode)
	116	{
2f809c40	117	if(gcode->has_g) {
97832d6d	118	// G code processing
d5f17053 JM	119	if(gcode->g == 29) { // test probe points for level
	120	if(!test_probe_points(gcode)) {
	121	gcode->stream->printf("Probe failed to complete, probe not triggered or other error\n");
	122	}
	123	return true;
	124
	125	} else if( gcode->g == 31 ) { // report status
2f809c40 JM	126	if(this->plane == nullptr) {
	127	gcode->stream->printf("Bed leveling plane is not set\n");
	128	}else{
	129	gcode->stream->printf("Bed leveling plane normal= %f, %f, %f\n", plane->getNormal()[0], plane->getNormal()[1], plane->getNormal()[2]);
	130	}
	131	gcode->stream->printf("Probe is %s\n", zprobe->getProbeStatus() ? "Triggered" : "Not triggered");
	132	return true;
	133
	134	} else if( gcode->g == 32 ) { // three point probe
97832d6d	135	// first wait for an empty queue i.e. no moves left
04782655	136	THEKERNEL->conveyor->wait_for_idle();
e0be983d JM	137
	138	// clear any existing plane and compensation
	139	delete this->plane;
	140	this->plane= nullptr;
	141	setAdjustFunction(false);
	142
0e44e7d7	143	if(!doProbing(gcode->stream)) {
25dc6344	144	gcode->stream->printf("Probe failed to complete, probe not triggered or other error\n");
a5542cae	145	} else {
0e44e7d7 JM	146	gcode->stream->printf("Probe completed, bed plane defined\n");
0e44e7d7 JM	147	}
97832d6d JM	148	return true;
	149	}
	150
	151	} else if(gcode->has_m) {
0e44e7d7	152	if(gcode->m == 557) { // M557 - set probe points eg M557 P0 X30 Y40.5 where P is 0,1,2
a5542cae JM	153	int idx = 0;
	154	float x = NAN, y = NAN;
	155	if(gcode->has_letter('P')) idx = gcode->get_value('P');
	156	if(gcode->has_letter('X')) x = gcode->get_value('X');
	157	if(gcode->has_letter('Y')) y = gcode->get_value('Y');
0e44e7d7	158	if(idx >= 0 && idx <= 2) {
a5542cae	159	probe_points[idx] = std::make_tuple(x, y);
25dc6344 JM	160	}else{
25dc6344 JM	161	gcode->stream->printf("only 3 probe points allowed P0-P2\n");
0e44e7d7 JM	162	}
	163	return true;
	164
230079d4	165	} else if(gcode->m == 561) { // M561: Set Identity Transform with no parameters, set the saved plane if A B C D are given
2f809c40	166	delete this->plane;
230079d4 JM	167	if(gcode->get_num_args() == 0) {
230079d4 JM	168	this->plane= nullptr;
3632a517	169	// delete the compensationTransform in robot
230079d4	170	setAdjustFunction(false);
6c3ab9b0	171	gcode->stream->printf("saved plane cleared\n");
230079d4	172	}else{
6c3ab9b0	173	// smoothie specific way to restore a saved plane
230079d4 JM	174	uint32_t a,b,c,d;
	175	a=b=c=d= 0;
	176	if(gcode->has_letter('A')) a = gcode->get_uint('A');
	177	if(gcode->has_letter('B')) b = gcode->get_uint('B');
	178	if(gcode->has_letter('C')) c = gcode->get_uint('C');
	179	if(gcode->has_letter('D')) d = gcode->get_uint('D');
	180	this->plane= new Plane3D(a, b, c, d);
a7fbf20c	181	setAdjustFunction(true);
230079d4	182	}
2f809c40 JM	183	return true;
2f809c40 JM	184
cef9acea JM	185	} else if(gcode->m == 565) { // M565: Set Z probe offsets
	186	float x= 0, y= 0, z= 0;
	187	if(gcode->has_letter('X')) x = gcode->get_value('X');
	188	if(gcode->has_letter('Y')) y = gcode->get_value('Y');
	189	if(gcode->has_letter('Z')) z = gcode->get_value('Z');
	190	probe_offsets = std::make_tuple(x, y, z);
	191	return true;
	192
230079d4	193	} else if(gcode->m == 500 \|\| gcode->m == 503) { // M500 save, M503 display
cef9acea	194	float x, y, z;
0e44e7d7 JM	195	gcode->stream->printf(";Probe points:\n");
0e44e7d7 JM	196	for (int i = 0; i < 3; ++i) {
0e44e7d7 JM	197	std::tie(x, y) = probe_points[i];
	198	gcode->stream->printf("M557 P%d X%1.5f Y%1.5f\n", i, x, y);
	199	}
cef9acea JM	200	gcode->stream->printf(";Probe offsets:\n");
	201	std::tie(x, y, z) = probe_offsets;
	202	gcode->stream->printf("M565 X%1.5f Y%1.5f Z%1.5f\n", x, y, z);
	203
230079d4	204	// encode plane and save if set and M500 and enabled
eaade38b JM	205	if(this->save && this->plane != nullptr) {
	206	if(gcode->m == 500) {
	207	uint32_t a, b, c, d;
	208	this->plane->encode(a, b, c, d);
	209	gcode->stream->printf(";Saved bed plane:\nM561 A%lu B%lu C%lu D%lu \n", a, b, c, d);
	210	}else{
	211	gcode->stream->printf(";The bed plane will be saved on M500\n");
	212	}
230079d4	213	}
0e44e7d7	214	return true;
5fdf2c47	215
d60189a9 JM	216	}
d60189a9 JM	217	#if 0
728477c4 JM	218	else if(gcode->m == 9999) {
728477c4 JM	219	// DEBUG run a test M9999 A B C X Y set Z to A B C and test for point at X Y
5fdf2c47 JM	220	Vector3 v[3];
	221	float x, y, z, a= 0, b= 0, c= 0;
	222	if(gcode->has_letter('A')) a = gcode->get_value('A');
	223	if(gcode->has_letter('B')) b = gcode->get_value('B');
	224	if(gcode->has_letter('C')) c = gcode->get_value('C');
	225	std::tie(x, y) = probe_points[0]; v[0].set(x, y, a);
	226	std::tie(x, y) = probe_points[1]; v[1].set(x, y, b);
	227	std::tie(x, y) = probe_points[2]; v[2].set(x, y, c);
	228	delete this->plane;
	229	this->plane = new Plane3D(v[0], v[1], v[2]);
25dc6344	230	gcode->stream->printf("plane normal= %f, %f, %f\n", plane->getNormal()[0], plane->getNormal()[1], plane->getNormal()[2]);
5fdf2c47 JM	231	x= 0; y=0;
	232	if(gcode->has_letter('X')) x = gcode->get_value('X');
	233	if(gcode->has_letter('Y')) y = gcode->get_value('Y');
	234	z= getZOffset(x, y);
	235	gcode->stream->printf("z= %f\n", z);
5e45206a	236	// tell robot to adjust z on each move
14568182	237	setAdjustFunction(true);
5fdf2c47	238	return true;
0e44e7d7	239	}
d60189a9	240	#endif
97832d6d JM	241	}
	242
	243	return false;
	244	}
	245
ff7e9858 JM	246	void ThreePointStrategy::homeXY()
	247	{
	248	Gcode gc("G28 X0 Y0", &(StreamOutput::NullStream));
	249	THEKERNEL->call_event(ON_GCODE_RECEIVED, &gc);
	250	}
	251
0e44e7d7 JM	252	bool ThreePointStrategy::doProbing(StreamOutput *stream)
	253	{
	254	float x, y;
	255	// check the probe points have been defined
	256	for (int i = 0; i < 3; ++i) {
	257	std::tie(x, y) = probe_points[i];
	258	if(isnan(x) \|\| isnan(y)) {
a5542cae JM	259	stream->printf("Probe point P%d has not been defined, use M557 P%d Xnnn Ynnn to define it\n", i, i);
a5542cae JM	260	return false;
0e44e7d7 JM	261	}
	262	}
	263
5fdf2c47 JM	264	// optionally home XY axis first, but allow for manual homing
	265	if(this->home)
	266	homeXY();
ff7e9858 JM	267
	268	// move to the first probe point
	269	std::tie(x, y) = probe_points[0];
cef9acea JM	270	// offset by the probe XY offset
	271	x -= std::get<X_AXIS>(this->probe_offsets);
	272	y -= std::get<Y_AXIS>(this->probe_offsets);
ff7e9858	273	zprobe->coordinated_move(x, y, NAN, zprobe->getFastFeedrate());
0e44e7d7	274
ff7e9858	275	// for now we use probe to find bed and not the Z min endstop
cef9acea JM	276	// the first probe point becomes Z == 0 effectively so if we home Z or manually set z after this, it needs to be at the first probe point
cef9acea JM	277
ff7e9858 JM	278	// TODO this needs to be configurable to use min z or probe
	279
	280	// find bed via probe
6d142b73	281	float mm;
71f0df19	282	if(!zprobe->run_probe(mm, zprobe->getSlowFeedrate())) return false;
ff7e9858	283
14568182	284	// TODO if using probe then we probably need to set Z to 0 at first probe point, but take into account probe offset from head
c8bac202	285	THEROBOT->reset_axis_position(std::get<Z_AXIS>(this->probe_offsets), Z_AXIS);
14568182	286
ff7e9858	287	// move up to specified probe start position
c58d32a8	288	zprobe->coordinated_move(NAN, NAN, zprobe->getProbeHeight(), zprobe->getSlowFeedrate()); // move to probe start position
0e44e7d7 JM	289
	290	// probe the three points
	291	Vector3 v[3];
	292	for (int i = 0; i < 3; ++i) {
	293	std::tie(x, y) = probe_points[i];
cef9acea JM	294	// offset moves by the probe XY offset
cef9acea JM	295	float z = zprobe->probeDistance(x-std::get<X_AXIS>(this->probe_offsets), y-std::get<Y_AXIS>(this->probe_offsets));
0e44e7d7	296	if(isnan(z)) return false; // probe failed
cef9acea	297	z= zprobe->getProbeHeight() - z; // relative distance between the probe points, lower is negative z
0e44e7d7	298	stream->printf("DEBUG: P%d:%1.4f\n", i, z);
bfcf42fe	299	v[i] = Vector3(x, y, z);
0e44e7d7 JM	300	}
0e44e7d7 JM	301
cef9acea	302	// if first point is not within tolerance report it, it should ideally be 0
8b261cdc JM	303	if(fabsf(v[0][2]) > this->tolerance) {
8b261cdc JM	304	stream->printf("WARNING: probe is not within tolerance: %f > %f\n", fabsf(v[0][2]), this->tolerance);
5fdf2c47	305	}
80605954	306
0e44e7d7 JM	307	// define the plane
0e44e7d7 JM	308	delete this->plane;
5fdf2c47	309	// check tolerance level here default 0.03mm
6d142b73 JM	310	auto mmx = std::minmax({v[0][2], v[1][2], v[2][2]});
6d142b73 JM	311	if((mmx.second - mmx.first) <= this->tolerance) {
ff7e9858 JM	312	this->plane= nullptr; // plane is flat no need to do anything
ff7e9858 JM	313	stream->printf("DEBUG: flat plane\n");
3632a517	314	// clear the compensationTransform in robot
14568182	315	setAdjustFunction(false);
33742399	316
ff7e9858 JM	317	}else{
	318	this->plane = new Plane3D(v[0], v[1], v[2]);
	319	stream->printf("DEBUG: plane normal= %f, %f, %f\n", plane->getNormal()[0], plane->getNormal()[1], plane->getNormal()[2]);
14568182	320	setAdjustFunction(true);
ff7e9858	321	}
0e44e7d7 JM	322
	323	return true;
	324	}
	325
d5f17053 JM	326	// Probes the 3 points and reports heights
	327	bool ThreePointStrategy::test_probe_points(Gcode *gcode)
	328	{
	329	// check the probe points have been defined
	330	float max_delta= 0;
	331	float last_z= NAN;
	332	for (int i = 0; i < 3; ++i) {
	333	float x, y;
	334	std::tie(x, y) = probe_points[i];
	335	if(isnan(x) \|\| isnan(y)) {
	336	gcode->stream->printf("Probe point P%d has not been defined, use M557 P%d Xnnn Ynnn to define it\n", i, i);
	337	return false;
	338	}
	339
	340	float z = zprobe->probeDistance(x-std::get<X_AXIS>(this->probe_offsets), y-std::get<Y_AXIS>(this->probe_offsets));
	341	if(isnan(z)) return false; // probe failed
	342	gcode->stream->printf("X:%1.4f Y:%1.4f Z:%1.4f\n", x, y, z);
	343
	344	if(isnan(last_z)) {
	345	last_z= z;
	346	}else{
	347	max_delta= std::max(max_delta, fabsf(z-last_z));
	348	}
	349	}
	350
	351	gcode->stream->printf("max delta: %f\n", max_delta);
	352
	353	return true;
	354	}
	355
14568182 JM	356	void ThreePointStrategy::setAdjustFunction(bool on)
	357	{
	358	if(on) {
3632a517	359	// set the compensationTransform in robot
8fe38353	360	THEROBOT->compensationTransform= [this](float *target, bool inverse) { if(inverse) target[2] -= this->plane->getz(target[0], target[1]); else target[2] += this->plane->getz(target[0], target[1]); };
14568182 JM	361	}else{
14568182 JM	362	// clear it
c8bac202	363	THEROBOT->compensationTransform= nullptr;
14568182 JM	364	}
	365	}
	366
0e44e7d7 JM	367	// find the Z offset for the point on the plane at x, y
	368	float ThreePointStrategy::getZOffset(float x, float y)
	369	{
ff7e9858 JM	370	if(this->plane == nullptr) return NAN;
ff7e9858 JM	371	return this->plane->getz(x, y);
0e44e7d7	372	}
97832d6d	373
0e44e7d7	374	// parse a "X,Y" string return x,y
a5542cae JM	375	std::tuple<float, float> ThreePointStrategy::parseXY(const char *str)
	376	{
	377	float x = NAN, y = NAN;
0e44e7d7	378	char *p;
a5542cae JM	379	x = strtof(str, &p);
	380	if(p + 1 < str + strlen(str)) {
	381	y = strtof(p + 1, nullptr);
0e44e7d7 JM	382	}
	383	return std::make_tuple(x, y);
	384	}
cef9acea JM	385
	386	// parse a "X,Y,Z" string return x,y,z tuple
	387	std::tuple<float, float, float> ThreePointStrategy::parseXYZ(const char *str)
	388	{
	389	float x = 0, y = 0, z= 0;
	390	char *p;
	391	x = strtof(str, &p);
	392	if(p + 1 < str + strlen(str)) {
	393	y = strtof(p + 1, &p);
	394	if(p + 1 < str + strlen(str)) {
	395	z = strtof(p + 1, nullptr);
	396	}
	397	}
	398	return std::make_tuple(x, y, z);
	399	}