src/modules/tools/zprobe/ThreePointStrategy.cpp

   1 #include "ThreePointStrategy.h"
   2 #include "Kernel.h"
   3 #include "Config.h"
   4 #include "Robot.h"
   5 #include "StreamOutputPool.h"
   6 #include "Gcode.h"
   7 #include "checksumm.h"
   8 #include "ConfigValue.h"
   9 #include "PublicDataRequest.h"
  10 #include "PublicData.h"
  11 #include "Conveyor.h"
  12 #include "ZProbe.h"
  13 #include "Plane3D.h"
  14
  15 #include <string>
  16 #include <algorithm>
  17 #include <cstdlib>
  18 #include <cmath>
  19
  20 #define probe_point_1_checksum       CHECKSUM("point1")
  21 #define probe_point_2_checksum       CHECKSUM("point2")
  22 #define probe_point_3_checksum       CHECKSUM("point3")
  23 #define home_checksum                CHECKSUM("home_first")
  24 #define tolerance_checksum           CHECKSUM("tolerance")
  25
  26 ThreePointStrategy::ThreePointStrategy(ZProbe *zprobe) : LevelingStrategy(zprobe)
  27 {
  28     for (int i = 0; i < 3; ++i) {
  29         probe_points[i] = std::make_tuple(NAN, NAN);
  30     }
  31     plane = nullptr;
  32 }
  33
  34 ThreePointStrategy::~ThreePointStrategy()
  35 {
  36     delete plane;
  37 }
  38
  39 bool ThreePointStrategy::handleConfig()
  40 {
  41     // format is xxx,yyy for the probe points
  42     std::string p1 = THEKERNEL->config->value(leveling_strategy_checksum, three_point_leveling_strategy_checksum, probe_point_1_checksum)->by_default("")->as_string();
  43     std::string p2 = THEKERNEL->config->value(leveling_strategy_checksum, three_point_leveling_strategy_checksum, probe_point_2_checksum)->by_default("")->as_string();
  44     std::string p3 = THEKERNEL->config->value(leveling_strategy_checksum, three_point_leveling_strategy_checksum, probe_point_3_checksum)->by_default("")->as_string();
  45     if(!p1.empty()) probe_points[0] = parseXY(p1.c_str());
  46     if(!p2.empty()) probe_points[1] = parseXY(p2.c_str());
  47     if(!p3.empty()) probe_points[2] = parseXY(p3.c_str());
  48
  49     this->home= THEKERNEL->config->value(leveling_strategy_checksum, three_point_leveling_strategy_checksum, home_checksum)->by_default(true)->as_bool();
  50     this->tolerance= THEKERNEL->config->value(leveling_strategy_checksum, three_point_leveling_strategy_checksum, tolerance_checksum)->by_default(0.03F)->as_number();
  51     return true;
  52 }
  53
  54 bool ThreePointStrategy::handleGcode(Gcode *gcode)
  55 {
  56     if(gcode->has_g) {
  57         // G code processing
  58         if( gcode->g == 31 ) { // report status
  59             if(this->plane == nullptr) {
  60                  gcode->stream->printf("Bed leveling plane is not set\n");
  61             }else{
  62                  gcode->stream->printf("Bed leveling plane normal= %f, %f, %f\n", plane->getNormal()[0], plane->getNormal()[1], plane->getNormal()[2]);
  63             }
  64             gcode->stream->printf("Probe is %s\n", zprobe->getProbeStatus() ? "Triggered" : "Not triggered");
  65             return true;
  66
  67         } else if( gcode->g == 32 ) { // three point probe
  68             // first wait for an empty queue i.e. no moves left
  69             THEKERNEL->conveyor->wait_for_empty_queue();
  70             if(!doProbing(gcode->stream)) {
  71                 gcode->stream->printf("Probe failed to complete, probe not triggered or other error\n");
  72             } else {
  73                 gcode->stream->printf("Probe completed, bed plane defined\n");
  74             }
  75             return true;
  76         }
  77
  78     } else if(gcode->has_m) {
  79         if(gcode->m == 557) { // M557 - set probe points eg M557 P0 X30 Y40.5  where P is 0,1,2
  80             int idx = 0;
  81             float x = NAN, y = NAN;
  82             if(gcode->has_letter('P')) idx = gcode->get_value('P');
  83             if(gcode->has_letter('X')) x = gcode->get_value('X');
  84             if(gcode->has_letter('Y')) y = gcode->get_value('Y');
  85             if(idx >= 0 && idx <= 2) {
  86                 probe_points[idx] = std::make_tuple(x, y);
  87             }else{
  88                  gcode->stream->printf("only 3 probe points allowed P0-P2\n");
  89             }
  90             return true;
  91
  92         } else if(gcode->m == 561) { // M561: Set Identity Transform
  93             delete this->plane;
  94             this->plane= nullptr;
  95             // delete the adjustZfnc in robot
  96             THEKERNEL->robot->adjustZfnc= nullptr;
  97             return true;
  98
  99         } else if(gcode->m == 503) {
 100             gcode->stream->printf(";Probe points:\n");
 101             for (int i = 0; i < 3; ++i) {
 102                 float x, y;
 103                 std::tie(x, y) = probe_points[i];
 104                 gcode->stream->printf("M557 P%d X%1.5f Y%1.5f\n", i, x, y);
 105             }
 106             // TODO encode plane if set and M500
 107             return true;
 108
 109         } else if(gcode->m == 999) {
 110             // DEBUG run a test M999 A B C X Y set Z to A B C and test for point at X Y
 111             Vector3 v[3];
 112             float x, y, z, a= 0, b= 0, c= 0;
 113             if(gcode->has_letter('A')) a = gcode->get_value('A');
 114             if(gcode->has_letter('B')) b = gcode->get_value('B');
 115             if(gcode->has_letter('C')) c = gcode->get_value('C');
 116             std::tie(x, y) = probe_points[0]; v[0].set(x, y, a);
 117             std::tie(x, y) = probe_points[1]; v[1].set(x, y, b);
 118             std::tie(x, y) = probe_points[2]; v[2].set(x, y, c);
 119             delete this->plane;
 120             this->plane = new Plane3D(v[0], v[1], v[2]);
 121             gcode->stream->printf("plane normal= %f, %f, %f\n", plane->getNormal()[0], plane->getNormal()[1], plane->getNormal()[2]);
 122             x= 0; y=0;
 123             if(gcode->has_letter('X')) x = gcode->get_value('X');
 124             if(gcode->has_letter('Y')) y = gcode->get_value('Y');
 125             z= getZOffset(x, y);
 126             gcode->stream->printf("z= %f\n", z);
 127             // tell robot to adjust z on each move
 128             THEKERNEL->robot->adjustZfnc= [this](float x, float y) { return this->plane->getz(x, y); };
 129             return true;
 130         }
 131     }
 132
 133     return false;
 134 }
 135
 136 void ThreePointStrategy::homeXY()
 137 {
 138     Gcode gc("G28 X0 Y0", &(StreamOutput::NullStream));
 139     THEKERNEL->call_event(ON_GCODE_RECEIVED, &gc);
 140 }
 141
 142 bool ThreePointStrategy::doProbing(StreamOutput *stream)
 143 {
 144     float x, y;
 145     // check the probe points have been defined
 146     for (int i = 0; i < 3; ++i) {
 147         std::tie(x, y) = probe_points[i];
 148         if(isnan(x) || isnan(y)) {
 149             stream->printf("Probe point P%d has not been defined, use M557 P%d Xnnn Ynnn to define it\n", i, i);
 150             return false;
 151         }
 152     }
 153
 154     // optionally home XY axis first, but allow for manual homing
 155     if(this->home)
 156         homeXY();
 157
 158     // move to the first probe point
 159     std::tie(x, y) = probe_points[0];
 160     zprobe->coordinated_move(x, y, NAN, zprobe->getFastFeedrate());
 161
 162     // for now we use probe to find bed and not the Z min endstop
 163     // TODO this needs to be configurable to use min z or probe
 164
 165     // find bed via probe
 166     int s;
 167     if(!zprobe->run_probe(s, true)) return false;
 168     // do we need to set set to Z == 0 here? as the rest is relative anyway
 169
 170     // move up to specified probe start position
 171     zprobe->coordinated_move(NAN, NAN, zprobe->getProbeHeight(), zprobe->getFastFeedrate(), true); // do a relative move from home to the point above the bed
 172
 173     // probe the three points
 174     Vector3 v[3];
 175     for (int i = 0; i < 3; ++i) {
 176         std::tie(x, y) = probe_points[i];
 177         float z = zprobe->probeDistance(x, y);
 178         if(isnan(z)) return false; // probe failed
 179         z -= zprobe->getProbeHeight(); // relative distance between the probe points
 180         stream->printf("DEBUG: P%d:%1.4f\n", i, z);
 181         v[i].set(x, y, z);
 182     }
 183
 184     // if first point is not within tolerance of probe height report it.
 185     if(abs(v[0][2] - zprobe->getProbeHeight()) > this->tolerance) {
 186         stream->printf("WARNING: probe is not within tolerance\n");
 187     }
 188
 189     // define the plane
 190     delete this->plane;
 191     // check tolerance level here default 0.03mm
 192     auto mm = std::minmax({v[0][2], v[1][2], v[2][2]});
 193     if((mm.second - mm.first) <= this->tolerance) {
 194         this->plane= nullptr; // plane is flat no need to do anything
 195         stream->printf("DEBUG: flat plane\n");
 196         THEKERNEL->robot->adjustZfnc= nullptr;
 197
 198     }else{
 199         this->plane = new Plane3D(v[0], v[1], v[2]);
 200         stream->printf("DEBUG: plane normal= %f, %f, %f\n", plane->getNormal()[0], plane->getNormal()[1], plane->getNormal()[2]);
 201         // set the adjustZfnc in robot
 202         THEKERNEL->robot->adjustZfnc= [this](float x, float y) { return this->plane->getz(x, y); };
 203     }
 204
 205     return true;
 206 }
 207
 208 // find the Z offset for the point on the plane at x, y
 209 float ThreePointStrategy::getZOffset(float x, float y)
 210 {
 211     if(this->plane == nullptr) return NAN;
 212     return this->plane->getz(x, y);
 213 }
 214
 215 // parse a "X,Y" string return x,y
 216 std::tuple<float, float> ThreePointStrategy::parseXY(const char *str)
 217 {
 218     float x = NAN, y = NAN;
 219     char *p;
 220     x = strtof(str, &p);
 221     if(p + 1 < str + strlen(str)) {
 222         y = strtof(p + 1, nullptr);
 223     }
 224     return std::make_tuple(x, y);
 225 }