Browse Source

G33 changes from 1.1.x

Scott Lahteine 7 years ago
parent
commit
6d5fcac54a

+ 102
- 95
Marlin/src/gcode/calibrate/G33.cpp View File

45
  *
45
  *
46
  *   Pn  Number of probe points:
46
  *   Pn  Number of probe points:
47
  *
47
  *
48
+ *      P0     No probe. Normalize only.
48
  *      P1     Probe center and set height only.
49
  *      P1     Probe center and set height only.
49
  *      P2     Probe center and towers. Set height, endstops, and delta radius.
50
  *      P2     Probe center and towers. Set height, endstops, and delta radius.
50
  *      P3     Probe all positions: center, towers and opposite towers. Set all.
51
  *      P3     Probe all positions: center, towers and opposite towers. Set all.
73
   SERIAL_PROTOCOL_F(f, 2);
74
   SERIAL_PROTOCOL_F(f, 2);
74
 }
75
 }
75
 
76
 
76
-static void print_G33_settings(const bool end_stops, const bool tower_angles){ // TODO echo these to LCD ???
77
+static void print_G33_settings(const bool end_stops, const bool tower_angles) {
77
   SERIAL_PROTOCOLPAIR(".Height:", DELTA_HEIGHT + home_offset[Z_AXIS]);
78
   SERIAL_PROTOCOLPAIR(".Height:", DELTA_HEIGHT + home_offset[Z_AXIS]);
78
   if (end_stops) {
79
   if (end_stops) {
79
     print_signed_float(PSTR("  Ex"), delta_endstop_adj[A_AXIS]);
80
     print_signed_float(PSTR("  Ex"), delta_endstop_adj[A_AXIS]);
86
     SERIAL_PROTOCOLPGM(".Tower angle :  ");
87
     SERIAL_PROTOCOLPGM(".Tower angle :  ");
87
     print_signed_float(PSTR("Tx"), delta_tower_angle_trim[A_AXIS]);
88
     print_signed_float(PSTR("Tx"), delta_tower_angle_trim[A_AXIS]);
88
     print_signed_float(PSTR("Ty"), delta_tower_angle_trim[B_AXIS]);
89
     print_signed_float(PSTR("Ty"), delta_tower_angle_trim[B_AXIS]);
89
-    SERIAL_PROTOCOLLNPGM("  Tz:+0.00");
90
+    print_signed_float(PSTR("Tz"), delta_tower_angle_trim[C_AXIS]);
91
+    SERIAL_EOL();
90
   }
92
   }
91
 }
93
 }
92
 
94
 
108
 void GcodeSuite::G33() {
110
 void GcodeSuite::G33() {
109
 
111
 
110
   const int8_t probe_points = parser.intval('P', DELTA_CALIBRATION_DEFAULT_POINTS);
112
   const int8_t probe_points = parser.intval('P', DELTA_CALIBRATION_DEFAULT_POINTS);
111
-  if (!WITHIN(probe_points, 1, 7)) {
112
-    SERIAL_PROTOCOLLNPGM("?(P)oints is implausible (1-7).");
113
+  if (!WITHIN(probe_points, 0, 7)) {
114
+    SERIAL_PROTOCOLLNPGM("?(P)oints is implausible (0-7).");
113
     return;
115
     return;
114
   }
116
   }
115
 
117
 
132
   }
134
   }
133
 
135
 
134
   const bool towers_set           = parser.boolval('T', true),
136
   const bool towers_set           = parser.boolval('T', true),
137
+             _0p_calibration      = probe_points == 0,
135
              _1p_calibration      = probe_points == 1,
138
              _1p_calibration      = probe_points == 1,
136
              _4p_calibration      = probe_points == 2,
139
              _4p_calibration      = probe_points == 2,
137
              _4p_towers_points    = _4p_calibration && towers_set,
140
              _4p_towers_points    = _4p_calibration && towers_set,
138
              _4p_opposite_points  = _4p_calibration && !towers_set,
141
              _4p_opposite_points  = _4p_calibration && !towers_set,
139
-             _7p_calibration      = probe_points >= 3,
142
+             _7p_calibration      = probe_points >= 3 || _0p_calibration,
140
              _7p_half_circle      = probe_points == 3,
143
              _7p_half_circle      = probe_points == 3,
141
              _7p_double_circle    = probe_points == 5,
144
              _7p_double_circle    = probe_points == 5,
142
              _7p_triple_circle    = probe_points == 6,
145
              _7p_triple_circle    = probe_points == 6,
157
         zero_std_dev = (verbose_level ? 999.0 : 0.0), // 0.0 in dry-run mode : forced end
160
         zero_std_dev = (verbose_level ? 999.0 : 0.0), // 0.0 in dry-run mode : forced end
158
         zero_std_dev_old = zero_std_dev,
161
         zero_std_dev_old = zero_std_dev,
159
         zero_std_dev_min = zero_std_dev,
162
         zero_std_dev_min = zero_std_dev,
160
-        e_old[XYZ] = {
163
+        e_old[ABC] = {
161
           delta_endstop_adj[A_AXIS],
164
           delta_endstop_adj[A_AXIS],
162
           delta_endstop_adj[B_AXIS],
165
           delta_endstop_adj[B_AXIS],
163
           delta_endstop_adj[C_AXIS]
166
           delta_endstop_adj[C_AXIS]
164
         },
167
         },
165
         dr_old = delta_radius,
168
         dr_old = delta_radius,
166
         zh_old = home_offset[Z_AXIS],
169
         zh_old = home_offset[Z_AXIS],
167
-        alpha_old = delta_tower_angle_trim[A_AXIS],
168
-        beta_old = delta_tower_angle_trim[B_AXIS];
170
+        ta_old[ABC] = {
171
+          delta_tower_angle_trim[A_AXIS],
172
+          delta_tower_angle_trim[B_AXIS],
173
+          delta_tower_angle_trim[C_AXIS]
174
+        };
169
 
