Browse Source

Comment/cleanup motion code

Scott Lahteine 7 years ago
parent
commit
8b7c274db5
5 changed files with 160 additions and 200 deletions
  1. 27
    1
      Marlin/Marlin.h
  2. 52
    60
      Marlin/Marlin_main.cpp
  3. 3
    3
      Marlin/planner.h
  4. 1
    2
      Marlin/stepper.cpp
  5. 77
    134
      Marlin/ubl_motion.cpp

+ 27
- 1
Marlin/Marlin.h View File

@@ -301,12 +301,38 @@ void report_current_position();
301 301
   extern float delta_height,
302 302
                delta_endstop_adj[ABC],
303 303
                delta_radius,
304
+               delta_tower_angle_trim[ABC],
305
+               delta_tower[ABC][2],
304 306
                delta_diagonal_rod,
305 307
                delta_calibration_radius,
308
+               delta_diagonal_rod_2_tower[ABC],
306 309
                delta_segments_per_second,
307
-               delta_tower_angle_trim[ABC],
308 310
                delta_clip_start_height;
311
+
309 312
   void recalc_delta_settings();
313
+  float delta_safe_distance_from_top();
314
+
315
+  #if ENABLED(DELTA_FAST_SQRT)
316
+    float Q_rsqrt(const float number);
317
+    #define _SQRT(n) (1.0f / Q_rsqrt(n))
318
+  #else
319
+    #define _SQRT(n) SQRT(n)
320
+  #endif
321
+
322
+  // Macro to obtain the Z position of an individual tower
323
+  #define DELTA_Z(T) raw[Z_AXIS] + _SQRT(     \
324
+    delta_diagonal_rod_2_tower[T] - HYPOT2(   \
325
+        delta_tower[T][X_AXIS] - raw[X_AXIS], \
326
+        delta_tower[T][Y_AXIS] - raw[Y_AXIS]  \
327
+      )                                       \
328
+    )
329
+
330
+  #define DELTA_RAW_IK() do {        \
331
+    delta[A_AXIS] = DELTA_Z(A_AXIS); \
332
+    delta[B_AXIS] = DELTA_Z(B_AXIS); \
333
+    delta[C_AXIS] = DELTA_Z(C_AXIS); \
334
+  }while(0)
335
+
310 336
 #elif IS_SCARA
311 337
   void forward_kinematics_SCARA(const float &a, const float &b);
312 338
 #endif

+ 52
- 60
Marlin/Marlin_main.cpp View File

