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 == 32 ) { // three point probe
  59             // first wait for an empty queue i.e. no moves left
  60             THEKERNEL->conveyor->wait_for_empty_queue();
  61             if(!doProbing(gcode->stream)) {
  62                 gcode->stream->printf("Probe failed to complete, probe not triggered\n");
  63             } else {
  64                 gcode->stream->printf("Probe completed, bed plane defined\n");
  65             }
  66             return true;
  67         }
  68
  69     } else if(gcode->has_m) {
  70         if(gcode->m == 557) { // M557 - set probe points eg M557 P0 X30 Y40.5  where P is 0,1,2
  71             int idx = 0;
  72             float x = NAN, y = NAN;
  73             if(gcode->has_letter('P')) idx = gcode->get_value('P');
  74             if(gcode->has_letter('X')) x = gcode->get_value('X');
  75             if(gcode->has_letter('Y')) y = gcode->get_value('Y');
  76             if(idx >= 0 && idx <= 2) {
  77                 probe_points[idx] = std::make_tuple(x, y);
  78             }
  79             return true;
  80
  81         } else if(gcode->m == 503) {
  82             gcode->stream->printf(";Probe points:\n");
  83             for (int i = 0; i < 3; ++i) {
  84                 float x, y;
  85                 std::tie(x, y) = probe_points[i];
  86                 gcode->stream->printf("M557 P%d X%1.5f Y%1.5f\n", i, x, y);
  87             }
  88             // TODO encode plane if set and M500
  89             return true;
  90
  91         } else if(gcode->m == 999) {
  92             // DEBUG run a test M999 A B C X Y set Z to A B C and test for point at X Y
  93             Vector3 v[3];
  94             float x, y, z, a= 0, b= 0, c= 0;
  95             if(gcode->has_letter('A')) a = gcode->get_value('A');
  96             if(gcode->has_letter('B')) b = gcode->get_value('B');
  97             if(gcode->has_letter('C')) c = gcode->get_value('C');
  98             std::tie(x, y) = probe_points[0]; v[0].set(x, y, a);
  99             std::tie(x, y) = probe_points[1]; v[1].set(x, y, b);
 100             std::tie(x, y) = probe_points[2]; v[2].set(x, y, c);
 101             delete this->plane;
 102             this->plane = new Plane3D(v[0], v[1], v[2]);
 103             x= 0; y=0;
 104             if(gcode->has_letter('X')) x = gcode->get_value('X');
 105             if(gcode->has_letter('Y')) y = gcode->get_value('Y');
 106             z= getZOffset(x, y);
 107             gcode->stream->printf("z= %f\n", z);
 108             return true;
 109         }
 110     }
 111
 112     return false;
 113 }
 114
 115 void ThreePointStrategy::homeXY()
 116 {
 117     Gcode gc("G28 X0 Y0", &(StreamOutput::NullStream));
 118     THEKERNEL->call_event(ON_GCODE_RECEIVED, &gc);
 119 }
 120
 121 bool ThreePointStrategy::doProbing(StreamOutput *stream)
 122 {
 123     float x, y;
 124     // check the probe points have been defined
 125     for (int i = 0; i < 3; ++i) {
 126         std::tie(x, y) = probe_points[i];
 127         if(isnan(x) || isnan(y)) {
 128             stream->printf("Probe point P%d has not been defined, use M557 P%d Xnnn Ynnn to define it\n", i, i);
 129             return false;
 130         }
 131     }
 132
 133     // optionally home XY axis first, but allow for manual homing
 134     if(this->home)
 135         homeXY();
 136
 137     // move to the first probe point
 138     std::tie(x, y) = probe_points[0];
 139     zprobe->coordinated_move(x, y, NAN, zprobe->getFastFeedrate());
 140
 141     // for now we use probe to find bed and not the Z min endstop
 142     // TODO this needs to be configurable to use min z or probe
 143
 144     // find bed via probe
 145     int s;
 146     if(!zprobe->run_probe(s, true)) return false;
 147     // do we need to set set to Z == 0 here? as the rest is relative anyway
 148
 149     // move up to specified probe start position
 150     zprobe->coordinated_move(NAN, NAN, zprobe->getProbeHeight(), zprobe->getFastFeedrate(), true); // do a relative move from home to the point above the bed
 151
 152     // probe the three points
 153     Vector3 v[3];
 154     for (int i = 0; i < 3; ++i) {
 155         std::tie(x, y) = probe_points[i];
 156         float z = zprobe->probeDistance(x, y);
 157         if(isnan(z)) return false; // probe failed
 158         z -= zprobe->getProbeHeight(); // relative distance between the probe points
 159         stream->printf("DEBUG: P%d:%1.4f\n", i, z);
 160         v[i].set(x, y, z);
 161     }
 162
 163     // if first point is not within tolerance of probe height report it.
 164     if(abs(v[0][2] - zprobe->getProbeHeight()) > this->tolerance) {
 165         stream->printf("WARNING: probe is not within tolerance\n");
 166     }
 167
 168     // define the plane
 169     delete this->plane;
 170     // check tolerance level here default 0.03mm
 171     auto mm = std::minmax({v[0][2], v[1][2], v[2][2]});
 172     if((mm.second - mm.first) <= this->tolerance) {
 173         this->plane= nullptr; // plane is flat no need to do anything
 174         stream->printf("DEBUG: flat plane\n");
 175         // THEKERNEL->robot->adjustZfnc= nullptr;
 176     }else{
 177         this->plane = new Plane3D(v[0], v[1], v[2]);
 178         stream->printf("DEBUG: plane normal= %f, %f, %f\n", plane->getNormal()[0], plane->getNormal()[1], plane->getNormal()[2]);
 179         // TODO set the adjustZfnc in robot
 180         // THEKERNEL->robot->adjustZfnc= [this](float x, float y) { return this->getZOffset(x, y); }
 181     }
 182
 183     return true;
 184 }
 185
 186 // find the Z offset for the point on the plane at x, y
 187 float ThreePointStrategy::getZOffset(float x, float y)
 188 {
 189     if(this->plane == nullptr) return NAN;
 190     return this->plane->getz(x, y);
 191 }
 192
 193 // parse a "X,Y" string return x,y
 194 std::tuple<float, float> ThreePointStrategy::parseXY(const char *str)
 195 {
 196     float x = NAN, y = NAN;
 197     char *p;
 198     x = strtof(str, &p);
 199     if(p + 1 < str + strlen(str)) {
 200         y = strtof(p + 1, nullptr);
 201     }
 202     return std::make_tuple(x, y);
 203 }