175
 
170
-  if (!_1p_calibration) {  // test if the outer radius is reachable
176
+  if (!_1p_calibration && !_0p_calibration) {  // test if the outer radius is reachable
171
     const float circles = (_7p_quadruple_circle ? 1.5 :
177
     const float circles = (_7p_quadruple_circle ? 1.5 :
172
                            _7p_triple_circle    ? 1.0 :
178
                            _7p_triple_circle    ? 1.0 :
173
                            _7p_double_circle    ? 0.5 : 0),
179
                            _7p_double_circle    ? 0.5 : 0),
198
 
204
 
199
   setup_for_endstop_or_probe_move();
205
   setup_for_endstop_or_probe_move();
200
   endstops.enable(true);
206
   endstops.enable(true);
201
-  if (!home_delta())
202
-    return;
203
-  endstops.not_homing();
207
+  if (!_0p_calibration) {
208
+    if (!home_delta())
209
+      return;
210
+    endstops.not_homing();
211
+  }
204
 
212
 
205
   // print settings
213
   // print settings
206
 
214
 
213
   print_G33_settings(!_1p_calibration, _7p_calibration && towers_set);
221
   print_G33_settings(!_1p_calibration, _7p_calibration && towers_set);
214
 
222
 
215
   #if DISABLED(PROBE_MANUALLY)
223
   #if DISABLED(PROBE_MANUALLY)
216
-    const float measured_z = probe_pt(dx, dy, stow_after_each, 1, false); // 1st probe to set height
217
-    if (isnan(measured_z)) return G33_CLEANUP();
218
-    home_offset[Z_AXIS] -= measured_z;
224
+    if (!_0p_calibration) {
225
+      const float measured_z = probe_pt(dx, dy, stow_after_each, 1, false); // 1st probe to set height
226
+      if (isnan(measured_z)) return G33_CLEANUP();
227
+      home_offset[Z_AXIS] -= measured_z;
228
+    }
219
   #endif
229
   #endif
220
 
230
 
221
   do {
231
   do {
222
 
232
 
223
     float z_at_pt[13] = { 0.0 };
233
     float z_at_pt[13] = { 0.0 };
224
 
234
 
225
-    test_precision = zero_std_dev_old != 999.0 ? (zero_std_dev + zero_std_dev_old) / 2 : zero_std_dev;
235
+    test_precision = _0p_calibration ? 0.00 : zero_std_dev_old != 999.0 ? (zero_std_dev + zero_std_dev_old) / 2 : zero_std_dev;
226
 
236
 
227
     iterations++;
237
     iterations++;
228
 
238
 
229
     // Probe the points
239
     // Probe the points
230
 
240
 
231
-    if (!_7p_half_circle && !_7p_triple_circle) { // probe the center
232
-      #if ENABLED(PROBE_MANUALLY)
233
-        z_at_pt[0] += lcd_probe_pt(0, 0);
234
-      #else
235
-        z_at_pt[0] += probe_pt(dx, dy, stow_after_each, 1, false);
236
-        if (isnan(z_at_pt[0])) return G33_CLEANUP();
237
-      #endif
238
-    }
239
-    if (_7p_calibration) { // probe extra center points
240
-      for (int8_t axis = _7p_multi_circle ? 11 : 9; axis > 0; axis -= _7p_multi_circle ? 2 : 4) {
241
-        const float a = RADIANS(180 + 30 * axis), r = delta_calibration_radius * 0.1;
241
+    if (!_0p_calibration){
242
+      if (!_7p_half_circle && !_7p_triple_circle) { // probe the center
242
         #if ENABLED(PROBE_MANUALLY)
243
         #if ENABLED(PROBE_MANUALLY)
243
-          z_at_pt[0] += lcd_probe_pt(cos(a) * r, sin(a) * r);
244
+          z_at_pt[0] += lcd_probe_pt(0, 0);
244
         #else
245
         #else
245
-          z_at_pt[0] += probe_pt(cos(a) * r + dx, sin(a) * r + dy, stow_after_each, 1);
246
+          z_at_pt[0] += probe_pt(dx, dy, stow_after_each, 1, false);
246
           if (isnan(z_at_pt[0])) return G33_CLEANUP();
247
           if (isnan(z_at_pt[0])) return G33_CLEANUP();
247
         #endif
248
         #endif
248
       }
249
       }
249
-      z_at_pt[0] /= float(_7p_double_circle ? 7 : probe_points);
250
-    }
251
-    if (!_1p_calibration) {  // probe the radius
252
-      bool zig_zag = true;
253
-      const uint8_t start = _4p_opposite_points ? 3 : 1,
254
-                     step = _4p_calibration ? 4 : _7p_half_circle ? 2 : 1;
255
-      for (uint8_t axis = start; axis < 13; axis += step) {
256
-        const float zigadd = (zig_zag ? 0.5 : 0.0),
257
-                    offset_circles = _7p_quadruple_circle ? zigadd + 1.0 :
258
-                                     _7p_triple_circle    ? zigadd + 0.5 :
259
-                                     _7p_double_circle    ? zigadd : 0;
260
-        for (float circles = -offset_circles ; circles <= offset_circles; circles++) {
261
-          const float a = RADIANS(180 + 30 * axis),
262
-                      r = delta_calibration_radius * (1 + circles * (zig_zag ? 0.1 : -0.1));
250
+      if (_7p_calibration) { // probe extra center points
251
+        for (int8_t axis = _7p_multi_circle ? 11 : 9; axis > 0; axis -= _7p_multi_circle ? 2 : 4) {
252
+          const float a = RADIANS(180 + 30 * axis), r = delta_calibration_radius * 0.1;
263
           #if ENABLED(PROBE_MANUALLY)
253
           #if ENABLED(PROBE_MANUALLY)
264
-            z_at_pt[axis] += lcd_probe_pt(cos(a) * r, sin(a) * r);
254
+            z_at_pt[0] += lcd_probe_pt(cos(a) * r, sin(a) * r);
265
           #else
255
           #else
266
-            z_at_pt[axis] += probe_pt(cos(a) * r + dx, sin(a) * r + dy, stow_after_each, 1);
267
-            if (isnan(z_at_pt[axis])) return G33_CLEANUP();
256
+            z_at_pt[0] += probe_pt(cos(a) * r + dx, sin(a) * r + dy, stow_after_each, 1);
257
+            if (isnan(z_at_pt[0])) return G33_CLEANUP();
268
           #endif
258
           #endif
269
         }
259
         }
270
-        zig_zag = !zig_zag;
271
-        z_at_pt[axis] /= (2 * offset_circles + 1);
260
+        z_at_pt[0] /= float(_7p_double_circle ? 7 : probe_points);
261
+      }
262
+      if (!_1p_calibration) {  // probe the radius
263
+        bool zig_zag = true;
264
+        const uint8_t start = _4p_opposite_points ? 3 : 1,
265
+                       step = _4p_calibration ? 4 : _7p_half_circle ? 2 : 1;
266
+        for (uint8_t axis = start; axis < 13; axis += step) {
267
+          const float zigadd = (zig_zag ? 0.5 : 0.0),
268
+                      offset_circles = _7p_quadruple_circle ? zigadd + 1.0 :
269
+                                       _7p_triple_circle    ? zigadd + 0.5 :
270
+                                       _7p_double_circle    ? zigadd : 0;
271
+          for (float circles = -offset_circles ; circles <= offset_circles; circles++) {
272
+            const float a = RADIANS(180 + 30 * axis),
273
+                        r = delta_calibration_radius * (1 + circles * (zig_zag ? 0.1 : -0.1));
274
+            #if ENABLED(PROBE_MANUALLY)
275
+              z_at_pt[axis] += lcd_probe_pt(cos(a) * r, sin(a) * r);
276
+            #else
277
+              z_at_pt[axis] += probe_pt(cos(a) * r + dx, sin(a) * r + dy, stow_after_each, 1);
278
+              if (isnan(z_at_pt[axis])) return G33_CLEANUP();
279
+            #endif
280
+          }
281
+          zig_zag = !zig_zag;
282
+          z_at_pt[axis] /= (2 * offset_circles + 1);
283
+        }
272
       }
284
       }
285
+      if (_7p_intermed_points) // average intermediates to tower and opposites
286
+        for (uint8_t axis = 1; axis < 13; axis += 2)
287
+          z_at_pt[axis] = (z_at_pt[axis] + (z_at_pt[axis + 1] + z_at_pt[(axis + 10) % 12 + 1]) / 2.0) / 2.0;
273
     }
288
     }
274
-    if (_7p_intermed_points) // average intermediates to tower and opposites
275
-      for (uint8_t axis = 1; axis < 13; axis += 2)
276
-        z_at_pt[axis] = (z_at_pt[axis] + (z_at_pt[axis + 1] + z_at_pt[(axis + 10) % 12 + 1]) / 2.0) / 2.0;
277
 
289
 
278
     float S1 = z_at_pt[0],
290
     float S1 = z_at_pt[0],
279
           S2 = sq(z_at_pt[0]);
291
           S2 = sq(z_at_pt[0]);
294
         COPY(e_old, delta_endstop_adj);
306
         COPY(e_old, delta_endstop_adj);
295
         dr_old = delta_radius;
307
         dr_old = delta_radius;
296
         zh_old = home_offset[Z_AXIS];
308
         zh_old = home_offset[Z_AXIS];
297
-        alpha_old = delta_tower_angle_trim[A_AXIS];
298
-        beta_old = delta_tower_angle_trim[B_AXIS];
309
+        COPY(ta_old, delta_tower_angle_trim);
299
       }
310
       }
300
 
311
 
301
-      float e_delta[XYZ] = { 0.0 }, r_delta = 0.0, t_alpha = 0.0, t_beta = 0.0;
312
+      float e_delta[ABC] = { 0.0 }, r_delta = 0.0, t_delta[ABC] = { 0.0 };
302
       const float r_diff = delta_radius - delta_calibration_radius,
313
       const float r_diff = delta_radius - delta_calibration_radius,
303
-                  h_factor = 1.00 + r_diff * 0.001,                          //1.02 for r_diff = 20mm
304
-                  r_factor = -(1.75 + 0.005 * r_diff + 0.001 * sq(r_diff)),  //2.25 for r_diff = 20mm
305
-                  a_factor = 100.0 / delta_calibration_radius;               //1.25 for cal_rd = 80mm
314
+                  h_factor = (1.00 + r_diff * 0.001) / 6.0,                        //1.02 / 6 for r_diff = 20mm
315
+                  r_factor = -(1.75 + 0.005 * r_diff + 0.001 * sq(r_diff)) / 6.0,  //2.25 / 6 for r_diff = 20mm
316
+                  a_factor = 66.66 / delta_calibration_radius;                     //1.25 for cal_rd = 80mm
306
 
317
 
307
       #define ZP(N,I) ((N) * z_at_pt[I])
318
       #define ZP(N,I) ((N) * z_at_pt[I])
308
-      #define Z1000(I) ZP(1.00, I)
309
-      #define Z1050(I) ZP(h_factor, I)
310
-      #define Z0700(I) ZP(h_factor * 2.0 / 3.00, I)
311
-      #define Z0350(I) ZP(h_factor / 3.00, I)
312
-      #define Z0175(I) ZP(h_factor / 6.00, I)
313
-      #define Z2250(I) ZP(r_factor, I)
314
-      #define Z0750(I) ZP(r_factor / 3.00, I)
315
-      #define Z0375(I) ZP(r_factor / 6.00, I)
316
-      #define Z0444(I) ZP(a_factor * 4.0 / 9.0, I)
317
-      #define Z0888(I) ZP(a_factor * 8.0 / 9.0, I)
319
+      #define Z6(I) ZP(6, I)
320
+      #define Z4(I) ZP(4, I)
321
+      #define Z2(I) ZP(2, I)
322
+      #define Z1(I) ZP(1, I)
318
 
323
 
319
       #if ENABLED(PROBE_MANUALLY)
324
       #if ENABLED(PROBE_MANUALLY)
320
         test_precision = 0.00; // forced end
325
         test_precision = 0.00; // forced end
323
       switch (probe_points) {
328
       switch (probe_points) {
324
         case 1:
329
         case 1:
325
           test_precision = 0.00; // forced end
330
           test_precision = 0.00; // forced end
326
-          LOOP_XYZ(i) e_delta[i] = Z1000(0);
331
+          LOOP_XYZ(axis) e_delta[axis] = Z1(0);
327
           break;
332
           break;
328
 
333
 
329
         case 2:
334
         case 2:
330
           if (towers_set) {
335
           if (towers_set) {
331
-            e_delta[X_AXIS] = Z1050(0) + Z0700(1) - Z0350(5) - Z0350(9);
332
-            e_delta[Y_AXIS] = Z1050(0) - Z0350(1) + Z0700(5) - Z0350(9);
333
-            e_delta[Z_AXIS] = Z1050(0) - Z0350(1) - Z0350(5) + Z0700(9);
334
-            r_delta         = Z2250(0) - Z0750(1) - Z0750(5) - Z0750(9);
336
+            e_delta[A_AXIS] = (Z6(0) + Z4(1) - Z2(5) - Z2(9)) * h_factor;
337
+            e_delta[B_AXIS] = (Z6(0) - Z2(1) + Z4(5) - Z2(9)) * h_factor;
338
+            e_delta[C_AXIS] = (Z6(0) - Z2(1) - Z2(5) + Z4(9)) * h_factor;
339
+            r_delta         = (Z6(0) - Z2(1) - Z2(5) - Z2(9)) * r_factor;
335
           }
340
           }
336
           else {
341
           else {
337
-            e_delta[X_AXIS] = Z1050(0) - Z0700(7) + Z0350(11) + Z0350(3);
338
-            e_delta[Y_AXIS] = Z1050(0) + Z0350(7) - Z0700(11) + Z0350(3);
339
-            e_delta[Z_AXIS] = Z1050(0) + Z0350(7) + Z0350(11) - Z0700(3);
340
-            r_delta         = Z2250(0) - Z0750(7) - Z0750(11) - Z0750(3);
342
+            e_delta[A_AXIS] = (Z6(0) - Z4(7) + Z2(11) + Z2(3)) * h_factor;
343
+            e_delta[B_AXIS] = (Z6(0) + Z2(7) - Z4(11) + Z2(3)) * h_factor;
344
+            e_delta[C_AXIS] = (Z6(0) + Z2(7) + Z2(11) - Z4(3)) * h_factor;
345
+            r_delta         = (Z6(0) - Z2(7) - Z2(11) - Z2(3)) * r_factor;
341
           }
346
           }
342
           break;
347
           break;
343
 
348
 
344
         default:
349
         default:
345
-          e_delta[X_AXIS] = Z1050(0) + Z0350(1) - Z0175(5) - Z0175(9) - Z0350(7) + Z0175(11) + Z0175(3);
346
-          e_delta[Y_AXIS] = Z1050(0) - Z0175(1) + Z0350(5) - Z0175(9) + Z0175(7) - Z0350(11) + Z0175(3);
347
-          e_delta[Z_AXIS] = Z1050(0) - Z0175(1) - Z0175(5) + Z0350(9) + Z0175(7) + Z0175(11) - Z0350(3);
348
-          r_delta         = Z2250(0) - Z0375(1) - Z0375(5) - Z0375(9) - Z0375(7) - Z0375(11) - Z0375(3);
350
+          e_delta[A_AXIS] = (Z6(0) + Z2(1) - Z1(5) - Z1(9) - Z2(7) + Z1(11) + Z1(3)) * h_factor;
351
+          e_delta[B_AXIS] = (Z6(0) - Z1(1) + Z2(5) - Z1(9) + Z1(7) - Z2(11) + Z1(3)) * h_factor;
352
+          e_delta[C_AXIS] = (Z6(0) - Z1(1) - Z1(5) + Z2(9) + Z1(7) + Z1(11) - Z2(3)) * h_factor;
353
+          r_delta         = (Z6(0) - Z1(1) - Z1(5) - Z1(9) - Z1(7) - Z1(11) - Z1(3)) * r_factor;
349
 
354
 
350
           if (towers_set) {
355
           if (towers_set) {
351
-            t_alpha = Z0444(1) - Z0888(5) + Z0444(9) + Z0444(7) - Z0888(11) + Z0444(3);
352
-            t_beta  = Z0888(1) - Z0444(5) - Z0444(9) + Z0888(7) - Z0444(11) - Z0444(3);
356
+            t_delta[A_AXIS] = (            - Z2(5) + Z1(9)         - Z2(11) + Z1(3)) * a_factor;
357
+            t_delta[B_AXIS] = (      Z2(1)         - Z1(9) + Z2(7)          - Z1(3)) * a_factor;
358
+            t_delta[C_AXIS] = (     -Z2(1) + Z1(5)         - Z2(7) + Z1(11)        ) * a_factor;
353
           }
359
           }
354
           break;
360
           break;
355
       }
361
       }
356
 
362
 
357
       LOOP_XYZ(axis) delta_endstop_adj[axis] += e_delta[axis];
363
       LOOP_XYZ(axis) delta_endstop_adj[axis] += e_delta[axis];
358
       delta_radius += r_delta;
364
       delta_radius += r_delta;
359
-      delta_tower_angle_trim[A_AXIS] += t_alpha;
360
-      delta_tower_angle_trim[B_AXIS] += t_beta;
361
-
362
-      // adjust delta_height and endstops by the max amount
363
-      const float z_temp = MAX3(delta_endstop_adj[A_AXIS], delta_endstop_adj[B_AXIS], delta_endstop_adj[C_AXIS]);
364
-      home_offset[Z_AXIS] -= z_temp;
365
-      LOOP_XYZ(i) delta_endstop_adj[i] -= z_temp;
366
-
367
-      recalc_delta_settings(delta_radius, delta_diagonal_rod);
365
+      LOOP_XYZ(axis) delta_tower_angle_trim[axis] += t_delta[axis];
368
     }
366
     }