@@ -12258,7 +12258,7 @@ void ok_to_send() {
12258 12258
      * Fast inverse sqrt from Quake III Arena
12259 12259
      * See: https://en.wikipedia.org/wiki/Fast_inverse_square_root
12260 12260
      */
12261
-    float Q_rsqrt(float number) {
12261
+    float Q_rsqrt(const float number) {
12262 12262
       long i;
12263 12263
       float x2, y;
12264 12264
       const float threehalfs = 1.5f;
@@ -12272,12 +12272,6 @@ void ok_to_send() {
12272 12272
       return y;
12273 12273
     }
12274 12274
 
12275
-    #define _SQRT(n) (1.0f / Q_rsqrt(n))
12276
-
12277
-  #else
12278
-
12279
-    #define _SQRT(n) SQRT(n)
12280
-
12281 12275
   #endif
12282 12276
 
12283 12277
   /**
@@ -12299,20 +12293,6 @@ void ok_to_send() {
12299 12293
    *   (see above)
12300 12294
    */
12301 12295
 
12302
-  // Macro to obtain the Z position of an individual tower
12303
-  #define DELTA_Z(T) raw[Z_AXIS] + _SQRT(     \
12304
-    delta_diagonal_rod_2_tower[T] - HYPOT2(   \
12305
-        delta_tower[T][X_AXIS] - raw[X_AXIS], \
12306
-        delta_tower[T][Y_AXIS] - raw[Y_AXIS]  \
12307
-      )                                       \
12308
-    )
12309
-
12310
-  #define DELTA_RAW_IK() do {        \
12311
-    delta[A_AXIS] = DELTA_Z(A_AXIS); \
12312
-    delta[B_AXIS] = DELTA_Z(B_AXIS); \
12313
-    delta[C_AXIS] = DELTA_Z(C_AXIS); \
12314
-  }while(0)
12315
-
12316 12296
   #define DELTA_DEBUG() do { \
12317 12297
       SERIAL_ECHOPAIR("cartesian X:", raw[X_AXIS]); \
12318 12298
       SERIAL_ECHOPAIR(" Y:", raw[Y_AXIS]);          \
@@ -12367,46 +12347,53 @@ void ok_to_send() {
12367 12347
    */
12368 12348
   void forward_kinematics_DELTA(float z1, float z2, float z3) {
12369 12349
     // Create a vector in old coordinates along x axis of new coordinate
12370
-    float p12[3] = { delta_tower[B_AXIS][X_AXIS] - delta_tower[A_AXIS][X_AXIS], delta_tower[B_AXIS][Y_AXIS] - delta_tower[A_AXIS][Y_AXIS], z2 - z1 };
12350
+    const float p12[] = {
12351
+      delta_tower[B_AXIS][X_AXIS] - delta_tower[A_AXIS][X_AXIS],
12352
+      delta_tower[B_AXIS][Y_AXIS] - delta_tower[A_AXIS][Y_AXIS],
12353
+      z2 - z1
12354
+    },
12371 12355
 
12372 12356
     // Get the Magnitude of vector.
12373
-    float d = SQRT( sq(p12[0]) + sq(p12[1]) + sq(p12[2]) );
12357
+    d = SQRT(sq(p12[0]) + sq(p12[1]) + sq(p12[2])),
12374 12358
 
12375 12359
     // Create unit vector by dividing by magnitude.
12376
-    float ex[3] = { p12[0] / d, p12[1] / d, p12[2] / d };
12360
+    ex[3] = { p12[0] / d, p12[1] / d, p12[2] / d },
12377 12361
 
12378 12362
     // Get the vector from the origin of the new system to the third point.
12379
-    float p13[3] = { delta_tower[C_AXIS][X_AXIS] - delta_tower[A_AXIS][X_AXIS], delta_tower[C_AXIS][Y_AXIS] - delta_tower[A_AXIS][Y_AXIS], z3 - z1 };
12363
+    p13[3] = {
12364
+      delta_tower[C_AXIS][X_AXIS] - delta_tower[A_AXIS][X_AXIS],
12365
+      delta_tower[C_AXIS][Y_AXIS] - delta_tower[A_AXIS][Y_AXIS],
12366
+      z3 - z1
12367
+    },
12380 12368
 
12381 12369
     // Use the dot product to find the component of this vector on the X axis.
12382
-    float i = ex[0] * p13[0] + ex[1] * p13[1] + ex[2] * p13[2];
12370
+    i = ex[0] * p13[0] + ex[1] * p13[1] + ex[2] * p13[2],
12383 12371
 
12384 12372
     // Create a vector along the x axis that represents the x component of p13.
12385
-    float iex[3] = { ex[0] * i, ex[1] * i, ex[2] * i };
12373
+    iex[] = { ex[0] * i, ex[1] * i, ex[2] * i };
12386 12374
 
12387 12375
     // Subtract the X component from the original vector leaving only Y. We use the
12388 12376
     // variable that will be the unit vector after we scale it.
12389 12377
     float ey[3] = { p13[0] - iex[0], p13[1] - iex[1], p13[2] - iex[2] };
12390 12378
 
12391 12379
     // The magnitude of Y component
12392
-    float j = SQRT( sq(ey[0]) + sq(ey[1]) + sq(ey[2]) );
12380
+    const float j = SQRT(sq(ey[0]) + sq(ey[1]) + sq(ey[2]));
12393 12381
 
12394 12382
     // Convert to a unit vector
12395 12383
     ey[0] /= j; ey[1] /= j;  ey[2] /= j;
12396 12384
 
12397 12385
     // The cross product of the unit x and y is the unit z
12398 12386
     // float[] ez = vectorCrossProd(ex, ey);
12399
-    float ez[3] = {
12387
+    const float ez[3] = {
12400 12388
       ex[1] * ey[2] - ex[2] * ey[1],
12401 12389
       ex[2] * ey[0] - ex[0] * ey[2],
12402 12390
       ex[0] * ey[1] - ex[1] * ey[0]
12403
-    };
12404
-
12391
+    },
12405 12392
     // We now have the d, i and j values defined in Wikipedia.
12406 12393
     // Plug them into the equations defined in Wikipedia for Xnew, Ynew and Znew
12407
-    float Xnew = (delta_diagonal_rod_2_tower[A_AXIS] - delta_diagonal_rod_2_tower[B_AXIS] + sq(d)) / (d * 2),
12408
-          Ynew = ((delta_diagonal_rod_2_tower[A_AXIS] - delta_diagonal_rod_2_tower[C_AXIS] + HYPOT2(i, j)) / 2 - i * Xnew) / j,
12409
-          Znew = SQRT(delta_diagonal_rod_2_tower[A_AXIS] - HYPOT2(Xnew, Ynew));
12394
+    Xnew = (delta_diagonal_rod_2_tower[A_AXIS] - delta_diagonal_rod_2_tower[B_AXIS] + sq(d)) / (d * 2),
12395
+    Ynew = ((delta_diagonal_rod_2_tower[A_AXIS] - delta_diagonal_rod_2_tower[C_AXIS] + HYPOT2(i, j)) / 2 - i * Xnew) / j,
12396
+    Znew = SQRT(delta_diagonal_rod_2_tower[A_AXIS] - HYPOT2(Xnew, Ynew));
12410 12397
 
12411 12398
     // Start from the origin of the old coordinates and add vectors in the
12412 12399
     // old coords that represent the Xnew, Ynew and Znew to find the point
@@ -12478,7 +12465,7 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
12478 12465
    * small incremental moves. This allows the planner to
12479 12466
    * apply more detailed bed leveling to the full move.
12480 12467
    */
12481
-  inline void segmented_line_to_destination(const float fr_mm_s, const float segment_size=LEVELED_SEGMENT_LENGTH) {
12468
+  inline void segmented_line_to_destination(const float &fr_mm_s, const float segment_size=LEVELED_SEGMENT_LENGTH) {
12482 12469
 
12483 12470
     const float xdiff = destination[X_AXIS] - current_position[X_AXIS],
12484 12471
                 ydiff = destination[Y_AXIS] - current_position[Y_AXIS];
@@ -12517,16 +12504,12 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
12517 12504
     // SERIAL_ECHOPAIR("mm=", cartesian_mm);
12518 12505
     // SERIAL_ECHOLNPAIR(" segments=", segments);
12519 12506
 
12520
-    // Drop one segment so the last move is to the exact target.
12521
-    // If there's only 1 segment, loops will be skipped entirely.
12522
-    --segments;
12523
-
12524 12507
     // Get the raw current position as starting point
12525 12508
     float raw[XYZE];
12526 12509
     COPY(raw, current_position);
12527 12510
 
12528 12511
     // Calculate and execute the segments
12529
-    for (uint16_t s = segments + 1; --s;) {
12512
+    while (--segments) {
12530 12513
       static millis_t next_idle_ms = millis() + 200UL;
12531 12514
       thermalManager.manage_heater();  // This returns immediately if not really needed.
12532 12515
       if (ELAPSED(millis(), next_idle_ms)) {
@@ -12548,7 +12531,8 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
12548 12531
    * Prepare a mesh-leveled linear move in a Cartesian setup,
12549 12532
    * splitting the move where it crosses mesh borders.
12550 12533
    */
12551
-  void mesh_line_to_destination(const float fr_mm_s, uint8_t x_splits = 0xFF, uint8_t y_splits = 0xFF) {
12534
+  void mesh_line_to_destination(const float fr_mm_s, uint8_t x_splits=0xFF, uint8_t y_splits=0xFF) {
12535
+    // Get current and destination cells for this line
12552 12536
     int cx1 = mbl.cell_index_x(current_position[X_AXIS]),
12553 12537
         cy1 = mbl.cell_index_y(current_position[Y_AXIS]),
12554 12538
         cx2 = mbl.cell_index_x(destination[X_AXIS]),
@@ -12558,8 +12542,8 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
12558 12542
     NOMORE(cx2, GRID_MAX_POINTS_X - 2);
12559 12543
     NOMORE(cy2, GRID_MAX_POINTS_Y - 2);
12560 12544
 
12545
+    // Start and end in the same cell? No split needed.
12561 12546
     if (cx1 == cx2 && cy1 == cy2) {
12562
-      // Start and end on same mesh square
12563 12547
       buffer_line_to_destination(fr_mm_s);
12564 12548
       set_current_from_destination();
12565 12549
       return;
@@ -12568,25 +12552,30 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
12568 12552
     #define MBL_SEGMENT_END(A) (current_position[A ##_AXIS] + (destination[A ##_AXIS] - current_position[A ##_AXIS]) * normalized_dist)
12569 12553
 
12570 12554
     float normalized_dist, end[XYZE];
12571
-
12572
-    // Split at the left/front border of the right/top square
12573 12555
     const int8_t gcx = max(cx1, cx2), gcy = max(cy1, cy2);
12556
+
12557
+    // Crosses on the X and not already split on this X?
12558
+    // The x_splits flags are insurance against rounding errors.
12574 12559
     if (cx2 != cx1 && TEST(x_splits, gcx)) {
12560
+      // Split on the X grid line
12561
+      CBI(x_splits, gcx);
12575 12562
       COPY(end, destination);
12576 12563
       destination[X_AXIS] = mbl.index_to_xpos[gcx];
12577 12564
       normalized_dist = (destination[X_AXIS] - current_position[X_AXIS]) / (end[X_AXIS] - current_position[X_AXIS]);
12578 12565
       destination[Y_AXIS] = MBL_SEGMENT_END(Y);
12579
-      CBI(x_splits, gcx);
12580 12566
     }
12567
+    // Crosses on the Y and not already split on this Y?
12581 12568
     else if (cy2 != cy1 && TEST(y_splits, gcy)) {
12569
+      // Split on the Y grid line
12570
+      CBI(y_splits, gcy);
12582 12571
       COPY(end, destination);
12583 12572
       destination[Y_AXIS] = mbl.index_to_ypos[gcy];
12584 12573
       normalized_dist = (destination[Y_AXIS] - current_position[Y_AXIS]) / (end[Y_AXIS] - current_position[Y_AXIS]);
12585 12574
       destination[X_AXIS] = MBL_SEGMENT_END(X);
12586
-      CBI(y_splits, gcy);
12587 12575
     }
12588 12576
     else {
12589
-      // Already split on a border
12577
+      // Must already have been split on these border(s)
12578
+      // This should be a rare case.
12590 12579
       buffer_line_to_destination(fr_mm_s);
12591 12580
       set_current_from_destination();
12592 12581
       return;
@@ -12611,7 +12600,8 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
12611 12600
    * Prepare a bilinear-leveled linear move on Cartesian,
12612 12601
    * splitting the move where it crosses grid borders.
12613 12602
    */
12614
-  void bilinear_line_to_destination(const float fr_mm_s, uint16_t x_splits = 0xFFFF, uint16_t y_splits = 0xFFFF) {
12603
+  void bilinear_line_to_destination(const float fr_mm_s, uint16_t x_splits=0xFFFF, uint16_t y_splits=0xFFFF) {
12604
+    // Get current and destination cells for this line
12615 12605
     int cx1 = CELL_INDEX(X, current_position[X_AXIS]),
12616 12606
         cy1 = CELL_INDEX(Y, current_position[Y_AXIS]),
12617 12607
         cx2 = CELL_INDEX(X, destination[X_AXIS]),
@@ -12621,8 +12611,8 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
12621 12611
     cx2 = constrain(cx2, 0, ABL_BG_POINTS_X - 2);
12622 12612
     cy2 = constrain(cy2, 0, ABL_BG_POINTS_Y - 2);
12623 12613
 
12614
+    // Start and end in the same cell? No split needed.
12624 12615
     if (cx1 == cx2 && cy1 == cy2) {
12625
-      // Start and end on same mesh square
12626 12616
       buffer_line_to_destination(fr_mm_s);
12627 12617
       set_current_from_destination();
12628 12618
       return;
@@ -12631,25 +12621,30 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
12631 12621
     #define LINE_SEGMENT_END(A) (current_position[A ##_AXIS] + (destination[A ##_AXIS] - current_position[A ##_AXIS]) * normalized_dist)
12632 12622
 
12633 12623
     float normalized_dist, end[XYZE];
12634
-
12635
-    // Split at the left/front border of the right/top square
12636 12624
     const int8_t gcx = max(cx1, cx2), gcy = max(cy1, cy2);
12625
+
12626
+    // Crosses on the X and not already split on this X?
12627
+    // The x_splits flags are insurance against rounding errors.
12637 12628
     if (cx2 != cx1 && TEST(x_splits, gcx)) {
12629
+      // Split on the X grid line
12630
+      CBI(x_splits, gcx);
12638 12631
       COPY(end, destination);
12639 12632
       destination[X_AXIS] = bilinear_start[X_AXIS] + ABL_BG_SPACING(X_AXIS) * gcx;
12640 12633
       normalized_dist = (destination[X_AXIS] - current_position[X_AXIS]) / (end[X_AXIS] - current_position[X_AXIS]);
12641 12634
       destination[Y_AXIS] = LINE_SEGMENT_END(Y);
12642
-      CBI(x_splits, gcx);
12643 12635
     }
12636
+    // Crosses on the Y and not already split on this Y?
12644 12637
     else if (cy2 != cy1 && TEST(y_splits, gcy)) {
12638
+      // Split on the Y grid line
12639
+      CBI(y_splits, gcy);
12645 12640
       COPY(end, destination);
12646 12641
       destination[Y_AXIS] = bilinear_start[Y_AXIS] + ABL_BG_SPACING(Y_AXIS) * gcy;
12647 12642
       normalized_dist = (destination[Y_AXIS] - current_position[Y_AXIS]) / (end[Y_AXIS] - current_position[Y_AXIS]);
12648 12643
       destination[X_AXIS] = LINE_SEGMENT_END(X);
12649
-      CBI(y_splits, gcy);
12650 12644
     }
12651 12645
     else {
12652
-      // Already split on a border
12646
+      // Must already have been split on these border(s)
12647
+      // This should be a rare case.
12653 12648
       buffer_line_to_destination(fr_mm_s);
12654 12649
       set_current_from_destination();
12655 12650
       return;
@@ -12745,16 +12740,13 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
12745 12740
             oldB = stepper.get_axis_position_degrees(B_AXIS);
12746 12741
     #endif
12747 12742
 
12748
-    // Get the raw current position as starting point
12743
+    // Get the current position as starting point
12749 12744
     float raw[XYZE];
12750 12745
     COPY(raw, current_position);
12751 12746
 
12752
-    // Drop one segment so the last move is to the exact target.
12753
-    // If there's only 1 segment, loops will be skipped entirely.
12754
-    --segments;
12755 12747
 
12756 12748
     // Calculate and execute the segments
12757
-    for (uint16_t s = segments + 1; --s;) {
12749
+    while (--segments) {
12758 12750
 
12759 12751
       static millis_t next_idle_ms = millis() + 200UL;
12760 12752
       thermalManager.manage_heater();  // This returns immediately if not really needed.
@@ -13033,7 +13025,7 @@ void prepare_move_to_destination() {
13033 13025
     if (mm_of_travel < 0.001) return;
13034 13026
 
13035 13027
     uint16_t segments = FLOOR(mm_of_travel / (MM_PER_ARC_SEGMENT));
13036
-    if (segments == 0) segments = 1;
13028
+    NOLESS(segments, 1);
13037 13029
 
13038 13030
     /**
13039 13031
      * Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,

+ 3
- 3
Marlin/planner.h View File

@@ -140,7 +140,7 @@ class Planner {
140 140
       static uint8_t last_extruder;             // Respond to extruder change
141 141
     #endif
142 142
 
143
-    static int16_t flow_percentage[EXTRUDERS]; // Extrusion factor for each extruder
143
+    static int16_t flow_percentage[EXTRUDERS];  // Extrusion factor for each extruder
144 144
 
145 145
     static float e_factor[EXTRUDERS],               // The flow percentage and volumetric multiplier combine to scale E movement
146 146
                  filament_size[EXTRUDERS],          // diameter of filament (in millimeters), typically around 1.75 or 2.85, 0 disables the volumetric calculations for the extruder
@@ -501,8 +501,8 @@ class Planner {
501 501
     /**
502 502
      * Get the index of the next / previous block in the ring buffer
503 503
      */
504
-    static int8_t next_block_index(int8_t block_index) { return BLOCK_MOD(block_index + 1); }
505
-    static int8_t prev_block_index(int8_t block_index) { return BLOCK_MOD(block_index - 1); }
504
+    static int8_t next_block_index(const int8_t block_index) { return BLOCK_MOD(block_index + 1); }
505
+    static int8_t prev_block_index(const int8_t block_index) { return BLOCK_MOD(block_index - 1); }
506 506
 
507 507
     /**
508 508
      * Calculate the distance (not time) it takes to accelerate

+ 1
- 2
Marlin/stepper.cpp View File

@@ -443,8 +443,7 @@ void Stepper::isr() {
443 443
   // If there is no current block, attempt to pop one from the buffer
444 444
   if (!current_block) {
445 445
     // Anything in the buffer?
446
-    current_block = planner.get_current_block();
447
-    if (current_block) {
446
+    if ((current_block = planner.get_current_block())) {
448 447
       trapezoid_generator_reset();
449 448
 
450 449
       // Initialize Bresenham counters to 1/2 the ceiling

+ 77
- 134
Marlin/ubl_motion.cpp View File

@@ -38,25 +38,6 @@
38 38
     extern void set_current_from_destination();
39 39
   #endif
40 40
 
41
-  #if ENABLED(DELTA)
42
-
43
-    extern float delta[ABC];
44
-
45
-    extern float delta_endstop_adj[ABC],
46
-                 delta_radius,
47
-                 delta_tower_angle_trim[ABC],
48
-                 delta_tower[ABC][2],
49
-                 delta_diagonal_rod,
50
-                 delta_calibration_radius,
51
-                 delta_diagonal_rod_2_tower[ABC],
52
-                 delta_segments_per_second,
53
-                 delta_clip_start_height;
54
-
55
-    extern float delta_safe_distance_from_top();
56
-
57
-  #endif
58
-
59
-
60 41
   static void debug_echo_axis(const AxisEnum axis) {
61 42
     if (current_position[axis] == destination[axis])
62 43
       SERIAL_ECHOPGM("-------------");
@@ -268,9 +249,9 @@
268 249
          * else, we know the next X is the same so we can recover and continue!
269 250
          * Calculate X at the next Y mesh line
270 251
          */
271
-        const float x = inf_m_flag ? start[X_AXIS] : (next_mesh_line_y - c) / m;
252
+        const float rx = inf_m_flag ? start[X_AXIS] : (next_mesh_line_y - c) / m;
272 253
 
273
-        float z0 = z_correction_for_x_on_horizontal_mesh_line(x, current_xi, current_yi)
254
+        float z0 = z_correction_for_x_on_horizontal_mesh_line(rx, current_xi, current_yi)
274 255
                    * planner.fade_scaling_factor_for_z(end[Z_AXIS]);
275 256
 
276 257
         /**
@@ -282,7 +263,7 @@
282 263
          */
283 264
         if (isnan(z0)) z0 = 0.0;
284 265
 
285
-        const float y = mesh_index_to_ypos(current_yi);
266
+        const float ry = mesh_index_to_ypos(current_yi);
286 267
 
287 268
         /**
288 269
          * Without this check, it is possible for the algorithm to generate a zero length move in the case
@@ -290,9 +271,9 @@
290 271
          * happens, it might be best to remove the check and always 'schedule' the move because
291 272
          * the planner._buffer_line() routine will filter it if that happens.
292 273
          */
293
-        if (y != start[Y_AXIS]) {
274
+        if (ry != start[Y_AXIS]) {
294 275
           if (!inf_normalized_flag) {
295
-            on_axis_distance = use_x_dist ? x - start[X_AXIS] : y - start[Y_AXIS];
276
+            on_axis_distance = use_x_dist ? rx - start[X_AXIS] : ry - start[Y_AXIS];
296 277
             e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;
297 278
             z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
298 279
           }
@@ -301,7 +282,7 @@
301 282
             z_position = end[Z_AXIS];
302 283
           }
303 284
 
304
-          planner._buffer_line(x, y, z_position + z0, e_position, feed_rate, extruder);
285
+          planner._buffer_line(rx, ry, z_position + z0, e_position, feed_rate, extruder);
305 286
         } //else printf("FIRST MOVE PRUNED  ");
306 287
       }
307 288
 
@@ -332,9 +313,9 @@
332 313
       while (current_xi != cell_dest_xi + left_flag) {
333 314
         current_xi += dxi;
334 315
         const float next_mesh_line_x = mesh_index_to_xpos(current_xi),
335
-                    y = m * next_mesh_line_x + c;   // Calculate Y at the next X mesh line
316
+                    ry = m * next_mesh_line_x + c;   // Calculate Y at the next X mesh line
336 317
 
337
-        float z0 = z_correction_for_y_on_vertical_mesh_line(y, current_xi, current_yi)
318
+        float z0 = z_correction_for_y_on_vertical_mesh_line(ry, current_xi, current_yi)
338 319
                    * planner.fade_scaling_factor_for_z(end[Z_AXIS]);
339 320
 
340 321
         /**
@@ -346,7 +327,7 @@
346 327
          */
347 328
         if (isnan(z0)) z0 = 0.0;
348 329
 
349
-        const float x = mesh_index_to_xpos(current_xi);
330
+        const float rx = mesh_index_to_xpos(current_xi);
350 331
 
351 332
         /**
352 333
          * Without this check, it is possible for the algorithm to generate a zero length move in the case
@@ -354,9 +335,9 @@
354 335
          * that happens, it might be best to remove the check and always 'schedule' the move because
355 336
          * the planner._buffer_line() routine will filter it if that happens.
356 337
          */
357
-        if (x != start[X_AXIS]) {
338
+        if (rx != start[X_AXIS]) {
358 339
           if (!inf_normalized_flag) {
359
-            on_axis_distance = use_x_dist ? x - start[X_AXIS] : y - start[Y_AXIS];
340
+            on_axis_distance = use_x_dist ? rx - start[X_AXIS] : ry - start[Y_AXIS];
360 341
             e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;  // is based on X or Y because this is a horizontal move
361 342
             z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
362 343
           }
@@ -365,7 +346,7 @@
365 346
             z_position = end[Z_AXIS];
366 347
           }
367 348
 
368
-          planner._buffer_line(x, y, z_position + z0, e_position, feed_rate, extruder);
349
+          planner._buffer_line(rx, ry, z_position + z0, e_position, feed_rate, extruder);
369 350
         } //else printf("FIRST MOVE PRUNED  ");
370 351
       }
371 352
 
@@ -398,15 +379,15 @@
398 379
 
399 380
       const float next_mesh_line_x = mesh_index_to_xpos(current_xi + dxi),
400 381
                   next_mesh_line_y = mesh_index_to_ypos(current_yi + dyi),
401
-                  y = m * next_mesh_line_x + c,   // Calculate Y at the next X mesh line
402
-                  x = (next_mesh_line_y - c) / m; // Calculate X at the next Y mesh line
403
-                                                  // (No need to worry about m being zero.
404
-                                                  //  If that was the case, it was already detected
405
-                                                  //  as a vertical line move above.)
382
+                  ry = m * next_mesh_line_x + c,   // Calculate Y at the next X mesh line
383
+                  rx = (next_mesh_line_y - c) / m; // Calculate X at the next Y mesh line
384
+                                                   // (No need to worry about m being zero.
385
+                                                   //  If that was the case, it was already detected
386
+                                                   //  as a vertical line move above.)
406 387
 
407
-      if (left_flag == (x > next_mesh_line_x)) { // Check if we hit the Y line first
388
+      if (left_flag == (rx > next_mesh_line_x)) { // Check if we hit the Y line first
408 389
         // Yes!  Crossing a Y Mesh Line next
409
-        float z0 = z_correction_for_x_on_horizontal_mesh_line(x, current_xi - left_flag, current_yi + dyi)
390
+        float z0 = z_correction_for_x_on_horizontal_mesh_line(rx, current_xi - left_flag, current_yi + dyi)
410 391
                    * planner.fade_scaling_factor_for_z(end[Z_AXIS]);
411 392
 
412 393
         /**
@@ -419,7 +400,7 @@
419 400
         if (isnan(z0)) z0 = 0.0;
420 401
 
421 402
         if (!inf_normalized_flag) {
422
-          on_axis_distance = use_x_dist ? x - start[X_AXIS] : next_mesh_line_y - start[Y_AXIS];
403
+          on_axis_distance = use_x_dist ? rx - start[X_AXIS] : next_mesh_line_y - start[Y_AXIS];
423 404
           e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;
424 405
           z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
425 406
         }
@@ -427,13 +408,13 @@
427 408
           e_position = end[E_AXIS];
428 409
           z_position = end[Z_AXIS];
429 410
         }
430
-        planner._buffer_line(x, next_mesh_line_y, z_position + z0, e_position, feed_rate, extruder);
411
+        planner._buffer_line(rx, next_mesh_line_y, z_position + z0, e_position, feed_rate, extruder);
431 412
         current_yi += dyi;
432 413
         yi_cnt--;
433 414
       }
434 415
       else {
435 416
         // Yes!  Crossing a X Mesh Line next
436
-        float z0 = z_correction_for_y_on_vertical_mesh_line(y, current_xi + dxi, current_yi - down_flag)
417
+        float z0 = z_correction_for_y_on_vertical_mesh_line(ry, current_xi + dxi, current_yi - down_flag)
437 418
                    * planner.fade_scaling_factor_for_z(end[Z_AXIS]);
438 419
 
439 420
         /**
@@ -446,7 +427,7 @@
446 427
         if (isnan(z0)) z0 = 0.0;
447 428
 
448 429
         if (!inf_normalized_flag) {
449
-          on_axis_distance = use_x_dist ? next_mesh_line_x - start[X_AXIS] : y - start[Y_AXIS];
430
+          on_axis_distance = use_x_dist ? next_mesh_line_x - start[X_AXIS] : ry - start[Y_AXIS];
450 431
           e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;
451 432
           z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
452 433
         }
@@ -455,7 +436,7 @@
455 436
           z_position = end[Z_AXIS];
456 437
         }
457 438
 
458
-        planner._buffer_line(next_mesh_line_x, y, z_position + z0, e_position, feed_rate, extruder);
439
+        planner._buffer_line(next_mesh_line_x, ry, z_position + z0, e_position, feed_rate, extruder);
459 440
         current_xi += dxi;
460 441
         xi_cnt--;
461 442
       }
@@ -489,29 +470,16 @@
489 470
     // We don't want additional apply_leveling() performed by regular buffer_line or buffer_line_kinematic,
490 471
     // so we call _buffer_line directly here.  Per-segmented leveling and kinematics performed first.
491 472
 
492
-    inline void _O2 ubl_buffer_segment_raw(const float &rx, const float &ry, const float rz, const float &e, const float &fr) {
473
+    inline void _O2 ubl_buffer_segment_raw(const float raw[XYZE], const float &fr) {
493 474
 
494 475
       #if ENABLED(DELTA)  // apply delta inverse_kinematics
495 476
 
496
-        const float delta_A = rz + SQRT( delta_diagonal_rod_2_tower[A_AXIS]
497
-                                         - HYPOT2( delta_tower[A_AXIS][X_AXIS] - rx,
498
-                                                   delta_tower[A_AXIS][Y_AXIS] - ry ));
499
-
500
-        const float delta_B = rz + SQRT( delta_diagonal_rod_2_tower[B_AXIS]
501
-                                         - HYPOT2( delta_tower[B_AXIS][X_AXIS] - rx,
502
-                                                   delta_tower[B_AXIS][Y_AXIS] - ry ));
503
-
504
-        const float delta_C = rz + SQRT( delta_diagonal_rod_2_tower[C_AXIS]
505
-                                         - HYPOT2( delta_tower[C_AXIS][X_AXIS] - rx,
506
-                                                   delta_tower[C_AXIS][Y_AXIS] - ry ));
477
+        DELTA_RAW_IK();
478
+        planner._buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], fr, active_extruder);
507 479
 
508
-        planner._buffer_line(delta_A, delta_B, delta_C, e, fr, active_extruder);
480
+      #elif IS_SCARA  // apply scara inverse_kinematics (should be changed to save raw->logical->raw)
509 481
 
510
-      #elif IS_SCARA  // apply scara inverse_kinematics
511
-
512
-        const float lseg[XYZ] = { rx, ry, rz };
513
-
514
-        inverse_kinematics(lseg); // this writes delta[ABC] from lseg[XYZ]
482
+        inverse_kinematics(raw);  // this writes delta[ABC] from raw[XYZE]
515 483
                                   // should move the feedrate scaling to scara inverse_kinematics
516 484
 
517 485
         const float adiff = FABS(delta[A_AXIS] - scara_oldA),
@@ -520,14 +488,13 @@
520 488
         scara_oldB = delta[B_AXIS];
521 489
         float s_feedrate = max(adiff, bdiff) * scara_feed_factor;
522 490
 
523
-        planner._buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], e, s_feedrate, active_extruder);
491
+        planner._buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], s_feedrate, active_extruder);
524 492
 
525 493
       #else // CARTESIAN
526 494
 
527
-        planner._buffer_line(rx, ry, rz, e, fr, active_extruder);
495
+        planner._buffer_line(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], raw[E_AXIS], fr, active_extruder);
528 496
 
529 497
       #endif
530
-
531 498
     }
532 499
 
533 500
 
@@ -542,12 +509,14 @@
542 509
       if (!position_is_reachable(rtarget[X_AXIS], rtarget[Y_AXIS]))  // fail if moving outside reachable boundary
543 510
         return true; // did not move, so current_position still accurate
544 511
 
545
-      const float tot_dx = rtarget[X_AXIS] - current_position[X_AXIS],
546
-                  tot_dy = rtarget[Y_AXIS] - current_position[Y_AXIS],
547
-                  tot_dz = rtarget[Z_AXIS] - current_position[Z_AXIS],
548
-                  tot_de = rtarget[E_AXIS] - current_position[E_AXIS];
512
+      const float total[XYZE] = {
513
+        rtarget[X_AXIS] - current_position[X_AXIS],
514
+        rtarget[Y_AXIS] - current_position[Y_AXIS],
515
+        rtarget[Z_AXIS] - current_position[Z_AXIS],
516
+        rtarget[E_AXIS] - current_position[E_AXIS]
517
+      };
549 518
 
550
-      const float cartesian_xy_mm = HYPOT(tot_dx, tot_dy);  // total horizontal xy distance
519
+      const float cartesian_xy_mm = HYPOT(total[X_AXIS], total[Y_AXIS]);  // total horizontal xy distance
551 520
 
552 521
       #if IS_KINEMATIC
553 522
         const float seconds = cartesian_xy_mm / feedrate;                                  // seconds to move xy distance at requested rate
@@ -567,49 +536,30 @@
567 536
         scara_oldB = stepper.get_axis_position_degrees(B_AXIS);
568 537
       #endif
569 538
 
570
-      const float seg_dx = tot_dx * inv_segments,
571
-                  seg_dy = tot_dy * inv_segments,
572
-                  seg_dz = tot_dz * inv_segments,
573
-                  seg_de = tot_de * inv_segments;
539
+      const float diff[XYZE] = {
540
+        total[X_AXIS] * inv_segments,
541
+        total[Y_AXIS] * inv_segments,
542
+        total[Z_AXIS] * inv_segments,
543
+        total[E_AXIS] * inv_segments
544
+      };
574 545
 
575 546
       // Note that E segment distance could vary slightly as z mesh height
576 547
       // changes for each segment, but small enough to ignore.
577 548
 
578
-      float seg_rx = current_position[X_AXIS],
579
-            seg_ry = current_position[Y_AXIS],
580
-            seg_rz = current_position[Z_AXIS],
581
-            seg_le = current_position[E_AXIS];
582
-
583
-      const bool above_fade_height = (
584
-        #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
585
-          planner.z_fade_height != 0 && planner.z_fade_height < rtarget[Z_AXIS]
586
-        #else
587
-          false
588
-        #endif
589
-      );
549
+      float raw[XYZE] = {
550
+        current_position[X_AXIS],
551
+        current_position[Y_AXIS],
552
+        current_position[Z_AXIS],
553
+        current_position[E_AXIS]
554
+      };
590 555
 
591 556
       // Only compute leveling per segment if ubl active and target below z_fade_height.
592
-
593 557
       if (!planner.leveling_active || !planner.leveling_active_at_z(rtarget[Z_AXIS])) {   // no mesh leveling
594
-
595
-        do {
596
-
597
-          if (--segments) {     // not the last segment
598
-            seg_rx += seg_dx;
599
-            seg_ry += seg_dy;
600
-            seg_rz += seg_dz;
601
-            seg_le += seg_de;
602
-          } else {              // last segment, use exact destination
603
-            seg_rx = rtarget[X_AXIS];
604
-            seg_ry = rtarget[Y_AXIS];
605
-            seg_rz = rtarget[Z_AXIS];
606
-            seg_le = rtarget[E_AXIS];
607
-          }
608
-
609
-          ubl_buffer_segment_raw(seg_rx, seg_ry, seg_rz, seg_le, feedrate);
610
-
611
-        } while (segments);
612
-
558
+        while (--segments) {
559
+          LOOP_XYZE(i) raw[i] += diff[i];
560
+          ubl_buffer_segment_raw(raw, feedrate);
561
+        }
562
+        ubl_buffer_segment_raw(rtarget, feedrate);
613 563
         return false; // moved but did not set_current_from_destination();
614 564
       }
615 565
 
@@ -617,15 +567,10 @@
617 567
 
618 568
       #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
619 569
         const float fade_scaling_factor = planner.fade_scaling_factor_for_z(rtarget[Z_AXIS]);
620
-      #else
621
-        constexpr float fade_scaling_factor = 1.0;
622 570
       #endif
623 571
 
624 572
       // increment to first segment destination
625
-      seg_rx += seg_dx;
626
-      seg_ry += seg_dy;
627
-      seg_rz += seg_dz;
628
-      seg_le += seg_de;
573
+      LOOP_XYZE(i) raw[i] += diff[i];
629 574
 
630 575
       for(;;) {  // for each mesh cell encountered during the move
631 576
 
@@ -636,8 +581,8 @@
636 581
         // in top of loop and again re-find same adjacent cell and use it, just less efficient
637 582
         // for mesh inset area.
638 583
 
639
-        int8_t cell_xi = (seg_rx - (MESH_MIN_X)) * (1.0 / (MESH_X_DIST)),
640
-               cell_yi = (seg_ry - (MESH_MIN_Y)) * (1.0 / (MESH_X_DIST));
584
+        int8_t cell_xi = (raw[X_AXIS] - (MESH_MIN_X)) * (1.0 / (MESH_X_DIST)),
585
+               cell_yi = (raw[Y_AXIS] - (MESH_MIN_Y)) * (1.0 / (MESH_X_DIST));
641 586
 
642 587
         cell_xi = constrain(cell_xi, 0, (GRID_MAX_POINTS_X) - 1);
643 588
         cell_yi = constrain(cell_yi, 0, (GRID_MAX_POINTS_Y) - 1);
@@ -655,8 +600,8 @@
655 600
         if (isnan(z_x0y1)) z_x0y1 = 0;              //   in order to avoid isnan tests per cell,
656 601
         if (isnan(z_x1y1)) z_x1y1 = 0;              //   thus guessing zero for undefined points
657 602
 
658
-        float cx = seg_rx - x0,   // cell-relative x and y
659
-              cy = seg_ry - y0;
603
+        float cx = raw[X_AXIS] - x0,   // cell-relative x and y
604
+              cy = raw[Y_AXIS] - y0;
660 605
 
661 606
         const float z_xmy0 = (z_x1y0 - z_x0y0) * (1.0 / (MESH_X_DIST)),   // z slope per x along y0 (lower left to lower right)
662 607
                     z_xmy1 = (z_x1y1 - z_x0y1) * (1.0 / (MESH_X_DIST));   // z slope per x along y1 (upper left to upper right)
@@ -674,36 +619,35 @@
674 619
         // and the z_cxym slope will change, both as a function of cx within the cell, and
675 620
         // each change by a constant for fixed segment lengths.
676 621
 
677
-        const float z_sxy0 = z_xmy0 * seg_dx,                                     // per-segment adjustment to z_cxy0
678
-                    z_sxym = (z_xmy1 - z_xmy0) * (1.0 / (MESH_Y_DIST)) * seg_dx;  // per-segment adjustment to z_cxym
622
+        const float z_sxy0 = z_xmy0 * diff[X_AXIS],                                     // per-segment adjustment to z_cxy0
623
+                    z_sxym = (z_xmy1 - z_xmy0) * (1.0 / (MESH_Y_DIST)) * diff[X_AXIS];  // per-segment adjustment to z_cxym
679 624
 
680 625
         for(;;) {  // for all segments within this mesh cell
681 626
 
682
-          float z_cxcy = (z_cxy0 + z_cxym * cy) * fade_scaling_factor; // interpolated mesh z height along cx at cy, scaled for fade
627
+          if (--segments == 0)                      // if this is last segment, use rtarget for exact
628
+            COPY(raw, rtarget);
683 629
 
684
-          if (--segments == 0) {                    // if this is last segment, use rtarget for exact
685
-            seg_rx = rtarget[X_AXIS];
686
-            seg_ry = rtarget[Y_AXIS];
687
-            seg_rz = rtarget[Z_AXIS];
688
-            seg_le = rtarget[E_AXIS];
689
-          }
630
+          const float z_cxcy = (z_cxy0 + z_cxym * cy) // interpolated mesh z height along cx at cy
631
+            #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
632
+              * fade_scaling_factor                   // apply fade factor to interpolated mesh height
633
+            #endif
634
+          ;
690 635
 
691
-          ubl_buffer_segment_raw(seg_rx, seg_ry, seg_rz + z_cxcy, seg_le, feedrate);
636
+          const float z = raw[Z_AXIS];
637
+          raw[Z_AXIS] += z_cxcy;
638
+          ubl_buffer_segment_raw(raw, feedrate);
639
+          raw[Z_AXIS] = z;
692 640
 
693 641
           if (segments == 0)                        // done with last segment
694 642
             return false;                           // did not set_current_from_destination()
695 643
 
696
-          seg_rx += seg_dx;
697
-          seg_ry += seg_dy;
698
-          seg_rz += seg_dz;
699
-          seg_le += seg_de;
644
+          LOOP_XYZE(i) raw[i] += diff[i];
700 645
 
701
-          cx += seg_dx;
702
-          cy += seg_dy;
646
+          cx += diff[X_AXIS];
647
+          cy += diff[Y_AXIS];
703 648
 
704
-          if (!WITHIN(cx, 0, MESH_X_DIST) || !WITHIN(cy, 0, MESH_Y_DIST)) {  // done within this cell, break to next
649
+          if (!WITHIN(cx, 0, MESH_X_DIST) || !WITHIN(cy, 0, MESH_Y_DIST))    // done within this cell, break to next
705 650
             break;
706
-          }
707 651
 
708 652
           // Next segment still within same mesh cell, adjust the per-segment
709 653
           // slope and intercept to compute next z height.
@@ -718,4 +662,3 @@
718 662
   #endif // UBL_DELTA
719 663
 
720 664
 #endif // AUTO_BED_LEVELING_UBL
721
-

Loading…
Cancel
Save