|
@@ -7038,151 +7038,155 @@ inline void gcode_M42() {
|
7038
|
7038
|
|
7039
|
7039
|
// Move to the first point, deploy, and probe
|
7040
|
7040
|
const float t = probe_pt(X_probe_location, Y_probe_location, stow_probe_after_each, verbose_level);
|
7041
|
|
- if (nan_error(t)) goto FAIL;
|
7042
|
|
-
|
7043
|
|
- randomSeed(millis());
|
7044
|
|
-
|
7045
|
|
- for (uint8_t n = 0; n < n_samples; n++) {
|
7046
|
|
- if (n_legs) {
|
7047
|
|
- const int dir = (random(0, 10) > 5.0) ? -1 : 1; // clockwise or counter clockwise
|
7048
|
|
- float angle = random(0.0, 360.0);
|
7049
|
|
- const float radius = random(
|
7050
|
|
- #if ENABLED(DELTA)
|
7051
|
|
- 0.1250000000 * (DELTA_PROBEABLE_RADIUS),
|
7052
|
|
- 0.3333333333 * (DELTA_PROBEABLE_RADIUS)
|
7053
|
|
- #else
|
7054
|
|
- 5.0, 0.125 * min(X_BED_SIZE, Y_BED_SIZE)
|
7055
|
|
- #endif
|
7056
|
|
- );
|
7057
|
|
-
|
7058
|
|
- if (verbose_level > 3) {
|
7059
|
|
- SERIAL_ECHOPAIR("Starting radius: ", radius);
|
7060
|
|
- SERIAL_ECHOPAIR(" angle: ", angle);
|
7061
|
|
- SERIAL_ECHOPGM(" Direction: ");
|
7062
|
|
- if (dir > 0) SERIAL_ECHOPGM("Counter-");
|
7063
|
|
- SERIAL_ECHOLNPGM("Clockwise");
|
7064
|
|
- }
|
|
7041
|
+ bool probing_good = !isnan(t);
|
|
7042
|
+
|
|
7043
|
+ if (probing_good) {
|
|
7044
|
+ randomSeed(millis());
|
|
7045
|
+
|
|
7046
|
+ for (uint8_t n = 0; n < n_samples; n++) {
|
|
7047
|
+ if (n_legs) {
|
|
7048
|
+ const int dir = (random(0, 10) > 5.0) ? -1 : 1; // clockwise or counter clockwise
|
|
7049
|
+ float angle = random(0.0, 360.0);
|
|
7050
|
+ const float radius = random(
|
|
7051
|
+ #if ENABLED(DELTA)
|
|
7052
|
+ 0.1250000000 * (DELTA_PROBEABLE_RADIUS),
|
|
7053
|
+ 0.3333333333 * (DELTA_PROBEABLE_RADIUS)
|
|
7054
|
+ #else
|
|
7055
|
+ 5.0, 0.125 * min(X_BED_SIZE, Y_BED_SIZE)
|
|
7056
|
+ #endif
|
|
7057
|
+ );
|
7065
|
7058
|
|
7066
|
|
- for (uint8_t l = 0; l < n_legs - 1; l++) {
|
7067
|
|
- double delta_angle;
|
|
7059
|
+ if (verbose_level > 3) {
|
|
7060
|
+ SERIAL_ECHOPAIR("Starting radius: ", radius);
|
|
7061
|
+ SERIAL_ECHOPAIR(" angle: ", angle);
|
|
7062
|
+ SERIAL_ECHOPGM(" Direction: ");
|
|
7063
|
+ if (dir > 0) SERIAL_ECHOPGM("Counter-");
|
|
7064
|
+ SERIAL_ECHOLNPGM("Clockwise");
|
|
7065
|
+ }
|
7068
|
7066
|
|
7069
|
|
- if (schizoid_flag)
|
7070
|
|
- // The points of a 5 point star are 72 degrees apart. We need to
|
7071
|
|
- // skip a point and go to the next one on the star.
|
7072
|
|
- delta_angle = dir * 2.0 * 72.0;
|
|
7067
|
+ for (uint8_t l = 0; l < n_legs - 1; l++) {
|
|
7068
|
+ double delta_angle;
|
7073
|
7069
|
|
7074
|
|
- else
|
7075
|
|
- // If we do this line, we are just trying to move further
|
7076
|
|
- // around the circle.
|
7077
|
|
- delta_angle = dir * (float) random(25, 45);
|
|
7070
|
+ if (schizoid_flag)
|
|
7071
|
+ // The points of a 5 point star are 72 degrees apart. We need to
|
|
7072
|
+ // skip a point and go to the next one on the star.
|
|
7073
|
+ delta_angle = dir * 2.0 * 72.0;
|
7078
|
7074
|
|
7079
|
|
- angle += delta_angle;
|
|
7075
|
+ else
|
|
7076
|
+ // If we do this line, we are just trying to move further
|
|
7077
|
+ // around the circle.
|
|
7078
|
+ delta_angle = dir * (float) random(25, 45);
|
7080
|
7079
|
|
7081
|
|
- while (angle > 360.0) // We probably do not need to keep the angle between 0 and 2*PI, but the
|
7082
|
|
- angle -= 360.0; // Arduino documentation says the trig functions should not be given values
|
7083
|
|
- while (angle < 0.0) // outside of this range. It looks like they behave correctly with
|
7084
|
|
- angle += 360.0; // numbers outside of the range, but just to be safe we clamp them.
|
|
7080
|
+ angle += delta_angle;
|
7085
|
7081
|
|
7086
|
|
- X_current = X_probe_location - (X_PROBE_OFFSET_FROM_EXTRUDER) + cos(RADIANS(angle)) * radius;
|
7087
|
|
- Y_current = Y_probe_location - (Y_PROBE_OFFSET_FROM_EXTRUDER) + sin(RADIANS(angle)) * radius;
|
|
7082
|
+ while (angle > 360.0) // We probably do not need to keep the angle between 0 and 2*PI, but the
|
|
7083
|
+ angle -= 360.0; // Arduino documentation says the trig functions should not be given values
|
|
7084
|
+ while (angle < 0.0) // outside of this range. It looks like they behave correctly with
|
|
7085
|
+ angle += 360.0; // numbers outside of the range, but just to be safe we clamp them.
|
7088
|
7086
|
|
7089
|
|
- #if DISABLED(DELTA)
|
7090
|
|
- X_current = constrain(X_current, X_MIN_POS, X_MAX_POS);
|
7091
|
|
- Y_current = constrain(Y_current, Y_MIN_POS, Y_MAX_POS);
|
7092
|
|
- #else
|
7093
|
|
- // If we have gone out too far, we can do a simple fix and scale the numbers
|
7094
|
|
- // back in closer to the origin.
|
7095
|
|
- while (!position_is_reachable_by_probe_xy(X_current, Y_current)) {
|
7096
|
|
- X_current *= 0.8;
|
7097
|
|
- Y_current *= 0.8;
|
7098
|
|
- if (verbose_level > 3) {
|
7099
|
|
- SERIAL_ECHOPAIR("Pulling point towards center:", X_current);
|
7100
|
|
- SERIAL_ECHOLNPAIR(", ", Y_current);
|
|
7087
|
+ X_current = X_probe_location - (X_PROBE_OFFSET_FROM_EXTRUDER) + cos(RADIANS(angle)) * radius;
|
|
7088
|
+ Y_current = Y_probe_location - (Y_PROBE_OFFSET_FROM_EXTRUDER) + sin(RADIANS(angle)) * radius;
|
|
7089
|
+
|
|
7090
|
+ #if DISABLED(DELTA)
|
|
7091
|
+ X_current = constrain(X_current, X_MIN_POS, X_MAX_POS);
|
|
7092
|
+ Y_current = constrain(Y_current, Y_MIN_POS, Y_MAX_POS);
|
|
7093
|
+ #else
|
|
7094
|
+ // If we have gone out too far, we can do a simple fix and scale the numbers
|
|
7095
|
+ // back in closer to the origin.
|
|
7096
|
+ while (!position_is_reachable_by_probe_xy(X_current, Y_current)) {
|
|
7097
|
+ X_current *= 0.8;
|
|
7098
|
+ Y_current *= 0.8;
|
|
7099
|
+ if (verbose_level > 3) {
|
|
7100
|
+ SERIAL_ECHOPAIR("Pulling point towards center:", X_current);
|
|
7101
|
+ SERIAL_ECHOLNPAIR(", ", Y_current);
|
|
7102
|
+ }
|
7101
|
7103
|
}
|
|
7104
|
+ #endif
|
|
7105
|
+ if (verbose_level > 3) {
|
|
7106
|
+ SERIAL_PROTOCOLPGM("Going to:");
|
|
7107
|
+ SERIAL_ECHOPAIR(" X", X_current);
|
|
7108
|
+ SERIAL_ECHOPAIR(" Y", Y_current);
|
|
7109
|
+ SERIAL_ECHOLNPAIR(" Z", current_position[Z_AXIS]);
|
7102
|
7110
|
}
|
7103
|
|
- #endif
|
7104
|
|
- if (verbose_level > 3) {
|
7105
|
|
- SERIAL_PROTOCOLPGM("Going to:");
|
7106
|
|
- SERIAL_ECHOPAIR(" X", X_current);
|
7107
|
|
- SERIAL_ECHOPAIR(" Y", Y_current);
|
7108
|
|
- SERIAL_ECHOLNPAIR(" Z", current_position[Z_AXIS]);
|
7109
|
|
- }
|
7110
|
|
- do_blocking_move_to_xy(X_current, Y_current);
|
7111
|
|
- } // n_legs loop
|
7112
|
|
- } // n_legs
|
|
7111
|
+ do_blocking_move_to_xy(X_current, Y_current);
|
|
7112
|
+ } // n_legs loop
|
|
7113
|
+ } // n_legs
|
7113
|
7114
|
|
7114
|
|
- // Probe a single point
|
7115
|
|
- sample_set[n] = probe_pt(X_probe_location, Y_probe_location, stow_probe_after_each, 0);
|
7116
|
|
- if (nan_error(sample_set[n])) goto FAIL;
|
|
7115
|
+ // Probe a single point
|
|
7116
|
+ sample_set[n] = probe_pt(X_probe_location, Y_probe_location, stow_probe_after_each, 0);
|
7117
|
7117
|
|
7118
|
|
- /**
|
7119
|
|
- * Get the current mean for the data points we have so far
|
7120
|
|
- */
|
7121
|
|
- double sum = 0.0;
|
7122
|
|
- for (uint8_t j = 0; j <= n; j++) sum += sample_set[j];
|
7123
|
|
- mean = sum / (n + 1);
|
|
7118
|
+ // Break the loop if the probe fails
|
|
7119
|
+ probing_good = !isnan(sample_set[n]);
|
|
7120
|
+ if (!probing_good) break;
|
7124
|
7121
|
|
7125
|
|
- NOMORE(min, sample_set[n]);
|
7126
|
|
- NOLESS(max, sample_set[n]);
|
|
7122
|
+ /**
|
|
7123
|
+ * Get the current mean for the data points we have so far
|
|
7124
|
+ */
|
|
7125
|
+ double sum = 0.0;
|
|
7126
|
+ for (uint8_t j = 0; j <= n; j++) sum += sample_set[j];
|
|
7127
|
+ mean = sum / (n + 1);
|
7127
|
7128
|
|
7128
|
|
- /**
|
7129
|
|
- * Now, use that mean to calculate the standard deviation for the
|
7130
|
|
- * data points we have so far
|
7131
|
|
- */
|
7132
|
|
- sum = 0.0;
|
7133
|
|
- for (uint8_t j = 0; j <= n; j++)
|
7134
|
|
- sum += sq(sample_set[j] - mean);
|
|
7129
|
+ NOMORE(min, sample_set[n]);
|
|
7130
|
+ NOLESS(max, sample_set[n]);
|
7135
|
7131
|
|
7136
|
|
- sigma = SQRT(sum / (n + 1));
|
7137
|
|
- if (verbose_level > 0) {
|
7138
|
|
- if (verbose_level > 1) {
|
7139
|
|
- SERIAL_PROTOCOL(n + 1);
|
7140
|
|
- SERIAL_PROTOCOLPGM(" of ");
|
7141
|
|
- SERIAL_PROTOCOL((int)n_samples);
|
7142
|
|
- SERIAL_PROTOCOLPGM(": z: ");
|
7143
|
|
- SERIAL_PROTOCOL_F(sample_set[n], 3);
|
7144
|
|
- if (verbose_level > 2) {
|
7145
|
|
- SERIAL_PROTOCOLPGM(" mean: ");
|
7146
|
|
- SERIAL_PROTOCOL_F(mean, 4);
|
7147
|
|
- SERIAL_PROTOCOLPGM(" sigma: ");
|
7148
|
|
- SERIAL_PROTOCOL_F(sigma, 6);
|
7149
|
|
- SERIAL_PROTOCOLPGM(" min: ");
|
7150
|
|
- SERIAL_PROTOCOL_F(min, 3);
|
7151
|
|
- SERIAL_PROTOCOLPGM(" max: ");
|
7152
|
|
- SERIAL_PROTOCOL_F(max, 3);
|
7153
|
|
- SERIAL_PROTOCOLPGM(" range: ");
|
7154
|
|
- SERIAL_PROTOCOL_F(max-min, 3);
|
|
7132
|
+ /**
|
|
7133
|
+ * Now, use that mean to calculate the standard deviation for the
|
|
7134
|
+ * data points we have so far
|
|
7135
|
+ */
|
|
7136
|
+ sum = 0.0;
|
|
7137
|
+ for (uint8_t j = 0; j <= n; j++)
|
|
7138
|
+ sum += sq(sample_set[j] - mean);
|
|
7139
|
+
|
|
7140
|
+ sigma = SQRT(sum / (n + 1));
|
|
7141
|
+ if (verbose_level > 0) {
|
|
7142
|
+ if (verbose_level > 1) {
|
|
7143
|
+ SERIAL_PROTOCOL(n + 1);
|
|
7144
|
+ SERIAL_PROTOCOLPGM(" of ");
|
|
7145
|
+ SERIAL_PROTOCOL((int)n_samples);
|
|
7146
|
+ SERIAL_PROTOCOLPGM(": z: ");
|
|
7147
|
+ SERIAL_PROTOCOL_F(sample_set[n], 3);
|
|
7148
|
+ if (verbose_level > 2) {
|
|
7149
|
+ SERIAL_PROTOCOLPGM(" mean: ");
|
|
7150
|
+ SERIAL_PROTOCOL_F(mean, 4);
|
|
7151
|
+ SERIAL_PROTOCOLPGM(" sigma: ");
|
|
7152
|
+ SERIAL_PROTOCOL_F(sigma, 6);
|
|
7153
|
+ SERIAL_PROTOCOLPGM(" min: ");
|
|
7154
|
+ SERIAL_PROTOCOL_F(min, 3);
|
|
7155
|
+ SERIAL_PROTOCOLPGM(" max: ");
|
|
7156
|
+ SERIAL_PROTOCOL_F(max, 3);
|
|
7157
|
+ SERIAL_PROTOCOLPGM(" range: ");
|
|
7158
|
+ SERIAL_PROTOCOL_F(max-min, 3);
|
|
7159
|
+ }
|
|
7160
|
+ SERIAL_EOL();
|
7155
|
7161
|
}
|
7156
|
|
- SERIAL_EOL();
|
7157
|
7162
|
}
|
7158
|
|
- }
|
7159
|
7163
|
|
7160
|
|
- } // End of probe loop
|
|
7164
|
+ } // n_samples loop
|
|
7165
|
+ }
|
7161
|
7166
|
|
7162
|
|
- if (STOW_PROBE()) goto FAIL;
|
|
7167
|
+ STOW_PROBE();
|
7163
|
7168
|
|
7164
|
|
- SERIAL_PROTOCOLPGM("Finished!");
|
7165
|
|
- SERIAL_EOL();
|
|
7169
|
+ if (probing_good) {
|
|
7170
|
+ SERIAL_PROTOCOLLNPGM("Finished!");
|
|
7171
|
+
|
|
7172
|
+ if (verbose_level > 0) {
|
|
7173
|
+ SERIAL_PROTOCOLPGM("Mean: ");
|
|
7174
|
+ SERIAL_PROTOCOL_F(mean, 6);
|
|
7175
|
+ SERIAL_PROTOCOLPGM(" Min: ");
|
|
7176
|
+ SERIAL_PROTOCOL_F(min, 3);
|
|
7177
|
+ SERIAL_PROTOCOLPGM(" Max: ");
|
|
7178
|
+ SERIAL_PROTOCOL_F(max, 3);
|
|
7179
|
+ SERIAL_PROTOCOLPGM(" Range: ");
|
|
7180
|
+ SERIAL_PROTOCOL_F(max-min, 3);
|
|
7181
|
+ SERIAL_EOL();
|
|
7182
|
+ }
|
7166
|
7183
|
|
7167
|
|
- if (verbose_level > 0) {
|
7168
|
|
- SERIAL_PROTOCOLPGM("Mean: ");
|
7169
|
|
- SERIAL_PROTOCOL_F(mean, 6);
|
7170
|
|
- SERIAL_PROTOCOLPGM(" Min: ");
|
7171
|
|
- SERIAL_PROTOCOL_F(min, 3);
|
7172
|
|
- SERIAL_PROTOCOLPGM(" Max: ");
|
7173
|
|
- SERIAL_PROTOCOL_F(max, 3);
|
7174
|
|
- SERIAL_PROTOCOLPGM(" Range: ");
|
7175
|
|
- SERIAL_PROTOCOL_F(max-min, 3);
|
|
7184
|
+ SERIAL_PROTOCOLPGM("Standard Deviation: ");
|
|
7185
|
+ SERIAL_PROTOCOL_F(sigma, 6);
|
|
7186
|
+ SERIAL_EOL();
|
7176
|
7187
|
SERIAL_EOL();
|
7177
|
7188
|
}
|
7178
|
7189
|
|
7179
|
|
- SERIAL_PROTOCOLPGM("Standard Deviation: ");
|
7180
|
|
- SERIAL_PROTOCOL_F(sigma, 6);
|
7181
|
|
- SERIAL_EOL();
|
7182
|
|
- SERIAL_EOL();
|
7183
|
|
-
|
7184
|
|
- FAIL:
|
7185
|
|
-
|
7186
|
7190
|
clean_up_after_endstop_or_probe_move();
|
7187
|
7191
|
|
7188
|
7192
|
// Re-enable bed level correction if it had been on
|