369
     else if (zero_std_dev >= test_precision) {   // step one back
367
     else if (zero_std_dev >= test_precision) {   // step one back
370
       COPY(delta_endstop_adj, e_old);
368
       COPY(delta_endstop_adj, e_old);
371
       delta_radius = dr_old;
369
       delta_radius = dr_old;
372
       home_offset[Z_AXIS] = zh_old;
370
       home_offset[Z_AXIS] = zh_old;
373
-      delta_tower_angle_trim[A_AXIS] = alpha_old;
374
-      delta_tower_angle_trim[B_AXIS] = beta_old;
375
-
376
-      recalc_delta_settings(delta_radius, delta_diagonal_rod);
371
+      COPY(delta_tower_angle_trim, ta_old);
372
+    }
373
+    if (verbose_level != 0) {                                    // !dry run
374
+      // normalise angles to least squares
375
+      float a_sum = 0.0;
376
+      LOOP_XYZ(axis) a_sum += delta_tower_angle_trim[axis];
377
+      LOOP_XYZ(axis) delta_tower_angle_trim[axis] -= a_sum / 3.0;
378
+      
379
+      // adjust delta_height and endstops by the max amount
380
+      const float z_temp = MAX3(delta_endstop_adj[A_AXIS], delta_endstop_adj[B_AXIS], delta_endstop_adj[C_AXIS]);
381
+      home_offset[Z_AXIS] -= z_temp;
382
+      LOOP_XYZ(axis) delta_endstop_adj[axis] -= z_temp;
377
     }
383
     }
