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