andrey
/
Marlin_FB4S
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Eliminate goto in gcode_M48

pull/1/head
Scott Lahteine 7 years ago
parent
75e6ead5fd
commit
ac76101ec3
1 changed files with 126 additions and 122 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 248
      Marlin/Marlin_main.cpp

248
Marlin/Marlin_main.cpp
View File

@ -7038,151 +7038,155 @@ inline void gcode_M42() {
// Move to the first point, deploy, and probe // Move to the first point, deploy, and probe
const float t = probe_pt(X_probe_location, Y_probe_location, stow_probe_after_each, verbose_level); const float t = probe_pt(X_probe_location, Y_probe_location, stow_probe_after_each, verbose_level);
if (nan_error(t)) goto FAIL; bool probing_good = !isnan(t);
randomSeed(millis()); if (probing_good) {
randomSeed(millis());
for (uint8_t n = 0; n < n_samples; n++) {
if (n_legs) { for (uint8_t n = 0; n < n_samples; n++) {
const int dir = (random(0, 10) > 5.0) ? -1 : 1; // clockwise or counter clockwise if (n_legs) {
float angle = random(0.0, 360.0); const int dir = (random(0, 10) > 5.0) ? -1 : 1; // clockwise or counter clockwise
const float radius = random( float angle = random(0.0, 360.0);
#if ENABLED(DELTA) const float radius = random(
0.1250000000 * (DELTA_PROBEABLE_RADIUS), #if ENABLED(DELTA)
0.3333333333 * (DELTA_PROBEABLE_RADIUS) 0.1250000000 * (DELTA_PROBEABLE_RADIUS),
#else 0.3333333333 * (DELTA_PROBEABLE_RADIUS)
5.0, 0.125 * min(X_BED_SIZE, Y_BED_SIZE) #else
#endif 5.0, 0.125 * min(X_BED_SIZE, Y_BED_SIZE)
); #endif
);
if (verbose_level > 3) {
SERIAL_ECHOPAIR("Starting radius: ", radius);
SERIAL_ECHOPAIR(" angle: ", angle);
SERIAL_ECHOPGM(" Direction: ");
if (dir > 0) SERIAL_ECHOPGM("Counter-");
SERIAL_ECHOLNPGM("Clockwise");
}
for (uint8_t l = 0; l < n_legs - 1; l++) { if (verbose_level > 3) {
double delta_angle; SERIAL_ECHOPAIR("Starting radius: ", radius);
SERIAL_ECHOPAIR(" angle: ", angle);
SERIAL_ECHOPGM(" Direction: ");
if (dir > 0) SERIAL_ECHOPGM("Counter-");
SERIAL_ECHOLNPGM("Clockwise");
}
if (schizoid_flag) for (uint8_t l = 0; l < n_legs - 1; l++) {
// The points of a 5 point star are 72 degrees apart. We need to double delta_angle;
// skip a point and go to the next one on the star.
delta_angle = dir * 2.0 * 72.0;
else if (schizoid_flag)
// If we do this line, we are just trying to move further // The points of a 5 point star are 72 degrees apart. We need to
// around the circle. // skip a point and go to the next one on the star.
delta_angle = dir * (float) random(25, 45); delta_angle = dir * 2.0 * 72.0;
angle += delta_angle; else
// If we do this line, we are just trying to move further
// around the circle.
delta_angle = dir * (float) random(25, 45);
while (angle > 360.0) // We probably do not need to keep the angle between 0 and 2*PI, but the angle += delta_angle;
angle -= 360.0; // Arduino documentation says the trig functions should not be given values
while (angle < 0.0) // outside of this range. It looks like they behave correctly with
angle += 360.0; // numbers outside of the range, but just to be safe we clamp them.
X_current = X_probe_location - (X_PROBE_OFFSET_FROM_EXTRUDER) + cos(RADIANS(angle)) * radius; while (angle > 360.0) // We probably do not need to keep the angle between 0 and 2*PI, but the
Y_current = Y_probe_location - (Y_PROBE_OFFSET_FROM_EXTRUDER) + sin(RADIANS(angle)) * radius; angle -= 360.0; // Arduino documentation says the trig functions should not be given values
while (angle < 0.0) // outside of this range. It looks like they behave correctly with
angle += 360.0; // numbers outside of the range, but just to be safe we clamp them.
#if DISABLED(DELTA) X_current = X_probe_location - (X_PROBE_OFFSET_FROM_EXTRUDER) + cos(RADIANS(angle)) * radius;
X_current = constrain(X_current, X_MIN_POS, X_MAX_POS); Y_current = Y_probe_location - (Y_PROBE_OFFSET_FROM_EXTRUDER) + sin(RADIANS(angle)) * radius;
Y_current = constrain(Y_current, Y_MIN_POS, Y_MAX_POS);
#else #if DISABLED(DELTA)
// If we have gone out too far, we can do a simple fix and scale the numbers X_current = constrain(X_current, X_MIN_POS, X_MAX_POS);
// back in closer to the origin. Y_current = constrain(Y_current, Y_MIN_POS, Y_MAX_POS);
while (!position_is_reachable_by_probe_xy(X_current, Y_current)) { #else
X_current *= 0.8; // If we have gone out too far, we can do a simple fix and scale the numbers
Y_current *= 0.8; // back in closer to the origin.
if (verbose_level > 3) { while (!position_is_reachable_by_probe_xy(X_current, Y_current)) {
SERIAL_ECHOPAIR("Pulling point towards center:", X_current); X_current *= 0.8;
SERIAL_ECHOLNPAIR(", ", Y_current); Y_current *= 0.8;
if (verbose_level > 3) {
SERIAL_ECHOPAIR("Pulling point towards center:", X_current);
SERIAL_ECHOLNPAIR(", ", Y_current);
}
} }
#endif
if (verbose_level > 3) {
SERIAL_PROTOCOLPGM("Going to:");
SERIAL_ECHOPAIR(" X", X_current);
SERIAL_ECHOPAIR(" Y", Y_current);
SERIAL_ECHOLNPAIR(" Z", current_position[Z_AXIS]);
} }
#endif do_blocking_move_to_xy(X_current, Y_current);
if (verbose_level > 3) { } // n_legs loop
SERIAL_PROTOCOLPGM("Going to:"); } // n_legs
SERIAL_ECHOPAIR(" X", X_current);
SERIAL_ECHOPAIR(" Y", Y_current);
SERIAL_ECHOLNPAIR(" Z", current_position[Z_AXIS]);
}
do_blocking_move_to_xy(X_current, Y_current);
} // n_legs loop
} // n_legs
// Probe a single point // Probe a single point
sample_set[n] = probe_pt(X_probe_location, Y_probe_location, stow_probe_after_each, 0); sample_set[n] = probe_pt(X_probe_location, Y_probe_location, stow_probe_after_each, 0);
if (nan_error(sample_set[n])) goto FAIL;
/** // Break the loop if the probe fails
* Get the current mean for the data points we have so far probing_good = !isnan(sample_set[n]);
*/ if (!probing_good) break;
double sum = 0.0;
for (uint8_t j = 0; j <= n; j++) sum += sample_set[j];
mean = sum / (n + 1);
NOMORE(min, sample_set[n]); /**
NOLESS(max, sample_set[n]); * Get the current mean for the data points we have so far
*/
double sum = 0.0;
for (uint8_t j = 0; j <= n; j++) sum += sample_set[j];
mean = sum / (n + 1);
/** NOMORE(min, sample_set[n]);
* Now, use that mean to calculate the standard deviation for the NOLESS(max, sample_set[n]);
* data points we have so far
*/
sum = 0.0;
for (uint8_t j = 0; j <= n; j++)
sum += sq(sample_set[j] - mean);
sigma = SQRT(sum / (n + 1)); /**
if (verbose_level > 0) { * Now, use that mean to calculate the standard deviation for the
if (verbose_level > 1) { * data points we have so far
SERIAL_PROTOCOL(n + 1); */
SERIAL_PROTOCOLPGM(" of "); sum = 0.0;
SERIAL_PROTOCOL((int)n_samples); for (uint8_t j = 0; j <= n; j++)
SERIAL_PROTOCOLPGM(": z: "); sum += sq(sample_set[j] - mean);
SERIAL_PROTOCOL_F(sample_set[n], 3);
if (verbose_level > 2) { sigma = SQRT(sum / (n + 1));
SERIAL_PROTOCOLPGM(" mean: "); if (verbose_level > 0) {
SERIAL_PROTOCOL_F(mean, 4); if (verbose_level > 1) {
SERIAL_PROTOCOLPGM(" sigma: "); SERIAL_PROTOCOL(n + 1);
SERIAL_PROTOCOL_F(sigma, 6); SERIAL_PROTOCOLPGM(" of ");
SERIAL_PROTOCOLPGM(" min: "); SERIAL_PROTOCOL((int)n_samples);
SERIAL_PROTOCOL_F(min, 3); SERIAL_PROTOCOLPGM(": z: ");
SERIAL_PROTOCOLPGM(" max: "); SERIAL_PROTOCOL_F(sample_set[n], 3);
SERIAL_PROTOCOL_F(max, 3); if (verbose_level > 2) {
SERIAL_PROTOCOLPGM(" range: "); SERIAL_PROTOCOLPGM(" mean: ");
SERIAL_PROTOCOL_F(max-min, 3); SERIAL_PROTOCOL_F(mean, 4);
SERIAL_PROTOCOLPGM(" sigma: ");
SERIAL_PROTOCOL_F(sigma, 6);
SERIAL_PROTOCOLPGM(" min: ");
SERIAL_PROTOCOL_F(min, 3);
SERIAL_PROTOCOLPGM(" max: ");
SERIAL_PROTOCOL_F(max, 3);
SERIAL_PROTOCOLPGM(" range: ");
SERIAL_PROTOCOL_F(max-min, 3);
}
SERIAL_EOL();
} }
SERIAL_EOL();
} }
}
} // End of probe loop } // n_samples loop
}
if (STOW_PROBE()) goto FAIL; STOW_PROBE();
SERIAL_PROTOCOLPGM("Finished!"); if (probing_good) {
SERIAL_EOL(); SERIAL_PROTOCOLLNPGM("Finished!");
if (verbose_level > 0) {
SERIAL_PROTOCOLPGM("Mean: ");
SERIAL_PROTOCOL_F(mean, 6);
SERIAL_PROTOCOLPGM(" Min: ");
SERIAL_PROTOCOL_F(min, 3);
SERIAL_PROTOCOLPGM(" Max: ");
SERIAL_PROTOCOL_F(max, 3);
SERIAL_PROTOCOLPGM(" Range: ");
SERIAL_PROTOCOL_F(max-min, 3);
SERIAL_EOL();
}
if (verbose_level > 0) { SERIAL_PROTOCOLPGM("Standard Deviation: ");
SERIAL_PROTOCOLPGM("Mean: "); SERIAL_PROTOCOL_F(sigma, 6);
SERIAL_PROTOCOL_F(mean, 6); SERIAL_EOL();
SERIAL_PROTOCOLPGM(" Min: ");
SERIAL_PROTOCOL_F(min, 3);
SERIAL_PROTOCOLPGM(" Max: ");
SERIAL_PROTOCOL_F(max, 3);
SERIAL_PROTOCOLPGM(" Range: ");
SERIAL_PROTOCOL_F(max-min, 3);
SERIAL_EOL(); SERIAL_EOL();
} }
SERIAL_PROTOCOLPGM("Standard Deviation: ");
SERIAL_PROTOCOL_F(sigma, 6);
SERIAL_EOL();
SERIAL_EOL();
FAIL:
clean_up_after_endstop_or_probe_move(); clean_up_after_endstop_or_probe_move();
// Re-enable bed level correction if it had been on // Re-enable bed level correction if it had been on

Write Preview
Loading…
Cancel
Save