384
+    recalc_delta_settings(delta_radius, delta_diagonal_rod, delta_tower_angle_trim);
378
     NOMORE(zero_std_dev_min, zero_std_dev);
385
     NOMORE(zero_std_dev_min, zero_std_dev);
379
 
386
 
380
     // print report
387
     // print report

+ 2
- 5
Marlin/src/gcode/calibrate/M665.cpp View File

54
     if (parser.seen('B')) delta_calibration_radius       = parser.value_float();
54
     if (parser.seen('B')) delta_calibration_radius       = parser.value_float();
55
     if (parser.seen('X')) delta_tower_angle_trim[A_AXIS] = parser.value_float();
55
     if (parser.seen('X')) delta_tower_angle_trim[A_AXIS] = parser.value_float();
56
     if (parser.seen('Y')) delta_tower_angle_trim[B_AXIS] = parser.value_float();
56
     if (parser.seen('Y')) delta_tower_angle_trim[B_AXIS] = parser.value_float();
57
-    if (parser.seen('Z')) { // rotate all 3 axis for Z = 0
58
-      delta_tower_angle_trim[A_AXIS] -= parser.value_float();
59
-      delta_tower_angle_trim[B_AXIS] -= parser.value_float();
60
-    }
61
-    recalc_delta_settings(delta_radius, delta_diagonal_rod);
57
+    if (parser.seen('Z')) delta_tower_angle_trim[C_AXIS] = parser.value_float();
58
+    recalc_delta_settings(delta_radius, delta_diagonal_rod, delta_tower_angle_trim);
62
   }
