Browse Source

leaner code for probe routine

LVD-AC 8 years ago
parent
commit
471a321624
1 changed files with 90 additions and 94 deletions
  1. 90
    94
      Marlin/Marlin_main.cpp

+ 90
- 94
Marlin/Marlin_main.cpp View File

@@ -61,7 +61,7 @@
61 61
  * G30 - Single Z probe, probes bed at X Y location (defaults to current XY location)
62 62
  * G31 - Dock sled (Z_PROBE_SLED only)
63 63
  * G32 - Undock sled (Z_PROBE_SLED only)
64
- * G33 - Delta '1-4-7-point' auto calibration : "G33 P<points> <A> <O> <T> V<verbose>" (Requires DELTA)
64
+ * G33 - Delta '1-4-7-point' auto calibration : "G33 V<verbose> P<points> <A> <O> <T>" (Requires DELTA)
65 65
  * G38 - Probe target - similar to G28 except it uses the Z_MIN_PROBE for all three axes
66 66
  * G90 - Use Absolute Coordinates
67 67
  * G91 - Use Relative Coordinates
@@ -4994,8 +4994,12 @@ inline void gcode_G28() {
4994 4994
      * G33 - Delta '1-4-7-point' auto calibration (Requires DELTA)
4995 4995
      * 
4996 4996
      * Usage:
4997
-     *   G33 <Pn> <A> <O> <T> <Vn>
4997
+     *   G33 <Vn> <Pn> <A> <O> <T>
4998 4998
      *   
4999
+     *     Vn = verbose level (n=0-2 default 1)
5000
+     *          n=0 dry-run mode: setting + probe results / no calibration
5001
+     *          n=1 settings 
5002
+     *          n=2 setting + probe results 
4999 5003
      *     Pn = n=-7 -> +7 : n*n probe points
5000 5004
      *          calibrates height ('1 point'), endstops, and delta radius ('4 points') 
5001 5005
      *          and tower angles with n > 2 ('7+ points')
@@ -5006,10 +5010,6 @@ inline void gcode_G28() {
5006 5010
      *     A  = abort 1 point delta height calibration after 1 probe
5007 5011
      *     O  = use oposite tower points instead of tower points with 4 point calibration
5008 5012
      *     T  = do not calibrate tower angles with 7+ point calibration
5009
-     *     Vn = verbose level (n=0-2 default 1)
5010
-     *          n=0 dry-run mode: no calibration
5011
-     *          n=1 settings 
5012
-     *          n=2 setting + probe results 
5013 5013
      */
5014 5014
     inline void gcode_G33() {
5015 5015
 
@@ -5019,14 +5019,14 @@ inline void gcode_G28() {
5019 5019
         set_bed_leveling_enabled(false);
5020 5020
       #endif
5021 5021
 
5022
-      int8_t pp = code_seen('P') ? code_value_int() : DELTA_CALIBRATION_DEFAULT_POINTS,
5023
-                   probe_mode = (WITHIN(pp, 1, 7)) ? pp : DELTA_CALIBRATION_DEFAULT_POINTS;
5022
+      int8_t pp = (code_seen('P') ? code_value_int() : DELTA_CALIBRATION_DEFAULT_POINTS),
5023
+             probe_mode = (WITHIN(pp, 1, 7) ? pp : DELTA_CALIBRATION_DEFAULT_POINTS);
5024 5024
 
5025
-      probe_mode = (code_seen('A') && probe_mode == 1) ? -probe_mode : probe_mode;
5026
-      probe_mode = (code_seen('O') && probe_mode == 2) ? -probe_mode : probe_mode;
5027
-      probe_mode = (code_seen('T') && probe_mode > 2) ? -probe_mode : probe_mode;
5025
+      probe_mode = (code_seen('A') && probe_mode == 1 ? -probe_mode : probe_mode);
5026
+      probe_mode = (code_seen('O') && probe_mode == 2 ? -probe_mode : probe_mode);
5027
+      probe_mode = (code_seen('T') && probe_mode > 2 ? -probe_mode : probe_mode);
5028 5028
       
5029
-      int8_t verbose_level = code_seen('V') ? code_value_byte() : 1;
5029
+      int8_t verbose_level = (code_seen('V') ? code_value_byte() : 1);
5030 5030
 
5031 5031
       if (!WITHIN(verbose_level, 0, 2)) verbose_level = 1;
5032 5032
 
@@ -5034,7 +5034,7 @@ inline void gcode_G28() {
5034 5034
 
5035 5035
       const static char save_message[] PROGMEM = "Save with M500 and/or copy to Configuration.h";
5036 5036
       float test_precision,
5037
-            zero_std_dev = verbose_level ? 999.0 : 0.0,                                          // 0.0 in dry-run mode : forced end
5037
+            zero_std_dev = (verbose_level ? 999.0 : 0.0), // 0.0 in dry-run mode : forced end
5038 5038
             e_old[XYZ] = {
5039 5039
               endstop_adj[A_AXIS],
5040 5040
               endstop_adj[B_AXIS],
@@ -5046,11 +5046,17 @@ inline void gcode_G28() {
5046 5046
             beta_old = delta_tower_angle_trim[B_AXIS]; 
5047 5047
       int8_t iterations = 0,
5048 5048
              probe_points = abs(probe_mode);
5049
-      bool _1_point = (probe_points <= 1),
5050
-           _7_point = (probe_mode > 2),
5051
-           o_mode = (probe_mode == -2),
5052
-           towers = (probe_points > 2 || probe_mode == 2),
5053
-           opposites = (probe_points > 2 || o_mode);
5049
+      const bool pp_equals_1 = (probe_points == 1),
5050
+                 pp_equals_2 = (probe_points == 2),
5051
+                 pp_equals_3 = (probe_points == 3),
5052
+                 pp_equals_4 = (probe_points == 4),
5053
+                 pp_equals_5 = (probe_points == 5),
5054
+                 pp_equals_6 = (probe_points == 6),
5055
+                 pp_equals_7 = (probe_points == 7),
5056
+                 pp_greather_2 = (probe_points > 2),
5057
+                 pp_greather_3 = (probe_points > 3),
5058
+                 pp_greather_4 = (probe_points > 4),
5059
+                 pp_greather_5 = (probe_points > 5);
5054 5060
  
5055 5061
       // print settings
5056 5062
 
@@ -5061,7 +5067,7 @@ inline void gcode_G28() {
5061 5067
       LCD_MESSAGEPGM("Checking... AC");
5062 5068
 
5063 5069
       SERIAL_PROTOCOLPAIR(".Height:", DELTA_HEIGHT + home_offset[Z_AXIS]);
5064
-      if (!_1_point) {
5070
+      if (!pp_equals_1) {
5065 5071
         SERIAL_PROTOCOLPGM("    Ex:");
5066 5072
         if (endstop_adj[A_AXIS] >= 0) SERIAL_CHAR('+');
5067 5073
         SERIAL_PROTOCOL_F(endstop_adj[A_AXIS], 2);
@@ -5074,7 +5080,7 @@ inline void gcode_G28() {
5074 5080
         SERIAL_PROTOCOLPAIR("    Radius:", delta_radius);
5075 5081
       }
5076 5082
       SERIAL_EOL;
5077
-      if (_7_point) {
5083
+      if (probe_mode > 2) { // negative disables tower angles
5078 5084
         SERIAL_PROTOCOLPGM(".Tower angle :    Tx:");
5079 5085
         if (delta_tower_angle_trim[A_AXIS] >= 0) SERIAL_CHAR('+');
5080 5086
         SERIAL_PROTOCOL_F(delta_tower_angle_trim[A_AXIS], 2);
@@ -5092,78 +5098,69 @@ inline void gcode_G28() {
5092 5098
       do {
5093 5099
 
5094 5100
         float z_at_pt[13] = { 0 },
5095
-              S1 = z_at_pt[0],
5096
-              S2 = sq(S1);
5097
-        int16_t N = 1;
5098
-        bool _4_probe = (probe_points == 2),
5099
-             _7_probe = (probe_points > 2),
5100
-             center_probe = (probe_points != 3 &&  probe_points != 6),
5101
-             multi_circle = (probe_points > 4),
5102
-             diff_circle = (probe_points > 5),
5103
-             max_circle = (probe_points > 6),
5104
-             intermediates = (probe_points == 4 || diff_circle);
5105
-
5106
-        setup_for_endstop_or_probe_move();
5101
+              S1 = 0.0,
5102
+              S2 = 0.0;
5103
+        int16_t N = 0;
5104
+
5107 5105
         test_precision = zero_std_dev;
5108 5106
         iterations++;
5109 5107
 
5110 5108
         // probe the points
5111 5109
 
5112
-        int16_t center_points = 0;
5113
-
5114
-        if (center_probe) {  // probe centre
5110
+        if (!pp_equals_3 && !pp_equals_6) { // probe the centre
5111
+          setup_for_endstop_or_probe_move();
5115 5112
           z_at_pt[0] += probe_pt(0.0, 0.0 , true, 1);
5116
-          center_points = 1;
5113
+          clean_up_after_endstop_or_probe_move();
5117 5114
         }
5118
-
5119
-        int16_t step_axis = (multi_circle) ? 2 : 4,
5120
-                start = (multi_circle) ? 11 : 9;                                              
5121
-        if (_7_probe) {  // probe extra 3 or 6 centre points
5122
-          for (int8_t axis = start; axis > 0; axis -= step_axis) {              
5115
+        if (pp_greather_2) { // probe extra centre points
5116
+          for (int8_t axis = (pp_greather_4 ? 11 : 9); axis > 0; axis -= (pp_greather_4 ? 2 : 4)) {              
5117
+            setup_for_endstop_or_probe_move();
5123 5118
             z_at_pt[0] += probe_pt(
5124 5119
               cos(RADIANS(180 + 30 * axis)) * (0.1 * delta_calibration_radius),
5125 5120
               sin(RADIANS(180 + 30 * axis)) * (0.1 * delta_calibration_radius), true, 1);
5121
+            clean_up_after_endstop_or_probe_move();
5126 5122
           }
5127
-          center_points += (multi_circle) ? 6 : 3;  // average centre points
5128
-          z_at_pt[0] /= center_points;
5123
+          z_at_pt[0] /= (pp_equals_5 ? 7 : probe_points);
5129 5124
         }
5130
-
5131
-        start = (o_mode) ? 3 : 1;
5132
-        step_axis = (_4_probe) ? 4 : (intermediates) ? 1 : 2;
5133
-
5134
-        if (!_1_point) {
5135
-          float start_circles = (max_circle) ? -1.5 : (multi_circle) ? -1 : 0,  // one or multi radius points
5125
+        if (!pp_equals_1) {  // probe the radius
5126
+          float start_circles = (pp_equals_7 ? -1.5 : pp_equals_6 || pp_equals_5 ? -1 : 0),
5136 5127
                 end_circles = -start_circles;
5137 5128
           bool zig_zag = true;
5138
-          for (uint8_t axis = start; axis < 13; axis += step_axis) {                             // probes 3, 6 or 12 points on the calibration radius
5139
-            for (float circles = start_circles ; circles <= end_circles; circles++)              // one or multi radius points
5129
+          for (uint8_t axis = (probe_mode == -2 ? 3 : 1); axis < 13; 
5130
+               axis += (pp_equals_2 ? 4 : pp_equals_3 || pp_equals_5 ? 2 : 1)) {
5131
+            for (float circles = start_circles ; circles <= end_circles; circles++) {
5132
+              setup_for_endstop_or_probe_move();
5140 5133
               z_at_pt[axis] += probe_pt(
5141
-                cos(RADIANS(180 + 30 * axis)) * (1 + circles * 0.1 * ((zig_zag) ? 1 : -1)) * delta_calibration_radius, 
5142
-                sin(RADIANS(180 + 30 * axis)) * (1 + circles * 0.1 * ((zig_zag) ? 1 : -1)) * delta_calibration_radius, true, 1);
5143
-
5144
-            if (diff_circle) {
5145
-              start_circles += (zig_zag) ? 0.5 : -0.5;  // opposites: one radius point less
5146
-              end_circles = -start_circles;
5134
+                cos(RADIANS(180 + 30 * axis)) * 
5135
+                (1 + circles * 0.1 * (zig_zag ? 1 : -1)) * delta_calibration_radius, 
5136
+                sin(RADIANS(180 + 30 * axis)) * 
5137
+                (1 + circles * 0.1 * (zig_zag ? 1 : -1)) * delta_calibration_radius, true, 1);
5138
+              clean_up_after_endstop_or_probe_move();
5147 5139
             }
5140
+            start_circles += (pp_greather_5 ? (zig_zag ? 0.5 : -0.5) : 0);
5141
+            end_circles = -start_circles;
5148 5142
             zig_zag = !zig_zag;
5149
-            if (multi_circle) z_at_pt[axis] /= (zig_zag) ? 3.0 : 2.0;  // average between radius points
5143
+            z_at_pt[axis] /= (pp_equals_7 ? (zig_zag ? 4.0 : 3.0) :
5144
+                              pp_equals_6 ? (zig_zag ? 3.0 : 2.0) : pp_equals_5 ? 3 : 1);
5150 5145
           }
5151 5146
         }
5152
-        if (intermediates) step_axis = 2;
5153
-
5154
-        for (uint8_t axis = start; axis < 13; axis += step_axis) {                               // average half intermediates to towers and opposites
5155
-          if (intermediates)
5147
+        if (pp_greather_3 && !pp_equals_5) // average intermediates to tower and opposites
5148
+          for (uint8_t axis = 1; axis < 13; axis += 2)
5156 5149
             z_at_pt[axis] = (z_at_pt[axis] + (z_at_pt[axis + 1] + z_at_pt[(axis + 10) % 12 + 1]) / 2.0) / 2.0;
5157 5150
 
5158
-          S1 += z_at_pt[axis];
5159
-          S2 += sq(z_at_pt[axis]);
5160
-          N++;
5161
-        }
5151
+        S1 += z_at_pt[0];
5152
+        S2 += sq(z_at_pt[0]);
5153
+        N++;
5154
+        if (!pp_equals_1) // std dev from zero plane
5155
+          for (uint8_t axis = 1; axis < 13; axis += (pp_equals_2 ? 4 : 2)) {
5156
+            S1 += z_at_pt[axis];
5157
+            S2 += sq(z_at_pt[axis]);
5158
+            N++;
5159
+          }
5160
+        zero_std_dev = round(sqrt(S2 / N) * 1000.0) / 1000.0 + 0.00001;
5162 5161
 
5163 5162
         // Solve matrices
5164 5163
 
5165
-        zero_std_dev = round(sqrt(S2 / N) * 1000.0) / 1000.0 + 0.00001;                          // deviation from zero plane
5166
-
5167 5164
         if (zero_std_dev < test_precision) {
5168 5165
           COPY(e_old, endstop_adj);
5169 5166
           dr_old = delta_radius;
@@ -5173,11 +5170,10 @@ inline void gcode_G28() {
5173 5170
 
5174 5171
           float e_delta[XYZ] = { 0.0 }, r_delta = 0.0,
5175 5172
                 t_alpha = 0.0, t_beta = 0.0;
5176
-
5177 5173
           const float r_diff = delta_radius - delta_calibration_radius,
5178
-                      h_factor = 1.00 + r_diff * 0.001,                                          //1.02 for r_diff = 20mm
5179
-                      r_factor = -(1.75 + 0.005 * r_diff + 0.001 * sq(r_diff)),                  //2.25 for r_diff = 20mm
5180
-                      a_factor = 100.0 / delta_calibration_radius;                               //1.25 for cal_rd = 80mm
5174
+                      h_factor = 1.00 + r_diff * 0.001,                          //1.02 for r_diff = 20mm
5175
+                      r_factor = -(1.75 + 0.005 * r_diff + 0.001 * sq(r_diff)),  //2.25 for r_diff = 20mm
5176
+                      a_factor = 100.0 / delta_calibration_radius;               //1.25 for cal_rd = 80mm
5181 5177
 
5182 5178
           #define ZP(N,I) ((N) * z_at_pt[I])
5183 5179
           #define Z1000(I) ZP(1.00, I)
@@ -5218,43 +5214,42 @@ inline void gcode_G28() {
5218 5214
               e_delta[Z_AXIS] = Z1050(0) - Z0175(1) - Z0175(5) + Z0350(9) + Z0175(7) + Z0175(11) - Z0350(3);
5219 5215
               r_delta         = Z2250(0) - Z0375(1) - Z0375(5) - Z0375(9) - Z0375(7) - Z0375(11) - Z0375(3);
5220 5216
               
5221
-              if (probe_mode > 0) {  //probe points negative disables tower angles
5217
+              if (probe_mode > 0) {  // negative disables tower angles
5222 5218
                 t_alpha = + Z0444(1) - Z0888(5) + Z0444(9) + Z0444(7) - Z0888(11) + Z0444(3);
5223 5219
                 t_beta  = - Z0888(1) + Z0444(5) + Z0444(9) - Z0888(7) + Z0444(11) + Z0444(3);
5224 5220
               }
5225 5221
               break;
5226 5222
           }
5227 5223
 
5228
-          // adjust delta_height and endstops by the max amount
5229 5224
           LOOP_XYZ(axis) endstop_adj[axis] += e_delta[axis];
5230 5225
           delta_radius += r_delta;
5226
+          delta_tower_angle_trim[A_AXIS] += t_alpha;
5227
+          delta_tower_angle_trim[B_AXIS] -= t_beta;
5231 5228
 
5229
+          // adjust delta_height and endstops by the max amount
5232 5230
           const float z_temp = MAX3(endstop_adj[A_AXIS], endstop_adj[B_AXIS], endstop_adj[C_AXIS]);
5233 5231
           home_offset[Z_AXIS] -= z_temp;
5234 5232
           LOOP_XYZ(i) endstop_adj[i] -= z_temp;
5235 5233
 
5236
-          delta_tower_angle_trim[A_AXIS] += t_alpha;
5237
-          delta_tower_angle_trim[B_AXIS] -= t_beta;
5238
-
5239 5234
           recalc_delta_settings(delta_radius, delta_diagonal_rod);
5240 5235
         }
5241
-        else { // !iterate
5242
-          // step one back
5236
+        else {   // step one back
5243 5237
           COPY(endstop_adj, e_old);
5244 5238
           delta_radius = dr_old;
5245 5239
           home_offset[Z_AXIS] = zh_old;
5246 5240
           delta_tower_angle_trim[A_AXIS] = alpha_old;
5247 5241
           delta_tower_angle_trim[B_AXIS] = beta_old;
5242
+
5248 5243
           recalc_delta_settings(delta_radius, delta_diagonal_rod);
5249 5244
         }
5250 5245
 
5251
-        // print report
5246
+         // print report
5252 5247
 
5253
-        if (verbose_level == 2) {
5248
+        if (verbose_level != 1) {
5254 5249
           SERIAL_PROTOCOLPGM(".      c:");
5255 5250
           if (z_at_pt[0] > 0) SERIAL_CHAR('+');
5256 5251
           SERIAL_PROTOCOL_F(z_at_pt[0], 2);
5257
-          if (towers) {
5252
+          if (probe_mode == 2 || pp_greather_2) {
5258 5253
             SERIAL_PROTOCOLPGM("     x:");
5259 5254
             if (z_at_pt[1] >= 0) SERIAL_CHAR('+');
5260 5255
             SERIAL_PROTOCOL_F(z_at_pt[1], 2);
@@ -5265,11 +5260,11 @@ inline void gcode_G28() {
5265 5260
             if (z_at_pt[9] >= 0) SERIAL_CHAR('+');
5266 5261
             SERIAL_PROTOCOL_F(z_at_pt[9], 2);
5267 5262
           }
5268
-          if (!o_mode) SERIAL_EOL;
5269
-          if (opposites) {
5270
-            if (_7_probe) {
5263
+          if (probe_mode != -2) SERIAL_EOL;
5264
+          if (probe_mode == -2 || pp_greather_2) {
5265
+            if (pp_greather_2) {
5271 5266
               SERIAL_CHAR('.');
5272
-              SERIAL_PROTOCOL_SP(12);
5267
+              SERIAL_PROTOCOL_SP(13);
5273 5268
             }
5274 5269
             SERIAL_PROTOCOLPGM("    yz:");
5275 5270
             if (z_at_pt[7] >= 0) SERIAL_CHAR('+');
@@ -5283,15 +5278,15 @@ inline void gcode_G28() {
5283 5278
             SERIAL_EOL;
5284 5279
           }
5285 5280
         }
5286
-        if (test_precision != 0.0) {                                                             // !forced end
5287
-          if (zero_std_dev >= test_precision) {                                                  // end iterations
5281
+        if (test_precision != 0.0) {                                 // !forced end
5282
+          if (zero_std_dev >= test_precision) {                      // end iterations
5288 5283
             SERIAL_PROTOCOLPGM("Calibration OK");
5289 5284
             SERIAL_PROTOCOL_SP(36);
5290 5285
             SERIAL_PROTOCOLPGM("rolling back.");
5291 5286
             SERIAL_EOL;
5292 5287
             LCD_MESSAGEPGM("Calibration OK");
5293 5288
           }
5294
-          else {                                                                                 // !end iterations
5289
+          else {                                                     // !end iterations
5295 5290
             char mess[15] = "No convergence";
5296 5291
             if (iterations < 31)
5297 5292
               sprintf_P(mess, PSTR("Iteration : %02i"), (int)iterations);
@@ -5303,7 +5298,7 @@ inline void gcode_G28() {
5303 5298
             lcd_setstatus(mess);
5304 5299
           }
5305 5300
           SERIAL_PROTOCOLPAIR(".Height:", DELTA_HEIGHT + home_offset[Z_AXIS]);
5306
-          if (!_1_point) {
5301
+          if (!pp_equals_1) {
5307 5302
             SERIAL_PROTOCOLPGM("    Ex:");
5308 5303
             if (endstop_adj[A_AXIS] >= 0) SERIAL_CHAR('+');
5309 5304
             SERIAL_PROTOCOL_F(endstop_adj[A_AXIS], 2);
@@ -5316,7 +5311,7 @@ inline void gcode_G28() {
5316 5311
             SERIAL_PROTOCOLPAIR("    Radius:", delta_radius);
5317 5312
           }
5318 5313
           SERIAL_EOL;
5319
-          if (_7_point) {
5314
+          if (probe_mode > 2) { // negative disables tower angles
5320 5315
             SERIAL_PROTOCOLPGM(".Tower angle :    Tx:");
5321 5316
             if (delta_tower_angle_trim[A_AXIS] >= 0) SERIAL_CHAR('+');
5322 5317
             SERIAL_PROTOCOL_F(delta_tower_angle_trim[A_AXIS], 2);
@@ -5328,8 +5323,9 @@ inline void gcode_G28() {
5328 5323
           }
5329 5324
           if (zero_std_dev >= test_precision)
5330 5325
             serialprintPGM(save_message);
5331
-        }
5332
-        else {                                                                                   // forced end
5326
+            SERIAL_EOL;
5327
+       }
5328
+        else {                                                       // forced end
5333 5329
           if (verbose_level == 0) {
5334 5330
             SERIAL_PROTOCOLPGM("End DRY-RUN");
5335 5331
             SERIAL_PROTOCOL_SP(39);
@@ -5343,10 +5339,10 @@ inline void gcode_G28() {
5343 5339
             SERIAL_PROTOCOLPAIR(".Height:", DELTA_HEIGHT + home_offset[Z_AXIS]);
5344 5340
             SERIAL_EOL;
5345 5341
             serialprintPGM(save_message);
5342
+            SERIAL_EOL;
5346 5343
           }
5347 5344
         }
5348 5345
 
5349
-        clean_up_after_endstop_or_probe_move();
5350 5346
         stepper.synchronize();
5351 5347
 
5352 5348
         gcode_G28();

Loading…
Cancel
Save