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