59
   }
63
 
60
 
64
 #elif IS_SCARA
61
 #elif IS_SCARA

+ 2
- 5
Marlin/src/gcode/calibrate/M666.cpp View File

42
     #endif
42
     #endif
43
     LOOP_XYZ(i) {
43
     LOOP_XYZ(i) {
44
       if (parser.seen(axis_codes[i])) {
44
       if (parser.seen(axis_codes[i])) {
45
-        delta_endstop_adj[i] = parser.value_linear_units();
45
+        const float v = parser.value_linear_units();
46
+        if (v * Z_HOME_DIR <= 0) delta_endstop_adj[i] = v;
46
         #if ENABLED(DEBUG_LEVELING_FEATURE)
47
         #if ENABLED(DEBUG_LEVELING_FEATURE)
47
           if (DEBUGGING(LEVELING)) {
48
           if (DEBUGGING(LEVELING)) {
48
             SERIAL_ECHOPAIR("delta_endstop_adj[", axis_codes[i]);
49
             SERIAL_ECHOPAIR("delta_endstop_adj[", axis_codes[i]);
56
         SERIAL_ECHOLNPGM("<<< M666");
57
         SERIAL_ECHOLNPGM("<<< M666");
57
       }
58
       }
58
     #endif
59
     #endif
59
-    // normalize endstops so all are <=0; set the residue to delta height
60
-    const float z_temp = MAX3(delta_endstop_adj[A_AXIS], delta_endstop_adj[B_AXIS], delta_endstop_adj[C_AXIS]);
61
-    home_offset[Z_AXIS] -= z_temp;
62
-    LOOP_XYZ(i) delta_endstop_adj[i] -= z_temp;
63
   }
60
   }
64
 
61
 
65
 #elif ENABLED(Z_DUAL_ENDSTOPS) // !DELTA && ENABLED(Z_DUAL_ENDSTOPS)
62
 #elif ENABLED(Z_DUAL_ENDSTOPS) // !DELTA && ENABLED(Z_DUAL_ENDSTOPS)

+ 1
- 1
Marlin/src/lcd/ultralcd.cpp View File

2666
       MENU_ITEM_EDIT(float52, MSG_DELTA_RADIUS, &delta_radius, DELTA_RADIUS - 5.0, DELTA_RADIUS + 5.0);
2666
       MENU_ITEM_EDIT(float52, MSG_DELTA_RADIUS, &delta_radius, DELTA_RADIUS - 5.0, DELTA_RADIUS + 5.0);
2667
       MENU_ITEM_EDIT(float43, "Tx", &delta_tower_angle_trim[A_AXIS], -5.0, 5.0);
2667
       MENU_ITEM_EDIT(float43, "Tx", &delta_tower_angle_trim[A_AXIS], -5.0, 5.0);
2668
       MENU_ITEM_EDIT(float43, "Ty", &delta_tower_angle_trim[B_AXIS], -5.0, 5.0);
2668
       MENU_ITEM_EDIT(float43, "Ty", &delta_tower_angle_trim[B_AXIS], -5.0, 5.0);
2669
-      MENU_ITEM_EDIT(float43, "Tz", &Tz, -5.0, 5.0);
2669
+      MENU_ITEM_EDIT(float43, "Tz", &delta_tower_angle_trim[C_AXIS], -5.0, 5.0);
2670
       END_MENU();
2670
       END_MENU();
2671
     }
2671
     }
2672
 
2672
 

+ 14
- 15
Marlin/src/module/configuration_store.cpp View File

36
  *
36
  *
37
  */
37
  */
38
 
38
 
39
-#define EEPROM_VERSION "V40"
39
+#define EEPROM_VERSION "V41"
40
 
40
 
41
 // Change EEPROM version if these are changed:
41
 // Change EEPROM version if these are changed:
42
 #define EEPROM_OFFSET 100
42
 #define EEPROM_OFFSET 100
43
 
43
 
44
 /**
44
 /**
45
- * V39 EEPROM Layout:
45
+ * V41 EEPROM Layout:
46
  *
46
  *
47
  *  100  Version                                    (char x4)
47
  *  100  Version                                    (char x4)
48
  *  104  EEPROM CRC16                               (uint16_t)
48
  *  104  EEPROM CRC16                               (uint16_t)
93
  *  329  G29 S     ubl.state.storage_slot           (int8_t)
93
  *  329  G29 S     ubl.state.storage_slot           (int8_t)
94
  *
94
  *
95
  * DELTA:                                           48 bytes
95
  * DELTA:                                           48 bytes
96
- *  348  M666 XYZ  delta_endstop_adj                      (float x3)
96
+ *  348  M666 XYZ  delta_endstop_adj                (float x3)
97
  *  360  M665 R    delta_radius                     (float)
97
  *  360  M665 R    delta_radius                     (float)
98
  *  364  M665 L    delta_diagonal_rod               (float)
98
  *  364  M665 L    delta_diagonal_rod               (float)
99
  *  368  M665 S    delta_segments_per_second        (float)
99
  *  368  M665 S    delta_segments_per_second        (float)
100
  *  372  M665 B    delta_calibration_radius         (float)
100
  *  372  M665 B    delta_calibration_radius         (float)
101
  *  376  M665 X    delta_tower_angle_trim[A]        (float)
101
  *  376  M665 X    delta_tower_angle_trim[A]        (float)
102
  *  380  M665 Y    delta_tower_angle_trim[B]        (float)
102
  *  380  M665 Y    delta_tower_angle_trim[B]        (float)
103
- *  ---  M665 Z    delta_tower_angle_trim[C]        (float) is always 0.0
103
+ *  384  M665 Z    delta_tower_angle_trim[C]        (float)
104
  *
104
  *
105
  * Z_DUAL_ENDSTOPS:                                 48 bytes
105
  * Z_DUAL_ENDSTOPS:                                 48 bytes
106
  *  348  M666 Z    endstops.z_endstop_adj           (float)
106
  *  348  M666 Z    endstops.z_endstop_adj           (float)
213
   // Make sure delta kinematics are updated before refreshing the
213
   // Make sure delta kinematics are updated before refreshing the
214
   // planner position so the stepper counts will be set correctly.
214
   // planner position so the stepper counts will be set correctly.
215
   #if ENABLED(DELTA)
215
   #if ENABLED(DELTA)
216
-    recalc_delta_settings(delta_radius, delta_diagonal_rod);
216
+    recalc_delta_settings(delta_radius, delta_diagonal_rod, delta_tower_angle_trim);
217
   #endif
217
   #endif
218
 
218
 
219
   // Refresh steps_to_mm with the reciprocal of axis_steps_per_mm
219
   // Refresh steps_to_mm with the reciprocal of axis_steps_per_mm
415
       EEPROM_WRITE(storage_slot);
415
       EEPROM_WRITE(storage_slot);
416
     #endif // AUTO_BED_LEVELING_UBL
416
     #endif // AUTO_BED_LEVELING_UBL
417
 
417
 
418
-    // 9 floats for DELTA / Z_DUAL_ENDSTOPS
418
+    // 10 floats for DELTA / Z_DUAL_ENDSTOPS
419
     #if ENABLED(DELTA)
419
     #if ENABLED(DELTA)
420
-      EEPROM_WRITE(delta_endstop_adj);               // 3 floats
420
+      EEPROM_WRITE(delta_endstop_adj);         // 3 floats
421
       EEPROM_WRITE(delta_radius);              // 1 float
421
       EEPROM_WRITE(delta_radius);              // 1 float
422
       EEPROM_WRITE(delta_diagonal_rod);        // 1 float
422
       EEPROM_WRITE(delta_diagonal_rod);        // 1 float
423
       EEPROM_WRITE(delta_segments_per_second); // 1 float
423
       EEPROM_WRITE(delta_segments_per_second); // 1 float
424
       EEPROM_WRITE(delta_calibration_radius);  // 1 float
424
       EEPROM_WRITE(delta_calibration_radius);  // 1 float
425
-      EEPROM_WRITE(delta_tower_angle_trim);    // 2 floats
425
+      EEPROM_WRITE(delta_tower_angle_trim);    // 3 floats
426
       dummy = 0.0f;
426
       dummy = 0.0f;
427
-      for (uint8_t q = 3; q--;) EEPROM_WRITE(dummy);
427
+      for (uint8_t q = 2; q--;) EEPROM_WRITE(dummy);
428
     #elif ENABLED(Z_DUAL_ENDSTOPS)
428
     #elif ENABLED(Z_DUAL_ENDSTOPS)
429
       EEPROM_WRITE(endstops.z_endstop_adj);    // 1 float
429
       EEPROM_WRITE(endstops.z_endstop_adj);    // 1 float
430
       dummy = 0.0f;
430
       dummy = 0.0f;
804
       #endif // AUTO_BED_LEVELING_UBL
804
       #endif // AUTO_BED_LEVELING_UBL
805
 
805
 
806
       #if ENABLED(DELTA)
806
       #if ENABLED(DELTA)
807
-        EEPROM_READ(delta_endstop_adj);               // 3 floats
807
+        EEPROM_READ(delta_endstop_adj);         // 3 floats
808
         EEPROM_READ(delta_radius);              // 1 float
808
         EEPROM_READ(delta_radius);              // 1 float
809
         EEPROM_READ(delta_diagonal_rod);        // 1 float
809
         EEPROM_READ(delta_diagonal_rod);        // 1 float
810
         EEPROM_READ(delta_segments_per_second); // 1 float
810
         EEPROM_READ(delta_segments_per_second); // 1 float
811
         EEPROM_READ(delta_calibration_radius);  // 1 float
811
         EEPROM_READ(delta_calibration_radius);  // 1 float
812
-        EEPROM_READ(delta_tower_angle_trim);    // 2 floats
812
+        EEPROM_READ(delta_tower_angle_trim);    // 3 floats
813
         dummy = 0.0f;
813
         dummy = 0.0f;
814
-        for (uint8_t q=3; q--;) EEPROM_READ(dummy);
814
+        for (uint8_t q=2; q--;) EEPROM_READ(dummy);
815
       #elif ENABLED(Z_DUAL_ENDSTOPS)
815
       #elif ENABLED(Z_DUAL_ENDSTOPS)
816
         EEPROM_READ(endstops.z_endstop_adj);    // 1 float
816
         EEPROM_READ(endstops.z_endstop_adj);    // 1 float
817
         dummy = 0.0f;
817
         dummy = 0.0f;
1199
     delta_diagonal_rod = DELTA_DIAGONAL_ROD;
1199
     delta_diagonal_rod = DELTA_DIAGONAL_ROD;
1200
     delta_segments_per_second = DELTA_SEGMENTS_PER_SECOND;
1200
     delta_segments_per_second = DELTA_SEGMENTS_PER_SECOND;
1201
     delta_calibration_radius = DELTA_CALIBRATION_RADIUS;
1201
     delta_calibration_radius = DELTA_CALIBRATION_RADIUS;
1202
-    delta_tower_angle_trim[A_AXIS] = dta[A_AXIS] - dta[C_AXIS];
1203
-    delta_tower_angle_trim[B_AXIS] = dta[B_AXIS] - dta[C_AXIS];
1202
+    COPY(delta_tower_angle_trim, dta);
1204
     home_offset[Z_AXIS] = 0;
1203
     home_offset[Z_AXIS] = 0;
1205
 
1204
 
1206
   #elif ENABLED(Z_DUAL_ENDSTOPS)
1205
   #elif ENABLED(Z_DUAL_ENDSTOPS)
1615
       SERIAL_ECHOPAIR(" B", LINEAR_UNIT(delta_calibration_radius));
1614
       SERIAL_ECHOPAIR(" B", LINEAR_UNIT(delta_calibration_radius));
1616
       SERIAL_ECHOPAIR(" X", LINEAR_UNIT(delta_tower_angle_trim[A_AXIS]));
1615
       SERIAL_ECHOPAIR(" X", LINEAR_UNIT(delta_tower_angle_trim[A_AXIS]));
1617
       SERIAL_ECHOPAIR(" Y", LINEAR_UNIT(delta_tower_angle_trim[B_AXIS]));
1616
       SERIAL_ECHOPAIR(" Y", LINEAR_UNIT(delta_tower_angle_trim[B_AXIS]));
1618
-      SERIAL_ECHOPAIR(" Z", 0.00);
1617
+      SERIAL_ECHOPAIR(" Z", LINEAR_UNIT(delta_tower_angle_trim[C_AXIS]));
1619
       SERIAL_EOL();
1618
       SERIAL_EOL();
1620
     #elif ENABLED(Z_DUAL_ENDSTOPS)
1619
     #elif ENABLED(Z_DUAL_ENDSTOPS)
1621
       if (!forReplay) {
1620
       if (!forReplay) {

+ 8
- 8
Marlin/src/module/delta.cpp View File

43
       delta_diagonal_rod,
43
       delta_diagonal_rod,
44
       delta_segments_per_second,
44
       delta_segments_per_second,
45
       delta_calibration_radius,
45
       delta_calibration_radius,
46
-      delta_tower_angle_trim[2];
46
+      delta_tower_angle_trim[ABC];
47
 
47
 
48
 float delta_tower[ABC][2],
48
 float delta_tower[ABC][2],
49
       delta_diagonal_rod_2_tower[ABC],
49
       delta_diagonal_rod_2_tower[ABC],
55
  * Recalculate factors used for delta kinematics whenever
55
  * Recalculate factors used for delta kinematics whenever
56
  * settings have been changed (e.g., by M665).
56
  * settings have been changed (e.g., by M665).
57
  */
57
  */
58
-void recalc_delta_settings(float radius, float diagonal_rod) {
58
+void recalc_delta_settings(const float radius, const float diagonal_rod, const float tower_angle_trim[ABC]) {
59
   const float trt[ABC] = DELTA_RADIUS_TRIM_TOWER,
59
   const float trt[ABC] = DELTA_RADIUS_TRIM_TOWER,
60
               drt[ABC] = DELTA_DIAGONAL_ROD_TRIM_TOWER;
60
               drt[ABC] = DELTA_DIAGONAL_ROD_TRIM_TOWER;
61
-  delta_tower[A_AXIS][X_AXIS] = cos(RADIANS(210 + delta_tower_angle_trim[A_AXIS])) * (radius + trt[A_AXIS]); // front left tower
62
-  delta_tower[A_AXIS][Y_AXIS] = sin(RADIANS(210 + delta_tower_angle_trim[A_AXIS])) * (radius + trt[A_AXIS]);
63
-  delta_tower[B_AXIS][X_AXIS] = cos(RADIANS(330 + delta_tower_angle_trim[B_AXIS])) * (radius + trt[B_AXIS]); // front right tower
64
-  delta_tower[B_AXIS][Y_AXIS] = sin(RADIANS(330 + delta_tower_angle_trim[B_AXIS])) * (radius + trt[B_AXIS]);
65
-  delta_tower[C_AXIS][X_AXIS] = 0.0; // back middle tower
66
-  delta_tower[C_AXIS][Y_AXIS] = (radius + trt[C_AXIS]);
61
+  delta_tower[A_AXIS][X_AXIS] = cos(RADIANS(210 + tower_angle_trim[A_AXIS])) * (radius + trt[A_AXIS]); // front left tower
62
+  delta_tower[A_AXIS][Y_AXIS] = sin(RADIANS(210 + tower_angle_trim[A_AXIS])) * (radius + trt[A_AXIS]);
63
+  delta_tower[B_AXIS][X_AXIS] = cos(RADIANS(330 + tower_angle_trim[B_AXIS])) * (radius + trt[B_AXIS]); // front right tower
64
+  delta_tower[B_AXIS][Y_AXIS] = sin(RADIANS(330 + tower_angle_trim[B_AXIS])) * (radius + trt[B_AXIS]);
65
+  delta_tower[C_AXIS][X_AXIS] = cos(RADIANS( 90 + tower_angle_trim[C_AXIS])) * (radius + trt[C_AXIS]); // back middle tower
66
+  delta_tower[C_AXIS][Y_AXIS] = sin(RADIANS( 90 + tower_angle_trim[C_AXIS])) * (radius + trt[C_AXIS]);
67
   delta_diagonal_rod_2_tower[A_AXIS] = sq(diagonal_rod + drt[A_AXIS]);
67
   delta_diagonal_rod_2_tower[A_AXIS] = sq(diagonal_rod + drt[A_AXIS]);
68
   delta_diagonal_rod_2_tower[B_AXIS] = sq(diagonal_rod + drt[B_AXIS]);
68
   delta_diagonal_rod_2_tower[B_AXIS] = sq(diagonal_rod + drt[B_AXIS]);
69
   delta_diagonal_rod_2_tower[C_AXIS] = sq(diagonal_rod + drt[C_AXIS]);
69
   delta_diagonal_rod_2_tower[C_AXIS] = sq(diagonal_rod + drt[C_AXIS]);

+ 2
- 2
Marlin/src/module/delta.h View File

32
              delta_diagonal_rod,
32
              delta_diagonal_rod,
33
              delta_segments_per_second,
33
              delta_segments_per_second,
34
              delta_calibration_radius,
34
              delta_calibration_radius,
35
-             delta_tower_angle_trim[2];
35
+             delta_tower_angle_trim[ABC];
36
 
36
 
37
 extern float delta_tower[ABC][2],
37
 extern float delta_tower[ABC][2],
38
              delta_diagonal_rod_2_tower[ABC],
38
              delta_diagonal_rod_2_tower[ABC],
42
  * Recalculate factors used for delta kinematics whenever
42
  * Recalculate factors used for delta kinematics whenever
43
  * settings have been changed (e.g., by M665).
43
  * settings have been changed (e.g., by M665).
44
  */
44
  */
45
-void recalc_delta_settings(float radius, float diagonal_rod);
45
+void recalc_delta_settings(const float radius, const float diagonal_rod, const float tower_angle_trim[ABC]);
46
 
46
 
47
 /**
47
 /**
48
  * Delta Inverse Kinematics
48
  * Delta Inverse Kinematics

+ 1
- 1
Marlin/src/module/motion.cpp View File

1134
       #if ENABLED(DEBUG_LEVELING_FEATURE)
1134
       #if ENABLED(DEBUG_LEVELING_FEATURE)
1135
         if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("delta_endstop_adj:");
1135
         if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("delta_endstop_adj:");
1136
       #endif
1136
       #endif
1137
-      do_homing_move(axis, delta_endstop_adj[axis] - 0.1);
1137
+      do_homing_move(axis, delta_endstop_adj[axis] - 0.1 * Z_HOME_DIR);
1138
     }
1138
     }
1139
 
1139
 
1140
   #else
1140
   #else

Loading…
Cancel
Save